|
|
7 F6 _1 ]' A/ z3 f6 U' M" a- #include <Arduino.h>6 \9 _: S1 e ?. j( G" J
- #include <MeAuriga.h>
; F7 g" Q5 A) K - #include "MeLineFollowerArray.h"6 }8 z' _' b# R% S8 h) j
9 K% F" j" h' c2 f$ E' f! ]$ \" l- MeEncoderOnBoard Encoder_1(SLOT1);9 y0 r3 N! G6 ]
- MeEncoderOnBoard Encoder_2(SLOT2);+ u' x) [: m) t* u4 [6 a; i5 E' K
- MeLightSensor lightsensor_1(12);
8 N* ]# }4 s. @ Q( o# s - MeLightSensor lightsensor_2(11);
1 \+ L7 I( F/ t3 a% s! N8 e - MeBuzzer buzzer;
% P8 j1 t# f! y& { - MeLineFollowerArray linefollower(PORT_6);
6 J) ~. k' a+ s. S7 {
5 K# b* ^; F2 P7 i- #define Error1 1
( I L: Z3 A# @, B: ?) ]8 R - #define Error2 2! i$ g, V0 t/ q% L P: i, J
- #define Error3 3
7 h, C' K! o$ h3 k/ B8 K; N
) i- d8 M d/ I9 ~- #define Kp 15
8 b; Z5 Z( @2 ~7 p3 r$ e - #define Ki 0.15
% c+ ~6 q9 c9 h; z - #define Kd 0.03& x! |. S6 K) M% B2 E, x
. w- K5 V, o/ i* A2 Q- uint8_t sData;
+ |5 i8 c- O4 C5 H. Q Y - uint8_t D1;# K0 e1 P9 o' m
- uint8_t D2;
5 F! C* `. j' @- O! ?5 y - uint8_t D3;
' j$ n7 d) J0 j- f& b. d - uint8_t D4;
+ U9 b4 u9 W9 K5 E& a - uint8_t D5;1 ~& M- B1 f3 i
- uint8_t D6;
, y ]/ @, ]: l' Q$ v
; j4 [; B3 ~& E& P* n# o- float previous_error = 0;
" c A* e1 {1 J7 W, N, L# a - float integral = 0;6 n. {# x$ w; w. G' |% w+ w: y9 x
- float derivative = 0;: g7 v6 t0 C( N, K! |; {
- int Speed = 160;& }# b5 s. l+ |( e0 U
- float output;6 E7 Z4 F- x6 B9 Z8 d3 [* ]/ K7 K
0 @& o5 E, _+ P! x* F' ?- byte Left;" T3 c; a- K% o' g8 O* v
/ N4 u" p" d2 u x# Y. Y0 C3 w- void setup() 9 z: n! j, r- E/ n, J, d4 W. G/ @
- {- ?( R% \8 ~, M4 T& _9 X
- //Set PWM 8KHz5 v Y" L" |5 q# d5 E" b1 Y
- TCCR1A = _BV(WGM10);
, y; |2 }0 G* G - TCCR1B = _BV(CS11) | _BV(WGM12);
8 Z* @) \" n' n; V( P s9 q2 H - TCCR2A = _BV(WGM21) | _BV(WGM20);
: s T& r m/ A1 i8 ` - TCCR2B = _BV(CS21);
; x3 c) t8 t" u* _1 q. @ - Serial.begin(9600);
3 i9 B" X- z& b0 T4 {: o1 X& c" U - buzzer.setpin(45);
; C$ L/ ]5 Q/ s1 ^( D. B" a - }6 w/ K8 F& [5 e' {: c Y' ^
- ) q; X5 U& K: e/ J8 S
- void loop()
z. G# ~9 @% b; x - {' d7 L. m3 Z+ Z N! R2 {$ n6 o+ B) }
- while(!((lightsensor_1.read()) < (10)));4 W+ Y# `# S; s& u- @* ?3 ^
- buzzer.tone(1047, 500);
* v) t7 H) ^) S6 B& p - delay(500);' s9 ?2 E! \& B3 ~/ N
- do, L% a9 T8 X) [/ M; @/ Q
- {
" Q8 o# i: z. r - int Err = getErr();
2 Q# R9 ]; A2 H) g! s - if(D1 == 1)
9 [/ f5 |0 I( i! F0 r - {: F- V* Z/ |+ ~ @$ D# ^/ E
- Left = 1;' H# _# T7 A7 A: U- f6 a& C# P9 B
- }4 `2 U4 R( t4 U( m& X
- if(D6 == 1); Z& m8 [& I( C4 Q+ w
- {- l" E" R$ \8 t4 L, v
- Left = 0;0 @- A' o# z) [3 K
- }3 q& Q: {: R4 p3 P6 ^
- if(Err == 99)
9 \4 z) s6 G9 e" e8 a - {
) F6 o6 Z( b3 r; k - if(Left == 1)) h1 c$ J& q2 C$ C
- {
F" a5 G0 e- W - Speed -= 5;3 U* _3 C4 Y5 H. M( o" J* x* {
- moto(0,Speed);
1 ~7 S; D- L$ F - do1 }# u$ E# T/ a6 e4 Q
- {0 A- |' K: k* U N, L
- Err = getErr();4 Z% @& T0 d8 C% u
- }while((D1+D6) == 0);" }" F/ ^' {& g' f9 e
- }
: U, ?/ w- r k" [3 Q - else
- p5 t+ F$ d3 Z+ h - {! R+ M; f" Z9 x2 c$ Q
- Speed -= 5;
* r* u* Y& W% e m2 Q) w1 X - moto(Speed,0); 2 J- d+ c6 h5 Y$ R2 k' j
- do/ x+ x) w0 A, {( H' T
- {# Y! v; s0 s! V, W
- Err = getErr();/ n7 D' ^( S W
- }while((D1+D6) == 0);% U* q# i* W1 T+ s0 X. K
- }
$ y1 ]3 N( Q1 y! b6 { - } P( U& [- o: A2 e3 B" h
- else
4 d6 M, d, x. T& H! y( K9 o b - {% c ?( i- \- m% ~( C
- if((Speed < 160) & (Err < 2)) Speed+=1;
( J0 h9 j/ N1 o3 ~ - if((Speed > 100) & (Err > 2)) Speed-=2;
) x( [5 _! K. ^7 u9 K( R - integral = integral + Err;
- F% a, Z! D1 a# j/ l, N- K+ x1 ]0 j - derivative = Err - previous_error;" I7 v6 K& V! B( ]+ O4 ~, d
- output = Kp*Err + Ki*integral + Kd*derivative;
' n5 }( d4 T/ Z; N& [1 O5 V& O - moto(int(Speed-output),int(Speed+output));5 B* o0 x5 i$ D( ~3 I2 ?8 F
- previous_error = Err;8 M3 \7 H ^3 Y8 ]+ u
- }
4 P7 v4 i7 c' y" M - }while(!((lightsensor_2.read()) < (10)));
: q7 j; z4 Y" q/ V: O - moto(0,0); C, W7 e2 z1 l! K E9 b% u7 d
- delay(500);
: \- L2 b ^9 d4 ?3 s" c9 u - buzzer.tone(262, 500);: c, j7 T; f i5 N
- }& P. C+ y! d' l9 I/ G% S
- ; [2 X- C. h! L
- int getErr()- P/ _# ~; X% I" j7 w% ]
- { - R6 {6 Y& F7 F8 f( l
- sData = linefollower.getValue();
+ f: k3 x% t! }- Y8 m4 G - D1 = ~(sData>>0)&1;- J3 Y/ Q* ^! B& B
- D2 = ~(sData>>1)&1;" g1 K. c- w$ Q3 R0 N* d
- D3 = ~(sData>>2)&1;
" h0 q$ X q: L; S% m - D4 = ~(sData>>3)&1;" s/ e& k0 L4 A3 U% K
- D5 = ~(sData>>4)&1;1 X' N8 A% l; W. |
- D6 = ~(sData>>5)&1;* Z( `% o: X" y
- int downD = D1+D2+D3+D4+D5+D6;
$ o! E1 A! U+ D" X+ d - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);' Q9 [* B* Q) G& J( P
- if(downD == 0)
4 v+ a2 M! r$ D) ?( K _ - {3 d |* S! f! c: d2 x+ h
- return 99;
- y' w4 o5 R2 _% k' F: P - }
/ n! l/ }0 S) q7 ]) z. e - else) c- c8 ]; Z% [, v& ~2 h
- {; ~0 p$ Z! ?4 {! [- g
- return upD/downD;+ H# k" _. Z4 y7 E4 m4 Z
- }
' l8 u3 t. _2 ~; j; a0 Y6 w - }3 k1 i1 g+ j. c0 [) u* t
- 8 ^" s2 h% D5 u, W) i
- void moto(int Speed_L,int Speed_R)
* E- i$ B! @0 P - {; J# X+ U* u; v
- Encoder_2.setMotorPwm(Speed_L);
* F, j$ T2 b8 F+ i; o ~+ n - Encoder_1.setMotorPwm(-Speed_R);
( M- Y1 C: x3 \% j - }
複製代碼 & ?3 r5 c6 Y; t4 c/ y8 Q
MeLineFollowerArray.cpp
/ [, V5 ^: l1 u( b- #include "MeLineFollowerArray.h"
+ |$ g9 S4 Z( D j% X - % }" D# \0 w% u7 F
- #ifdef ME_PORT_DEFINED0 C- Y# f1 r0 P+ |. ?6 i# n3 N
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
' H F# g1 ?2 X5 ]( V- J. q - {
7 N# N+ ?1 [; `' u9 i
9 W- o R7 ]4 o- }/ P( o9 c( f: @
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)& W7 b4 C, C# W2 ~" U- r( v
- {
3 S, q: _. z0 E2 v" e$ p - _DataPin = mePort[port].s2;
1 B0 _, t! H- D2 ?8 ] - pinMode(_DataPin, OUTPUT); - O G" W; N' `. N1 {* u
- digitalWrite(_DataPin, HIGH);& U) x5 c. t" {5 {- O
- }
. R# R1 t4 S, e0 U1 a - #else // ME_PORT_DEFINED
" x- x( b3 D$ c3 n) c3 C - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
% K/ {$ Z2 {% Y" q& s - {6 B0 ]5 J% K, R8 W
- _DataPin = pin;
+ u9 C$ K; s% B8 B. q - pinMode(_DataPin, OUTPUT);
/ e8 X a- W) ^( Z( a A - digitalWrite(_DataPin, HIGH);
9 c$ j, q, _$ V0 G, u: K6 k - }
& ~) j$ V6 @1 ?, o( I) E0 A7 m - #endif // ME_PORT_DEFINED; ^7 V4 A, v/ I% y, T" o. L. x k( S
8 }7 ?; n2 I1 m) A" ?/ p, A0 K- % L7 W+ L! V4 g
- void MeLineFollowerArray::setpin(uint8_t pin)- e7 k7 R. ]! ^% {
- {4 w( P0 W) v3 ]3 z9 o+ w
- _DataPin = pin;0 R9 V8 x9 \1 ?% a0 ~; H' e0 T+ r
- pinMode(_DataPin, OUTPUT);
) Z; M. Y' l7 T3 m5 I0 b- n9 j; N/ x - digitalWrite(_DataPin, HIGH);
3 f8 J0 ^% g+ ^1 k8 M& w
6 V' T& X- k8 P2 e- #ifdef ME_PORT_DEFINED1 h7 N- n! Y9 n+ J- p" j5 k0 Q
- s2 = pin;
" I2 f' J- N, ~( x - #endif
+ X' f9 ]: o5 Q4 b& ^" A- z% l y - }8 y; U" f8 m( l( N# p
5 B9 s" ^: V4 A! w/ y; ~! y) Q* `- uint8_t MeLineFollowerArray::getValue()2 X% r5 F7 S N+ }6 i& ~; o+ ?
- {, r. `, g7 ?0 `) O, W
- uint32_t LOW_level_read_time;0 E* M1 I/ k/ {! D! h, x
- uint32_t HIGH_level_read_time;
; }8 |" i/ g( o3 O5 j- X4 Y/ J* [! T - uint32_t time_out_flag;
# }' G& y5 ^6 T4 F% `+ ~ - uint8_t Sensor_Data[3];2 d" V: n3 X. Z. I$ }, p
- static uint8_t old_data = 0xff;
& k# ?, p+ `( `4 e: x0 b - ( g7 e( L8 X7 A4 v5 Y3 [% W' d
- pinMode(_DataPin, OUTPUT);
9 p. \+ f1 f: j: o - digitalWrite(_DataPin, LOW);
* c1 h- I1 ^# G8 A/ y& X, I1 |) l - delayMicroseconds(980);# ?% B* y, O0 T0 z( }
- digitalWrite(_DataPin, HIGH);9 w& [+ I: M) W: u/ R+ V' S
/ e% _$ p4 M/ `: {5 p- pinMode(_DataPin, INPUT_PULLUP);; t k7 ]0 p2 c' z
- delayMicroseconds(10);
. T* M8 R$ M; ?/ Z- l: g
4 n) Y' }0 T3 v- m/ N4 q- time_out_flag = millis();' e* R4 s# i/ K% m4 _
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 [' \+ { N: W7 e9 U% V- S
- 9 E& k5 l4 o$ [2 a6 u; d( g+ s
- LOW_level_read_time = micros();& G/ C! F3 F3 \# ]& ]+ u( w% r/ L
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! V" B5 i9 o/ f: C0 h2 O" O
- {( \+ B A" o# }3 f! v# u" c
- return 0xff;
# U5 Q9 h% q n! w9 A, I - }) Z9 x0 g) X( W& s, i5 @
$ i: X6 Y/ p& s! r5 }6 r- d# m3 m- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
' R2 Q$ }: `! r [! s$ J' E - 8 n, J$ I% r' H) v( b/ K' E
- HIGH_level_read_time = micros();
7 V% d3 K' `1 i) \5 q) W6 G - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
+ U9 s6 H" Y/ f; \) |& ` - , I' G% F3 d7 K I5 W( P
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out$ r4 s6 t5 C; r7 q7 }0 F8 m
- {
5 t: Q, {: W, m1 I. U - return 0xff;0 s; [& w' b9 i1 T
- }$ F; h) y7 p8 q* T- R. @
- 3 b M9 ^' h" ], R+ f) `% O+ o# W
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
& y1 \" ]6 ]: r& R7 e - {, a- Y" W; b/ R9 q9 ~" H& T9 w4 d) ~
- return 0xff;6 w8 M/ F! l5 W
- }
' j9 [8 n% j9 P, d7 l
' a8 G; ^6 h; E0 n. k- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );# D6 l% s! ?, b% A: \! [( [6 X
- LOW_level_read_time = micros();
9 l' k. h3 L3 _! j2 P) b$ \ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
- }+ b4 E1 z2 o6 K
6 K8 B: c( u1 A- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 K) |( f5 i: x, B5 j - {( {2 V D5 A7 W# m; a
- return 0xff;
% M3 l0 U- N. |; ^9 h - }+ a8 w8 M' K1 k; B% K) S7 ~
- `- N! C) [8 z/ o% i$ ?8 r) Q- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))6 V) V$ D, n1 I- u( u7 t
- {
! K/ Y- ?/ d$ X) v - return 0xff;
" f1 s+ j& j0 Z. x2 f: x$ v, M - }( Y* G% G# g# ]3 h
- ! R8 G- F* @0 A+ f+ U6 z
- for(uint8_t k=0; k<3; k++)
2 n0 y+ }. h5 J - {
- S! i1 j: }* ~* v7 a3 Y- a0 F! q$ ? - Sensor_Data[k] = 0x00;
) c6 P N0 |5 o7 m# x - " x1 R. ~8 V7 v
- for(uint8_t i=0;i<8;i++)
1 `5 v8 j1 B* R3 ? S7 I3 b! \8 p - {3 j1 G" A7 b; Z( E |) v0 ^
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level: Q6 i5 A8 u9 V# w6 a/ F
- HIGH_level_read_time = micros();* n0 N3 Q- z i; U% [
- LOW_level_read_time = micros() - LOW_level_read_time;
4 X o. b1 a) {0 ~) V3 m* C - 2 H L; [9 w% W0 C, I( b
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
2 D! S# p. }$ ? - {
& S a4 h) U6 z. @8 g - return 0xff;
0 L. a3 V5 D% y - }& N% @5 y/ W- f' B4 O5 _5 S& w# D
- % v% H; S8 H1 o
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 {7 ?! g! r2 `: ], i6 g' \ - LOW_level_read_time = micros();
2 @1 ^. j. W% C3 x8 V" c - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
0 d- t0 P! o9 b+ X2 u. W - ( K( V# D: q) a7 {) f' I/ E
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1" }8 x! Z# ]& N( K% ]
- {, ~: D& v8 f% D. _
- Sensor_Data[k] |= (0x80 >> i);( u: c, X# J; p1 {' e
- }
. y. U" [! k! v$ ]4 t1 Z - else if(HIGH_level_read_time >= 100)
6 c+ \- l8 E" { - {
% j; {: ~7 P, _+ A$ M7 } - return 0xff;% U8 {; d" Z# }8 H0 {' y9 L
- }7 |# _% x5 V1 n+ j
- ! r8 h% {1 Z1 j
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
1 @- l; [3 H9 x5 d. J, R& d5 {1 N - {
. V- w8 l# ]8 g9 ?8 ]6 | - return 0xff;
4 X5 N8 [$ x3 P2 F7 r9 |, l - }9 s+ }4 f8 N" _6 B7 J
- }
/ q g4 c: @* D. l - }' {$ V4 L; s6 C0 z- x9 \( O2 N
8 L D% l3 s) y* \$ \% Y- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
9 J# F. m5 f7 ^2 O( T( ~ d - HIGH_level_read_time = micros();7 G4 C! p/ n$ U
- LOW_level_read_time = micros() - LOW_level_read_time;+ @! m* D& N% s% q% M7 Q
- : W6 x; f. O$ }4 G% P, z
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
- C$ K& `% d9 r' F - {
& F) T! N. S L$ F8 M0 E - return 0xff;2 T$ G+ Y; M9 X& G, t5 J0 v% k3 N
- }) o% V8 n6 N* |" s! Q+ n6 G" E1 M
% A: o4 k* B1 R7 Y- pinMode(_DataPin, OUTPUT);
* C. i- l( \; l$ G - digitalWrite(_DataPin, HIGH);
s0 d0 Y0 J5 n* _6 C, ]* m - ; m5 H# j; j4 ^* p# m
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
! r1 `1 ]" s \# l. k - {
8 ?( Q/ y8 O. v - old_data = Sensor_Data[0];, n; O2 s/ @3 b/ y
- return Sensor_Data[0];' J4 k8 |" }0 D; F3 Z
- }
0 h$ d3 Y% k$ d - else
/ b: j8 d* j% _3 { - {0 j; ~: _; Y l
- return old_data;6 _7 ?- k' I" z5 D, C9 x7 Q* {
- }5 r4 ^1 y- a, Y( ~; R" z6 ~
- }6 x& x( v& i) z1 d# r; N9 h$ N
複製代碼
! T; I/ u# T% u+ P& O" \MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
: _2 X& U8 C K/ e0 ]- \
1 X( w+ w U" G2 r* u
|
|