|
3 O2 u4 w+ l0 W- #include <Arduino.h>
6 I2 |" \9 j) r4 T& Q- l2 x - #include <MeAuriga.h>6 u( B3 B4 y$ R: r9 |$ u& D
- #include "MeLineFollowerArray.h"
. c/ H# Y1 D! I, M$ p6 v! q/ \
! d; a3 n- X# t. j* |. S( o* M( ~ H8 f- MeEncoderOnBoard Encoder_1(SLOT1);, |, m- E& D" }& h% |. x( ^5 C
- MeEncoderOnBoard Encoder_2(SLOT2);' p! ]# {! z: {1 w) [: W
- MeLightSensor lightsensor_1(12);
. A, r0 y2 Q+ _2 `% U - MeLightSensor lightsensor_2(11);: h. u: J) \! e+ U. ?! V: K: F
- MeBuzzer buzzer;
% K* H# Q/ O7 H( S7 x, @4 I% D4 ]# } - MeLineFollowerArray linefollower(PORT_6);
6 P/ _* a5 i6 Y3 @
+ [# n6 j+ Z" F! I5 N, p7 r4 |6 E- #define Error1 11 B5 n* j& J! [: |7 L
- #define Error2 20 B$ ?4 o9 Z9 J) B' @8 K7 o
- #define Error3 3- R; W' t' y& @
- ( e( q8 }- {! X& ` {' ^/ v m( C, M# C
- #define Kp 15
7 p8 C9 }/ T3 Z: e# m3 N# R - #define Ki 0.15
( [. ?4 ?5 ?0 d4 f" i - #define Kd 0.037 a( Z: |- J3 K0 E
- . B0 |: _/ l4 `. _
- uint8_t sData;
7 \& r# _4 F1 g) L6 s2 r- m" Z - uint8_t D1;1 n( d0 S; r _ H) x$ w
- uint8_t D2;
4 U% l1 Z" F) V/ v. x5 }& U3 U6 d W - uint8_t D3;
$ w" L1 S* |0 N' { M5 Q - uint8_t D4;* B7 N1 w; `) ?) F0 Q
- uint8_t D5;
. _: _, z6 Q2 R: p# M$ F; ~ - uint8_t D6;3 U2 h- B& ], P0 @ S- M
0 Q$ l( i4 Q( r4 B" F% `- float previous_error = 0;; X4 N. E! Z- x6 r5 H, P
- float integral = 0;9 o4 t- W3 W: c+ }) w* _
- float derivative = 0;
7 U E5 D' ]9 j; N& d; C - int Speed = 160;! n0 |) S8 p9 A, x8 H
- float output;
2 _! Z" L4 W K% M* w' v1 ]# j2 |7 J j
6 F" ?7 X- D8 K* F. L( J- byte Left;& A3 E) h2 d8 e! @, j5 l* m( Y7 V, b
- " w! s/ q8 Z8 J1 ]
- void setup()
9 Q4 R; y; r! [+ G( c5 @ - {
' u3 X: L. S; k! `% K - //Set PWM 8KHz
_8 c+ s& A0 I r - TCCR1A = _BV(WGM10);2 @4 }- Y* p- D) x9 r
- TCCR1B = _BV(CS11) | _BV(WGM12);, j' {. i- k# S7 r8 Z
- TCCR2A = _BV(WGM21) | _BV(WGM20);$ ]2 Q" v3 @! y
- TCCR2B = _BV(CS21);. T) j# v: Y2 k" z( M, v
- Serial.begin(9600);
$ [0 c' w2 R/ n - buzzer.setpin(45);7 l0 f" ]; |4 H) Y
- }
1 G6 o; [; R8 ^" I% G
* W; s! l9 o% ?% N- {8 G5 |- void loop()
4 [$ `9 g; t7 u1 m6 z6 p8 } - {3 w% O4 ?; ^ K& F/ C
- while(!((lightsensor_1.read()) < (10)));. l" ~# `: W# s% r. U
- buzzer.tone(1047, 500);1 y, R6 ^& l) M4 I8 k
- delay(500);" @' _+ |8 K" ^* D
- do. n7 H" s% }) N
- {, J* h8 }* K# `: D$ A) Z8 w
- int Err = getErr();$ D( a; @6 W( o" M* z; m
- if(D1 == 1)
! r4 \4 s9 x) W - {; W4 F7 x, d2 S3 J* k& C, ]
- Left = 1;
n1 R: o2 ^* v3 q. [* b7 I7 X - }) f+ C" V; p' I I3 y+ I: c/ [1 Q0 u1 ^
- if(D6 == 1)5 d! r( n( R' }* u, i, {
- {
/ ^+ v& M* c: u. r - Left = 0;$ E" H) A- T; c
- }
; D4 B8 S& M- I5 Z9 c& F; I7 W2 w9 c - if(Err == 99)
, N8 }1 S5 ~. [0 b' {0 L' F - {
' o- G) ]8 K/ S* y - if(Left == 1)6 A- Q: n" S; M
- {
0 G" k! a7 E9 m% j - Speed -= 5;
: k' ^' i( v" } - moto(0,Speed);
4 [- G! v$ l' D. t' W2 N - do0 [6 ?' Y& l/ ^( o+ R8 Z& j* o
- {8 |- d5 X) S. n' {7 v% s
- Err = getErr();
' v. q4 n* j8 w - }while((D1+D6) == 0);
6 T3 Z/ G- A9 I- X j - }, Z" o1 F4 r0 @3 A( t
- else
/ ^2 s9 f, R1 s- Y9 {( q - {
" r/ p% j& {% u - Speed -= 5;6 ^' D$ q y" v# H% [# b* _! k
- moto(Speed,0);
8 Y. I# |8 j% ^: G - do" \4 B5 D# u: A
- {8 X$ ^; B9 I2 X
- Err = getErr();
+ H9 n* M+ C/ y) }# p - }while((D1+D6) == 0);
; g$ ]' @) M u+ }( f - }
- A$ {7 `4 T# ~5 Y7 f: A - }
; @7 @9 [+ ]% b2 E! m P - else. Y" i, |3 R' p! a9 U
- {
" n7 M g% n7 q8 v/ I1 | - if((Speed < 160) & (Err < 2)) Speed+=1; 1 G L, _6 f0 Y: ]4 I
- if((Speed > 100) & (Err > 2)) Speed-=2; 1 ~5 m( l6 l# u" c, S2 x9 v
- integral = integral + Err;
% r+ @3 k: X3 G' @6 i7 q - derivative = Err - previous_error;$ H3 l& H5 ?* d; O; ]1 v" H1 c
- output = Kp*Err + Ki*integral + Kd*derivative;' Y" z& _# d+ R+ S
- moto(int(Speed-output),int(Speed+output));4 L! v/ g$ @! F/ P! v9 z
- previous_error = Err;
1 S3 s8 H5 E* A6 W. \- X4 ] - }
" e/ X h3 F# a- S* \ u - }while(!((lightsensor_2.read()) < (10)));- K4 J4 G1 h/ y9 P1 i( s
- moto(0,0);
3 l8 a/ ?; `6 W - delay(500);/ |: D# Q, n, @6 t5 j9 w: u
- buzzer.tone(262, 500);. g% P$ A! [7 e
- }
; d1 e% {8 w4 E( W1 G - 1 D: N- x* Z- I c8 F8 [, e
- int getErr()- }3 \0 z% \6 Z' R3 c4 _: i
- {
2 e8 i' `7 e# Z# g) C' E - sData = linefollower.getValue();
! Q& D) ?( ^/ _. @) G - D1 = ~(sData>>0)&1;( o! F$ O# F0 ]/ F) Y: a: A
- D2 = ~(sData>>1)&1;. Z S8 s! o2 ]. ~/ O
- D3 = ~(sData>>2)&1;
$ x- l; F4 T# c# T# p* z* X( o0 v - D4 = ~(sData>>3)&1;
! g- z3 _4 u, x9 r6 Z: ]/ G0 S - D5 = ~(sData>>4)&1;# r( ~6 @3 X3 G( K; A& B6 a* N; m
- D6 = ~(sData>>5)&1;9 P/ f* ~# \" f; g4 b
- int downD = D1+D2+D3+D4+D5+D6;
" u- Y+ h0 ^7 ]! Y - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);; g$ Q5 Z6 H: V, V2 e, G
- if(downD == 0)
; |. G; j% ^( y; T - {- P7 t9 I9 A: z% K" B
- return 99;
. G- {9 ]: L6 t3 ? w5 n - }3 a# L( D/ Q/ W) C1 L# a
- else( {, w3 `9 Q: j$ y
- {4 V2 W! h' T7 Q' W" M
- return upD/downD;9 l* G! v& W, X" J) z+ R% t
- }1 q6 F7 m. C5 W4 U" V7 V' K' [0 g
- }2 Q" o. i/ D; Z8 W5 o
4 c8 D- t; J P7 _. }) v8 L6 O- void moto(int Speed_L,int Speed_R)8 N8 p9 t% J9 p
- {! u7 \6 U% b: }* Q7 U8 ^) L
- Encoder_2.setMotorPwm(Speed_L);3 @4 O9 W) J0 W4 P+ N3 h. q
- Encoder_1.setMotorPwm(-Speed_R); R, e4 H6 ]$ ]; l
- }
複製代碼 $ X* w/ f, x' J- s
MeLineFollowerArray.cpp
8 a" r a. l1 j# Z- #include "MeLineFollowerArray.h"
7 j* X+ }7 G V( B! j9 y# H8 m, f. T
5 X1 ]! v8 \# Z+ q- #ifdef ME_PORT_DEFINED
" M# F1 ~( u. O6 a" q- h: K8 d+ U - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)5 W2 i- k; V8 g( ^
- {
% l% z- t" O1 h( _) N2 ?4 V - + c7 v* z! A, Q) |& z8 W1 f6 }- d
- }
' \5 O' v& t5 m) V3 Z+ }! c/ W - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)( E+ g# F. Z _: y( U. i5 r. X! A5 V
- {8 F- M- J, Y& _) E
- _DataPin = mePort[port].s2;
% H1 Q9 W2 v7 t' _: r3 \ [ b - pinMode(_DataPin, OUTPUT); * k! t3 D; X) Z' m5 O. @8 Z: ~ A
- digitalWrite(_DataPin, HIGH);
8 O' u# l0 @( @' p8 s8 \1 a" ]8 S - }
; M( m; t" ?5 a2 P. J - #else // ME_PORT_DEFINED
8 O& k3 U5 S% |$ H% W1 t! y - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
$ p! W! L5 ~. C/ M _0 c, y - {( L% o( }) t* z# E4 r0 X1 H. s4 O
- _DataPin = pin;
# l' w# U6 E' p7 c3 ?& M - pinMode(_DataPin, OUTPUT);
9 s( |" l1 {+ @6 Z3 t - digitalWrite(_DataPin, HIGH);
( v# G+ O4 p! l& @8 x7 ] - }
6 p# {" A; o0 V. X, G - #endif // ME_PORT_DEFINED
9 x$ v. q7 U) s* h5 \/ v/ E# u# A+ w
# T( W2 d5 ^( A5 ]+ }
3 D0 [! _( n1 ~! @" Z! u- @- void MeLineFollowerArray::setpin(uint8_t pin)( i3 E1 h# r% Z# G0 _( Q
- {7 t' `6 q. V/ h) R
- _DataPin = pin;
/ \2 i5 g/ o, L- ]1 |! y - pinMode(_DataPin, OUTPUT); & V) w) y8 F5 G0 X* g
- digitalWrite(_DataPin, HIGH);9 m" M s, u' [' H/ {
4 D9 Y r8 y% k; _5 p- #ifdef ME_PORT_DEFINED
/ V* C! i v3 [& _ - s2 = pin;, X2 d0 e* h" `8 ^/ P) \& R I" m" ~+ [
- #endif2 N7 Z1 R1 E$ O$ D, H& _2 _# d
- }
# ^4 O, a+ A' { - / `! ^+ v3 T$ W5 P. C
- uint8_t MeLineFollowerArray::getValue()
% L, N2 q" k- Q% |% Q# i7 b2 S - {
0 p" ~; I, ]+ r# Y7 L - uint32_t LOW_level_read_time;) Q0 |& ?) f4 b/ i) D m" j: B
- uint32_t HIGH_level_read_time;
2 D1 J0 Z$ b0 ~6 O8 ]& J - uint32_t time_out_flag;: |- M1 w! \& w; V# x; o' J; Z w! v; O
- uint8_t Sensor_Data[3];
3 k% o/ ]1 b4 z - static uint8_t old_data = 0xff;
: n b9 g3 q/ `) y1 J- o
) c7 ` t% [! r8 u* n3 q: K& \. `" \* r- pinMode(_DataPin, OUTPUT);: M4 K [. C. R. h. S8 h% `" m, ?
- digitalWrite(_DataPin, LOW);
; o+ N( L- a1 E* ]: D! H - delayMicroseconds(980);/ r- M4 W! J' \% @7 |
- digitalWrite(_DataPin, HIGH);
# a# ~/ p6 S0 e. p1 A" ~3 H' q - 1 ` t) v' o3 L8 M
- pinMode(_DataPin, INPUT_PULLUP);& U8 R( w. y% `! F
- delayMicroseconds(10);, i9 u' k, v' r q+ f/ A' z/ J8 n8 J
e" S$ F) H0 \; H/ Y# T- time_out_flag = millis(); g# C. Y; u) G+ g+ @ i
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- e8 Z+ m s7 H5 }% w( r1 p
- $ M* G& ^6 d$ ~ r$ N ~
- LOW_level_read_time = micros();0 w' P, C. S1 K+ A* \6 H
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out" n2 o7 N/ L& U; E0 ^5 {+ n, R3 X
- {/ a$ f L, Y0 F) L3 H
- return 0xff;* E) J5 M# W5 Q/ e' s# ~' L
- }/ B# D. z7 d' |) t! f" I6 X* L
; Y x. k1 d. A- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. c- B( h& t5 C% t7 C3 u' [
- - @+ O- m% A8 K+ e" @# D
- HIGH_level_read_time = micros();
0 ~' Q- D, R& G: P - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level7 P* F2 v# ` D9 G4 l
- # a( m% D( N2 @; D
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 u! ?4 m. S! H* P. e+ Q- ~ - {
. ^# ^$ V- y) ~6 @ - return 0xff;1 L* a, h2 Q/ ]9 g/ _4 S6 f, \
- }
7 Y8 m+ C A( Z - " z: n6 k/ Z. J: g) [; y, K
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))2 c- H( G9 r, B
- {$ ?( `0 P* I' v, [
- return 0xff;8 O8 G; X4 Z. c: l- a; O$ C
- }3 h* e- H8 z' ~) d$ p: n1 W
- 3 `, V( d' W+ l, X. }
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" A# B! ]* i, Q' x- v y. I1 }
- LOW_level_read_time = micros();: P/ p" h, T' e' e
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level4 I' D1 x+ d/ j& ]; B- I6 D
; V/ |+ U- U9 Q, Z' F3 }- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
- I' K/ O) M" ]7 W, q - {: j% C, P. Q0 x, W$ L3 i0 n+ j
- return 0xff;. W! E' @2 M( ?: V$ H! F
- }
" T) [: u. o0 \! N- y1 ` - $ e3 V6 E) T8 L1 k4 F! u
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)). a* u0 A% B8 a( ]$ I, i, i
- {* j8 y+ w* L9 Y+ C8 s0 U
- return 0xff;
* T8 y# u% k1 T0 ] - }
( Y7 u: p j- ?, c$ E
$ h% R# u! `% D" S5 j. d) s9 ?7 e' J- for(uint8_t k=0; k<3; k++)0 }# M, E2 j/ c. J: [" x. d
- {+ X, M/ y( V; p. |* }
- Sensor_Data[k] = 0x00;
) U( p% n- b( n; H8 |1 ~0 k
3 e% J( k I; P+ ?- for(uint8_t i=0;i<8;i++)
4 p, L8 B# m+ J& c5 i - {
, C7 g2 `9 A( ` - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level3 W! q0 \7 _- J3 @! u
- HIGH_level_read_time = micros();0 x* E% V* i0 p! v: p% `. t
- LOW_level_read_time = micros() - LOW_level_read_time;4 c& j. e5 U) O' T# }; w1 P
- 9 ^) t( T' [. N
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ); Z* j( W. L# ~% h
- {
( E7 N+ O: }1 U/ |/ I - return 0xff;& e4 D2 B) |. e0 w) O U
- }+ E) x" z" M& c/ h. m9 D8 G" [
4 F9 n4 e, P4 X6 \- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
, ?0 ` @ `/ o8 H, h+ i. ^: W - LOW_level_read_time = micros();. b6 @9 H& t6 b/ k- m- L+ u
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
4 q" x ]3 Q) E) k9 J6 n) e
8 b* g: e/ Z4 S8 g) u1 b: }- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1- c# d- n) @8 h3 _9 g) p
- {5 ~' u: x; G! I S5 Z5 [
- Sensor_Data[k] |= (0x80 >> i);- z6 X0 v8 B8 X$ \+ z
- }
6 X6 K: \$ `$ d. r; B3 d& l( x - else if(HIGH_level_read_time >= 100)6 M' {: f0 z# B# l0 J; U! U( Q
- {
; M& k- @4 K4 s( h3 s - return 0xff;0 z4 C( H$ S7 S# ]+ D. ^; L
- }) C. i+ I) w g0 O4 c
5 n: i7 T# W9 W: ?# ]2 k- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
7 \; F8 y4 t7 ?- X6 c3 H- [' y! l4 f - {3 [7 s: _8 z8 g9 q6 M4 X. P0 s
- return 0xff;4 [) y# i& _7 a( E
- }
/ t. ?$ R; E1 s8 \ - }
; o+ }4 B, [0 s3 [* v - }
; i- c3 w3 b" z; o& m! G8 g
1 L+ G' c! d6 V" T+ `$ D2 V" a- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
" w4 U- F* ]8 N8 T - HIGH_level_read_time = micros();
% U6 w, P0 B9 }$ s0 j. O0 g - LOW_level_read_time = micros() - LOW_level_read_time;
8 ?( G# B* U# C% @ - 1 k' U7 E8 F/ ^) c) G- M% s5 j( H
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
% ?+ M9 {1 x) j1 E+ `5 M; ? - {" `# m7 y& L6 R2 D2 w5 c, Z E
- return 0xff;, {* n& M4 d9 x) \1 y" m
- }+ `- V. Q6 I$ D$ _6 M4 r: R/ I+ N
- $ g! {7 ?7 I. e5 s7 W d- |
- pinMode(_DataPin, OUTPUT);
; \ Y5 l6 `3 F K( I - digitalWrite(_DataPin, HIGH);& ]5 a9 c% X$ E% [. c* H% B
4 U' x* j3 T# ~9 U- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
^6 ~: s' Y# A. E+ L/ n4 y - {
2 y+ B. k2 i# O, I1 n; k - old_data = Sensor_Data[0];
9 J, y, L7 R/ h. F3 ]5 A - return Sensor_Data[0];) y5 x; h3 A. Q( }7 q8 I
- }
( x8 U, n n0 E+ [ _! j% R - else
+ V+ B# \7 L( N5 A! s6 C* U5 N - {
1 n2 Z5 ], [- { - return old_data;& C/ P3 g2 }/ ]; G
- }' |) ]0 l: ^4 S* R$ T5 P
- }) `' a6 j. x0 y
複製代碼
- }4 d( a4 Z$ m9 x OMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
7 J* z& L; W3 Z
% ?" F0 Y7 o" X! c2 Y5 K3 g' A |
|