|
|
& X- a( W6 u4 v9 V/ ]* r1 h1 _* Q& T- #include <Arduino.h>
/ u7 H4 U' ~+ e - #include <MeAuriga.h>3 E4 ]8 @8 O7 h3 V
- #include "MeLineFollowerArray.h"
+ s+ J) `: U, ] A/ s
3 Y$ r' @6 X( q- MeEncoderOnBoard Encoder_1(SLOT1); m4 K1 Z0 `- I$ ?/ z8 c! R6 u8 Y/ c
- MeEncoderOnBoard Encoder_2(SLOT2);) b+ H/ o' B! s7 k5 _; P
- MeLightSensor lightsensor_1(12);
% V1 N, A7 w3 X - MeLightSensor lightsensor_2(11);
0 l) [7 d0 B/ u# D6 m" n - MeBuzzer buzzer;! l( s/ J& ^. R% w& U
- MeLineFollowerArray linefollower(PORT_6);
& p* n N7 ]& o. r( y7 i' T
* }* ~8 D1 s: m- #define Error1 1
& {1 E: c9 c- u5 r' A. E1 { - #define Error2 27 p2 H( s) u6 U- a
- #define Error3 3: h+ t" H" f0 I! o$ H8 \) v
- 6 D. ]4 V" f) z; W5 V; p/ F$ i
- #define Kp 15
/ U- y$ ^ r# I- U# v5 g' r2 X - #define Ki 0.15. w2 M8 O3 @! W6 {& y+ d9 V) q
- #define Kd 0.03
' [% h7 A" p9 c) R
4 w7 R: P- i0 n8 X }+ _! W$ A M3 g# Z8 B- uint8_t sData;
* o+ J$ N4 r: z7 K8 ?: F - uint8_t D1;/ S4 V8 y# E6 F# y
- uint8_t D2;
# X" c: ?& q2 w. p - uint8_t D3;
! l1 Q$ g' M3 q - uint8_t D4;
0 [8 A2 |- l# M) r - uint8_t D5;
1 h, Y1 ^! \$ b0 { - uint8_t D6;1 K/ F6 z$ F p
- 4 A" D8 X% A" ~8 s ?; H
- float previous_error = 0;
- I) X4 [( W D6 s; f2 V' y - float integral = 0;
, Y4 s, v, c& b2 ?% }0 V - float derivative = 0;5 d2 c2 w( K2 W" g/ z
- int Speed = 160;
+ z6 Q) `' h) V9 y: H5 @2 I - float output;
: _: p) }% V2 s' q. @6 d7 T - + D |" _' l. R4 ?( i
- byte Left;
1 v! ]$ ~1 T" ^; H0 l
9 Q5 x# e0 K' m+ O" h' m! A, A- void setup() . S: N- S2 X$ r% m8 B8 @" C2 N
- {+ s2 ?+ ~3 u3 d E8 J/ H# [' ^0 ?0 a3 s
- //Set PWM 8KHz
" l# E+ w$ {5 ~ - TCCR1A = _BV(WGM10);4 E2 Z5 B2 U1 J1 t
- TCCR1B = _BV(CS11) | _BV(WGM12);4 \* \/ h7 K( K3 l$ }' P
- TCCR2A = _BV(WGM21) | _BV(WGM20);
8 f' C, ^/ |/ @" L - TCCR2B = _BV(CS21);- J1 A9 h4 h; H
- Serial.begin(9600);0 n6 @% m* H+ J
- buzzer.setpin(45);9 G' j6 ]% d2 `$ C( W
- }
3 h6 r- @" `# C# l2 W - * \- F4 d0 t+ A0 ]5 L9 o8 \
- void loop()
0 {. q- i# d* G2 ? - {
, x9 N9 B7 Y E: r/ E8 c: U; X - while(!((lightsensor_1.read()) < (10)));
# T7 O8 [9 u# w/ i3 x - buzzer.tone(1047, 500);
# Y& h8 ~ e6 M' Z8 I - delay(500);: @) j4 X( G( m, {+ [
- do
; e9 `1 G# _! \: R5 X; C! f9 A$ Y1 k - {% x; Z+ T% _( M; ]
- int Err = getErr();0 j! A8 C8 k% o3 \% [
- if(D1 == 1)2 I% _1 d7 m" ~9 X. P. \6 O
- {$ w: k2 R: i2 p) x3 k& N6 P
- Left = 1;! I$ m1 _' R. z& n
- }
* ]6 d* K3 R9 u9 C. G - if(D6 == 1)
0 f; x8 p r7 y+ ?( C6 P9 [ - {
& d7 V% I9 a: l0 l - Left = 0;% p2 a( F/ e( Q# o$ z
- }+ ]) |! a5 l" I$ {5 A' c
- if(Err == 99)
* N1 f. G# p0 y2 l2 m6 W) e( M - {7 ^8 i! @5 o% H/ |# v
- if(Left == 1)
0 u' v6 P: K) N9 h - {3 n! ?. ]. g2 ~/ Q+ M/ G
- Speed -= 5;$ c' ?. _1 N+ m& k7 k
- moto(0,Speed);! @" ^4 i! v6 d& K. b+ p, c; c3 p# X
- do
9 k, U3 j; c, B$ K' u0 G - {
2 ^3 k1 M3 @7 P' x - Err = getErr();
3 f+ ~1 {9 ?3 d" G6 t. R - }while((D1+D6) == 0);
/ u2 c" O6 T- r; W3 p - }; N9 F5 D; W4 e1 K# x+ [3 E
- else% |. }) H' T+ P8 p6 o) h
- {; h9 x" |( |6 N& E3 z o
- Speed -= 5;
" x& X8 c3 A, w& p0 ] - moto(Speed,0);
8 t( v8 Y7 ]0 J% I - do9 ]" H: M; t0 D" W6 T
- {; b+ \+ A* e- F8 H- n' ?& g& r# D
- Err = getErr();
& d' b d9 W" f9 A! I9 o - }while((D1+D6) == 0); m' g1 I- H! z4 ?6 ]; Z
- }) O+ s( t% L1 `- }4 A
- }
/ G; c& ]$ G c3 r - else4 b+ s8 K# {" u' H
- {% E) y9 d4 J) h* ~$ f. a& U
- if((Speed < 160) & (Err < 2)) Speed+=1;
1 W3 S- G/ T+ l6 s2 r6 _* c! B - if((Speed > 100) & (Err > 2)) Speed-=2;
% G9 G1 `# i0 X6 P* H5 n- t - integral = integral + Err;
% x# v9 t/ a, M' J$ H7 A' C - derivative = Err - previous_error;: F% N. V( a6 H) t) d
- output = Kp*Err + Ki*integral + Kd*derivative;# |5 A+ w: J' {# x, Q, o
- moto(int(Speed-output),int(Speed+output));4 u4 `: A6 s/ d2 p: g! t
- previous_error = Err;
2 u, |$ b& |* k2 c& b q; `, u, c - }
4 o( N3 P/ ^: g6 ? - }while(!((lightsensor_2.read()) < (10)));
7 w; [! i2 |! E R- A - moto(0,0);
8 `" c/ t) U' Q5 Y2 F# ^ - delay(500);
/ X, a% \) x. S8 z. w$ X - buzzer.tone(262, 500);
3 J/ _7 D; I$ l& P V - }
% U y* T- [/ _) ], J$ [ - 1 v$ J" ]0 A; l1 }1 G9 w }) x$ n
- int getErr()5 l# O( M6 U6 ]3 V
- { $ y' ], h l$ f$ Q) ?9 S; o% }
- sData = linefollower.getValue();
) K. a% u n, [% `! J6 j/ }# F - D1 = ~(sData>>0)&1;
{: m7 s. E* K2 L - D2 = ~(sData>>1)&1;
7 |& g% ~# ~+ h+ Q* y - D3 = ~(sData>>2)&1;0 v6 {, k7 ~ h7 a3 \
- D4 = ~(sData>>3)&1;4 H4 O8 ~- f# v! @ H W9 ^! ?/ u
- D5 = ~(sData>>4)&1;
. Z: Q! ~; ?3 {& A/ d% f- x. ~; I# L - D6 = ~(sData>>5)&1;
7 P$ a& u; B F( K @ - int downD = D1+D2+D3+D4+D5+D6;: n" l2 b: k* b+ ]+ ]/ g7 `
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
5 s9 c' N9 K; J+ e; S - if(downD == 0)' U! X- K1 w( F% p% b
- {
% {2 w& f$ J' M4 o. w - return 99;; S6 J, E/ l: h: _! S$ P9 k* c
- }" k% T2 Z. b. f2 {
- else0 ]2 Y0 `+ ~# C1 Q
- {+ t4 f p- g! x( ?5 k) e% ^8 b: v
- return upD/downD;
2 M: E' U, _" ~/ V+ g - }
' l _3 @1 ?4 k! X7 {5 v - }/ p5 P j5 D/ l- T
- - M. h6 r! ^6 }/ r }2 ~4 _
- void moto(int Speed_L,int Speed_R)# N# ]6 k4 v! | u
- {
: Y/ {5 ]9 }2 C: K+ Y - Encoder_2.setMotorPwm(Speed_L);
# \3 D4 g s ]" `/ i- y/ y - Encoder_1.setMotorPwm(-Speed_R);: F+ p% H' ]; Y* H9 k8 r' L
- }
複製代碼
2 O8 e% \. |: r! T) k7 M, i7 b# r7 cMeLineFollowerArray.cpp* K2 y" U- |( N5 C5 o- J+ N+ ^
- #include "MeLineFollowerArray.h"$ y3 h# U5 H/ `3 ~
- a; D+ S( H [4 E# F- #ifdef ME_PORT_DEFINED4 W) O$ ]! K0 S6 Q
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)) ~3 S( T4 l' O
- {
2 L/ t" V' q3 J5 x4 B' M - % k3 t3 F7 ^( P. v
- }
3 a# l, v% M- l; L5 H2 ~* ^' B/ | - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)7 d- }; E. F+ P3 G/ z# K9 B
- {/ |2 T9 |0 O" s- q; ?8 V( @
- _DataPin = mePort[port].s2;8 d; X+ k% a2 M/ K9 c+ {
- pinMode(_DataPin, OUTPUT); 3 g2 k; K* h0 y I
- digitalWrite(_DataPin, HIGH);
1 D* t% b& v3 j! X) E+ q1 y0 \ - }# z7 |4 v$ O9 O/ B, R
- #else // ME_PORT_DEFINED3 s8 N2 u, Y( { {1 |* {7 k2 k" T6 Q
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)/ u; y( k- p! ^& R0 {
- {
2 G3 l7 n. p7 a9 h, l& X6 w% L7 e - _DataPin = pin;1 N; z: {0 o6 T
- pinMode(_DataPin, OUTPUT); ?0 n4 m1 y8 I" H
- digitalWrite(_DataPin, HIGH);
* z) G& t+ e1 N" m* Q3 O( n1 } - }1 j! n4 I8 i" q: q6 U0 h
- #endif // ME_PORT_DEFINED
: r* U0 x! \5 |/ o* {
' B. S, ]% n3 P0 C; k8 c4 M$ c- ! @! k, E8 |' y
- void MeLineFollowerArray::setpin(uint8_t pin)* @' D+ h, u5 c b& o4 g5 E" e& K
- {
! o4 t% H3 \& }, u$ q - _DataPin = pin;
: T- x) z$ A1 ^% S- o, J% i1 ` - pinMode(_DataPin, OUTPUT); ; _3 b1 s$ S1 S8 O, `/ j) |; {( @3 J2 v
- digitalWrite(_DataPin, HIGH);
$ t; Z$ l) M, L6 ^" U6 W( S
3 m6 |2 V5 J: T' r4 D! k- #ifdef ME_PORT_DEFINED
; Q% ~& L# ]# a2 }! T8 \2 z - s2 = pin;
' P6 F8 u/ J) ^' K Y, k% j - #endif1 Z7 p1 a9 S5 e; c7 M& Q: U/ j/ s7 Y
- }8 h0 s0 N4 t- l+ V! c7 H4 ^
- / L9 C0 D6 b2 ]$ z3 X+ i' N) F
- uint8_t MeLineFollowerArray::getValue()# B2 s7 n$ |$ V9 P" G9 l1 n( U2 [- j
- {
3 ~8 K; |$ K# j2 O8 ~* L% s1 Q - uint32_t LOW_level_read_time;& w/ s' K {. `, n3 B
- uint32_t HIGH_level_read_time;
7 r4 P1 h7 v6 F k - uint32_t time_out_flag;
! P& z) g& ?* Z- U" v - uint8_t Sensor_Data[3];4 H+ f+ `. M6 C, C/ T- m
- static uint8_t old_data = 0xff;& G0 @( J' Z( T4 ^
* n4 E$ s5 E) U0 i- pinMode(_DataPin, OUTPUT);
5 P( ]0 O$ Q, R- X$ |0 U - digitalWrite(_DataPin, LOW);
$ T0 b4 C9 x+ G! b( e - delayMicroseconds(980);& \. ]0 {) v; L$ i- ^/ A
- digitalWrite(_DataPin, HIGH);
. H) j. q) C% c$ | - 6 {" |# b; H4 g/ A7 ?
- pinMode(_DataPin, INPUT_PULLUP);
' ]4 J2 `5 }( ?' M/ h0 \' C) `) h - delayMicroseconds(10);3 E5 R5 x( ]' K
2 `1 Q6 g k* |- M5 w- time_out_flag = millis();
' i# J4 h: R6 `' Y) I' u% h! _4 ? v; }! c - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
* J# M" f9 _2 r - 6 |' R/ X8 t4 H9 X
- LOW_level_read_time = micros();
1 t- x+ L8 D6 a/ c5 F/ K - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 W$ v( d5 O; t! Z, n5 n% G
- {
+ [" H2 C( n" T5 }/ k! R - return 0xff;
: J& i- K+ W1 F - }8 K; K, o5 ~" {2 N6 ~9 y
0 Z- n3 |/ O% A* S; P2 j- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ B' _/ D0 r5 d) @# U% H# U
7 u; x2 Y! S! @- d5 J/ t; U- HIGH_level_read_time = micros();. A2 ^- K* N5 `' D" t
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
1 r0 l4 D6 M8 K; a; r - 8 j* n# F* T! W: a
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
! v/ F6 R, M" m% g2 v8 o" R - {
g+ d E/ z- y$ ~: _ x- R) z - return 0xff;
$ ^6 O B2 @/ m9 w) ~6 a - }
5 j$ R; K( Z- `: T0 V2 z
/ W3 S# Q0 ~" O3 d- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))( f& ?" f+ s( K% e" u R: q- i/ z
- {2 L) b `. K6 j" b; a
- return 0xff;
5 M- v) i r, R- K - }
; a7 }9 g G0 T+ ^
9 a( }' w, [. V5 w( D( D- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); [( I7 _; n" c
- LOW_level_read_time = micros();" Z4 r2 u/ B( Y! T; i
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
2 o$ u+ J @2 c- [5 G p! |
2 U, X' F5 O7 {3 V9 Q- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
: M+ P/ }$ l/ ~7 Q - {
- C% a* q4 W3 K: t - return 0xff;
! Z8 z9 f+ \, A* D# ~2 u: B - }
K# O! t* P* Q0 p
% {2 F' S" r4 g5 ?/ w. M# ?. x- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
& Y) `! }: s* i - {
+ ^( t0 T( G! T+ o) q - return 0xff;
; F0 {: N3 }: }9 f3 F2 s4 p* V - }4 S- g+ K' ^0 h4 g; d# L
- : p% q; A$ p) l) g% I
- for(uint8_t k=0; k<3; k++)
: {: Z3 d* `/ A5 ]2 b/ X( O - {
# j9 K7 B k8 k6 E6 s/ q - Sensor_Data[k] = 0x00;' C9 L5 B6 w1 T x
- ; a `5 }; U# i" G6 `8 F, A
- for(uint8_t i=0;i<8;i++): k w' l r/ p
- {6 A8 j) Q# k/ b+ U$ j7 G: J; M! Z
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
" h7 W& p. \# M, I% z - HIGH_level_read_time = micros();& K4 O7 n$ ?% A k9 R4 r
- LOW_level_read_time = micros() - LOW_level_read_time;% d4 l; S# d6 ~, m
( V' A, T s+ F1 X& |- C: ~- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )0 U" s% x; a8 n
- {$ G# y4 J, u+ v2 k8 i& L/ o
- return 0xff;2 P- P0 [- m' o6 ~3 {$ x
- }* U4 a( Q- u0 j7 u3 _; K" q
4 L9 E" g' i& y8 n& H) O' O& t: [; v- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: _4 N0 C: v) ^; s; j( M" s0 }' p, W - LOW_level_read_time = micros();/ l6 A( n- u" S: q1 ]& p
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level' Q" C/ r) G" g/ W1 `) S# Q
/ X2 k; M# f& B- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
$ }# z/ H5 U9 ] K( a, g - {
, i9 l, O$ m/ }$ k' ^: p2 n3 x - Sensor_Data[k] |= (0x80 >> i);3 @$ |5 b6 r# O+ G/ V
- }! B: Q- V4 p4 u4 Z
- else if(HIGH_level_read_time >= 100)
5 K& }: v* r" {/ ^6 r* q - {8 {7 }* [# s: B# R
- return 0xff;) w8 S8 S W& g8 H
- }) B5 o9 G& C5 q6 D; [
( y' o! P0 e' F+ A& i" g: ]- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
7 Z8 l1 C+ b# Y$ l* Y! K - { ?3 e3 X/ d+ F) i% w0 f# l" m
- return 0xff;
- R6 h+ S- c' X/ c$ Q! n - }
( W! c0 S$ g$ y4 ~' i - }
+ x6 v4 Y" E2 N# X - }
" G7 o: e9 `: t! m! Q% g
+ p1 g$ i' o) F* i- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level8 K: O" H4 n* E# z! L- K
- HIGH_level_read_time = micros();
6 o: D6 P7 r* G; T* p5 z - LOW_level_read_time = micros() - LOW_level_read_time;
- a* Z& s5 O: `& p
* ]2 j2 d" ]( E6 M8 B- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )% g: y" t. b5 S' I% Q
- {
9 v* `& \* H8 p; R - return 0xff;! g. e+ x' a4 s3 S
- }
% x/ I, E% u9 d" m9 W5 d9 N" { - ! P2 u/ ^# I6 t# E5 n
- pinMode(_DataPin, OUTPUT);
- ?: S% i! [1 ^- S - digitalWrite(_DataPin, HIGH);
4 w1 ]$ f+ Q( `) P - / O/ o8 c2 i4 U5 \% {7 w/ t7 x. {4 B
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))3 ~) g% ~# u* y) S) f5 Z
- {( m6 n( C7 e! Q5 z
- old_data = Sensor_Data[0];
. T4 \9 p" x1 P6 A/ f9 r6 s - return Sensor_Data[0];/ L) O- ]3 Z: N& `7 A/ k7 m& h
- }
* _7 ~ z! \2 e" ~6 Q - else$ M# I3 D" c* ]4 ^
- {/ ~+ }! e" c- n' R- x& h* j* y
- return old_data;/ z( U( d& I- _* R3 H) k9 n
- }+ j2 M, _2 m' j3 f: K+ l& h
- }' u# H7 r6 ]7 f9 @% @6 \8 l
複製代碼
& j- k3 B5 O) j/ n8 m; ?# n9 LMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
Q! B. r0 l- [, b9 d' p
0 q6 B4 |0 q P8 z) e9 U- ~ |
|