|
|
, z6 y$ ^6 L2 a4 f# D- #include <Arduino.h>5 N$ d8 M- W' m+ S* f5 N% J. p
- #include <MeAuriga.h>& M0 t- `! @; j N0 y' [
- #include "MeLineFollowerArray.h"/ ?7 x( S5 h+ R& ~
( Q" o6 A5 C3 w* n2 K- MeEncoderOnBoard Encoder_1(SLOT1);
7 w3 w* j7 R3 s; k3 X$ v W! G. G - MeEncoderOnBoard Encoder_2(SLOT2);
0 z, N& k& V# ~ - MeLightSensor lightsensor_1(12);+ V3 e' I* k3 m8 D/ o" s: @1 ]
- MeLightSensor lightsensor_2(11);
) U( F, y1 K, ? - MeBuzzer buzzer;
* `! ?. w# p2 o* s$ Z$ Z s - MeLineFollowerArray linefollower(PORT_6);
0 o! M4 e% Q) j& q- X$ ?$ ` - # Q% `7 E" V$ F7 K. K1 ?5 S
- #define Error1 1
& H/ ?7 H7 @+ b4 B, r - #define Error2 23 l( R$ p3 @* d, {( Z9 H
- #define Error3 33 u y" c# t) ] G1 k. H" M
; O% m, ^. l J5 b$ y# g7 |: }5 j- #define Kp 15! B5 v/ }! h r3 ~
- #define Ki 0.15
2 U1 L5 I8 Z8 N* q3 x# k$ d - #define Kd 0.03
4 B1 L7 g# ]6 ~* H, x - ; H2 {' c) J5 d. u* o
- uint8_t sData;
. _' b @; J" ?2 c - uint8_t D1;. `- k) l$ o) V
- uint8_t D2;" `/ N* L/ o, L+ x! G n
- uint8_t D3;: K4 M2 P6 I8 V4 h
- uint8_t D4;$ B% U! C$ d$ q6 W2 |, h
- uint8_t D5;
5 d$ f) s, W7 I4 C) }# B - uint8_t D6;
5 r7 d! ?$ |2 ?3 B
/ i) n: r9 Q" f- p- float previous_error = 0;
+ k2 W: T1 n' | - float integral = 0;
, L' y J+ H- A& h; V) b - float derivative = 0;
, c* n; c1 r& {! q& Y - int Speed = 160;
7 o7 g$ u+ }! F3 H) Y; d - float output; \2 B" _' v `, F8 `0 \
- # H+ u4 f7 e- o; r4 m4 O
- byte Left;
/ ]2 ]* F0 D: {) y5 L- y7 b$ o - : S; o/ a3 f; I0 I B: {" l8 Z
- void setup() / a V2 P8 i6 Q! m/ Z
- {
1 d: I" C' j f+ E5 k - //Set PWM 8KHz3 s5 A5 ?) I/ l6 b4 F) i
- TCCR1A = _BV(WGM10);. R: R) j# U% w* F' u! ]# X
- TCCR1B = _BV(CS11) | _BV(WGM12);! H* r$ ^! H" q. W+ B
- TCCR2A = _BV(WGM21) | _BV(WGM20);1 y1 Y2 ` j8 G
- TCCR2B = _BV(CS21);
/ Z! i+ p6 @$ r0 O r8 c - Serial.begin(9600);
# q! l! w# D5 a - buzzer.setpin(45);
8 K( c8 F& a \" f5 y4 f% O: Z3 ~' T - }- _) M& j, n3 [% t A8 T& m
/ q# @7 F" l# I6 w9 \( ]- X+ n- void loop()8 E2 n2 R8 x% { E1 G
- {
, N' O- b8 q+ K" T, [ - while(!((lightsensor_1.read()) < (10)));
" _4 o5 p5 d& o1 g2 B `7 ?1 M; E - buzzer.tone(1047, 500);1 p J, v/ b+ Y1 B) E
- delay(500);
5 i$ O p7 ], X: K9 M, C - do! n0 f- C/ K5 t! Q. M& K
- {
$ E: S' h, ?' `% Q6 }5 u8 u- k4 Y - int Err = getErr();' ~# t4 \* Y, Y+ Q
- if(D1 == 1)
+ |' i8 q6 ~) x. _2 c$ y - {
1 d$ e/ `! s( x - Left = 1;
% F: a% m& R! o6 |! H' v - }! O/ T0 i" C+ I1 ~. ^) W
- if(D6 == 1). E, E8 C# d' s/ w% w
- {
0 X' F% {* p( k! C4 [: I7 U2 e - Left = 0;
0 f1 N$ c9 w: W: y) Q; K) l7 z& o6 j - }; u: ?; g* z7 m, ~) g
- if(Err == 99)
+ ~2 p6 _8 _& g2 r. M& x' I - {
$ n; u6 ?8 V0 n5 M+ B/ g, D& Q - if(Left == 1)
2 R# O/ y4 f- }; F - {
) j) R& q* [4 w" v - Speed -= 5;
# X& n5 C$ ` _) C9 j - moto(0,Speed);
1 E5 J# r( u- v' t" N - do2 r# t' \1 T! V) E
- {2 \# ^" g/ `) r3 z( A
- Err = getErr();
( q3 K1 u) }9 _% _+ B; q - }while((D1+D6) == 0);
3 B% k- S- n) g; i: w+ ] - }; `1 Z' \3 \5 R: h! k* o2 n% t" M
- else
! z0 t, y% B5 F - {/ ~1 q% c+ j3 G6 _1 ^$ i3 r, K" n
- Speed -= 5;
2 [; C8 Y% H4 Y( _ - moto(Speed,0);
, F" }2 _( h( |' Q3 ?6 ]: c - do
) S4 q5 o1 |8 k* e* C - {
6 e9 q# b0 k, d- Y, t8 {+ j0 s - Err = getErr();
J2 Q* |& c# \- k8 l) N - }while((D1+D6) == 0);
" R) ^# t' [ l - }1 D/ p7 U$ S/ H% @/ r
- }( ~ A0 H5 ]+ P9 }$ U, j, R' S( n
- else+ ^6 L' V5 @ @1 c
- {6 U/ x( J- v/ J6 H
- if((Speed < 160) & (Err < 2)) Speed+=1; 7 o' ~+ A& E* c1 n+ I
- if((Speed > 100) & (Err > 2)) Speed-=2; " X) [+ D% r6 F1 R/ D; I! d
- integral = integral + Err;5 n8 B& L) e% b5 I, q5 c6 d" ?* ~
- derivative = Err - previous_error;
' K. Z- d6 L1 }$ H0 c. @ - output = Kp*Err + Ki*integral + Kd*derivative;8 g$ p" P; D% d
- moto(int(Speed-output),int(Speed+output));
% c, c8 C1 }+ `# F( S* ~# O - previous_error = Err;2 o- ^+ N% g1 o/ z# I
- }
' j3 _3 w. w! Q; F1 \ n l - }while(!((lightsensor_2.read()) < (10)));
4 q: p7 c; s9 t1 K/ Q - moto(0,0);( v; M+ |1 l6 _0 X
- delay(500);
& z q1 c5 l& Q2 Q) l8 z - buzzer.tone(262, 500);
* M6 M# ~2 G9 k - }- [) E3 [- E" }& ~6 X& E/ t1 e) k
% E9 D; N7 r5 n) s- int getErr()+ u4 ?3 y7 M' c( b7 g" ^
- {
0 u( @% n: C7 @6 [ - sData = linefollower.getValue();
6 V; m7 Y% N1 j% q" ~- B3 I - D1 = ~(sData>>0)&1;
1 r& h. j" y* d! _! U - D2 = ~(sData>>1)&1;
; z% E: H* K; T! E& b2 T# R - D3 = ~(sData>>2)&1;
8 b) e" a8 d! ?# Z5 l$ Q, S5 q - D4 = ~(sData>>3)&1;
5 d1 U7 R0 w; W - D5 = ~(sData>>4)&1;
0 ~/ s! G+ N9 {9 N4 j. s - D6 = ~(sData>>5)&1;, H( E9 X" T! u6 n
- int downD = D1+D2+D3+D4+D5+D6;
7 c: h$ C# @1 Z; t. E2 }4 x% c" G - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);8 E) v6 ?' a( H+ \' _' C* h8 w
- if(downD == 0)* \7 E! r) ?1 {9 Z& k" g$ f
- {
7 |- r1 I& x5 n+ M( T - return 99;
5 Q) u- X8 l3 {* y - }6 r1 Z4 N a1 F. L) T9 r
- else9 k+ i4 | f# y3 ?8 M9 K
- {
5 c* [: B( V8 O" l7 s1 l - return upD/downD;
, ]) t) Q6 a( h, l6 n+ Y; t - }2 J5 r. y7 ?% f; [+ Z
- }
1 C; }0 y6 h0 [" F) C
6 R. n. S: `5 K$ K$ C- void moto(int Speed_L,int Speed_R)5 _; R" [1 b3 m& Z* u
- {
- }0 ^4 N0 i r" M - Encoder_2.setMotorPwm(Speed_L);7 k' |" r* u" n! _2 i
- Encoder_1.setMotorPwm(-Speed_R);8 p4 |8 R% @- ~" R3 F% e
- }
複製代碼
c. {% i* {2 R9 D" p; n) IMeLineFollowerArray.cpp
& q% N( G' T0 D! c" ^- #include "MeLineFollowerArray.h"8 G' k1 g) E& w& L' E/ q
- ' ?6 g0 x4 O4 a3 p( ~" `
- #ifdef ME_PORT_DEFINED) o& G# g. M! O2 r+ E# U
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)* b: j! \% T* R( B2 t2 I0 j0 B
- {5 N: x5 r' r$ S( d! D
- . m8 J$ h) D( N: r: G
- }6 }$ Q7 o% p ~
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)# ^" N' j* g' r6 O, P4 n" J9 O# c- i
- {
' }' c+ e/ V" k - _DataPin = mePort[port].s2;8 J: y7 X; V h* z
- pinMode(_DataPin, OUTPUT); 9 _) X3 f+ m& H/ q8 Z1 @6 i
- digitalWrite(_DataPin, HIGH);
: C7 K4 @, [& }( g! N/ C - }- j/ Y% c( `0 d; q; q
- #else // ME_PORT_DEFINED% K/ h4 a% W2 O) n" L0 e
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
" n/ y1 F" }4 Y" K3 z& c& @: r - {2 @& \" d8 K0 ~: N# U
- _DataPin = pin;& q4 b) e! f+ u2 ]1 p( C. R/ Y
- pinMode(_DataPin, OUTPUT); ( X: F/ M2 o+ U$ k5 I$ ]. O
- digitalWrite(_DataPin, HIGH);
3 \ ~( i" c$ R( s) V2 z# G8 ] - }
G: } j+ q- `& M5 L) T2 g - #endif // ME_PORT_DEFINED
2 T: `/ m4 G' Y2 O
. Y% V5 w3 t; A3 O" F. \' H- " f {) S. v. J3 \, V3 q! }" O4 H
- void MeLineFollowerArray::setpin(uint8_t pin): q* y, B5 U( n5 W
- {- z' ?3 S; q2 z) P O W
- _DataPin = pin;
+ y! E1 m: U) K+ {2 r4 \ k - pinMode(_DataPin, OUTPUT); 3 N1 {6 |" x0 v3 l* ]2 J
- digitalWrite(_DataPin, HIGH);
/ p7 c; [/ j$ y, t - u4 t+ ?7 }. ^4 [, a
- #ifdef ME_PORT_DEFINED0 H- W- T6 ]; h3 f5 Z; I
- s2 = pin;. f% R0 B: p. w7 W" D3 A0 |) g( l
- #endif
* B/ o* ~ m; ?: c9 d - }
A) x: {. c! N+ e2 i - 3 U/ I( c- P1 Y2 {. b& }/ V
- uint8_t MeLineFollowerArray::getValue()
- n1 K; u' e1 u; X+ L7 ] - {
. |* O! D( \7 a/ p. I1 c& U+ z - uint32_t LOW_level_read_time;
2 h& [& A4 a3 Z3 C# K - uint32_t HIGH_level_read_time;
( t& F' J, H2 B4 ]( b4 Z: j - uint32_t time_out_flag;
8 z! q3 ]+ J+ P2 H" I6 J - uint8_t Sensor_Data[3];5 q m+ w2 d" M% M# S5 s x" a
- static uint8_t old_data = 0xff;3 n+ a; a* N7 Y
- 6 k4 @; m7 B6 h1 k
- pinMode(_DataPin, OUTPUT);& V0 Y1 P- j; Y8 @: \$ j, y
- digitalWrite(_DataPin, LOW);
( E7 U! ?% x& G' i8 `6 H - delayMicroseconds(980);
$ z4 t. Y) ^0 ?, _ - digitalWrite(_DataPin, HIGH);% M% X, ?5 L: Y& Y2 @
. X) i" s4 n1 a7 e1 S4 W$ P: t) ^" j+ d- pinMode(_DataPin, INPUT_PULLUP);& C6 X3 j+ X3 X( p( [" C% ]
- delayMicroseconds(10);
: z% }! N/ V* `$ g7 X - 4 \1 M% `- x) r7 O5 Y
- time_out_flag = millis();% ?5 d$ ~# {3 Y
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ }! z! @- {. O$ m7 o$ | - : }6 Z5 d, K e
- LOW_level_read_time = micros();7 ?* R" f! F5 e9 h! y) h
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out; b7 ?3 D; ~, Z
- {, n$ u- L( L9 c2 c0 R. q1 H j
- return 0xff;
+ T5 F* }# H p% B$ } - }
/ w( j: V* l& u
$ C u: Q( r) P( j: R- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );& ? \9 Y8 G. X4 R
- # F7 B6 Q0 K1 G
- HIGH_level_read_time = micros();
8 G% t( }4 n5 K; t8 |$ ` - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
: j i* D9 q7 }' M& j5 ~ - # N. G! `, c0 h$ `) q: ?4 p; Y# t5 p
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 _* c/ l/ Q4 a3 P# h8 u3 { - {
3 e5 {% M* ~" l0 B3 w3 W4 Y8 B - return 0xff;& _% I3 v5 p k4 D; ~+ I
- }% I' H& D+ D- r B
- & P" e+ ^& S3 y2 g8 U" [
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
$ T+ k# s6 l3 C* Y, l) a X - {$ n9 z6 n7 \: @: p2 |7 \
- return 0xff;
1 h/ z. L# d) _ - }6 _4 z: c% ]* n4 H% ?* u( R( z
0 b F( v( R3 }8 _- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: n3 g3 d% u7 X - LOW_level_read_time = micros();2 [/ }& |% i; Y. J
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
- E1 w& o0 ^8 C3 g( g
. m7 @6 }; j) {: {1 o- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
4 f) [3 _# ?) o! b% T - {
5 m D# H6 F6 U' J% Q ^; N9 u& K7 m - return 0xff;% P; \' }% z4 ^9 v- I
- }% U* D: d2 z( c/ c# ^: `- q
/ c8 P) G. ^ R- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))6 s v ^3 _6 a$ { L7 m2 L
- {
; p* N0 f1 D6 O) K, ` - return 0xff;
* L4 F p$ f% K! `# t - }( d( \) [8 Y) H9 Q8 B* A" X
- \; A4 E) }1 n
- for(uint8_t k=0; k<3; k++)" W0 ^! w+ N+ ]6 @* s
- {: L" \5 }2 K/ J4 T( I
- Sensor_Data[k] = 0x00;, G* w+ _# m5 U7 `( R/ z3 I
. Z) z% I- B x- for(uint8_t i=0;i<8;i++)
% l9 }4 W, L8 Q) W2 V - {
7 c& e3 f% G. A& t. ?6 e( n+ [ - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level7 ~+ M/ s$ n e1 W0 ]
- HIGH_level_read_time = micros();
6 `! K& a, O8 ~4 C9 l- m5 p - LOW_level_read_time = micros() - LOW_level_read_time;: z. H" d! d h' \ c
- & S' B6 q# C& X) T
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
( @4 N8 S1 T ~' T3 |/ F+ U- a - {) M+ s% d. Q$ X; A- I2 R
- return 0xff;
" w6 R8 s3 j# c; I - }" X; v( V: M3 w1 ]
- ! V/ e( n+ Q! D
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ M, H3 h8 b! J6 h# a
- LOW_level_read_time = micros();
B' T1 c2 c' E - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
8 m1 q9 U/ E3 o @
8 l0 j+ F& z- d- l. i6 P+ O- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1% I+ _$ S) d4 P
- {
6 E' K/ J/ N0 p+ ?, X/ o$ {' l - Sensor_Data[k] |= (0x80 >> i);8 V4 G' p" s% |. Z5 ^
- }) E, N, M$ ]8 M/ c% t0 ]# Z5 w! } d
- else if(HIGH_level_read_time >= 100); O3 M. M& c+ b) i9 r
- {* S0 J' Z) i2 h6 a ~; l1 G
- return 0xff;' e7 J1 {: D* x+ Z5 E8 n
- }
8 F; D( u! w5 m0 }
, G* t9 M4 a* a2 Z- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
6 z0 d% ]0 c7 V" @ - {
T2 f' Z" s- r$ O3 C - return 0xff;
4 P8 q5 k F$ P8 P* _; q/ }7 w - }( b0 ]3 c: i; I- U) y, }
- }8 z; `+ ?4 P \* P$ @$ U# e. M+ F
- }
! b. f8 T3 }: ]2 S9 ~ N. v/ y - % U* f) ], _8 b& B- Q! A0 _! M2 @
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level/ }; n+ V7 Z2 p
- HIGH_level_read_time = micros();
3 i' Q( @! Q; c3 U" z - LOW_level_read_time = micros() - LOW_level_read_time;
9 n" H4 D5 _' g2 o; }! Y
& a @! r* j5 h$ w! I0 ^3 l' w- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
! `# R7 S \& J - {
4 d4 V! ^! A. P" t8 O3 g - return 0xff;
5 a( d. \( n+ I3 K$ N - }
5 }% m/ i" R, t$ f, Y4 C" P
, a) A( i+ n$ v, _' C- pinMode(_DataPin, OUTPUT);0 P# h7 Y. i/ `$ \0 _
- digitalWrite(_DataPin, HIGH);
4 w4 l- O+ ]7 Q9 l& R - , r1 j' c* ?, x( u- V8 F
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0]))). V: b: |) @+ L# a: o
- {; j2 s( z, A: G7 |8 A
- old_data = Sensor_Data[0];6 M D" C' j: G
- return Sensor_Data[0];
7 }$ X: y( x- T0 g, b; K - }
* j$ J: a M" u: X - else
- N" f' t/ n( l- y$ c - {- R# D9 ], e* g2 n3 N
- return old_data;: _- L6 L7 B% I$ c! g5 o
- }
' ]+ {8 F& L& f- X* K) r! D - }
4 f0 K$ l8 B% r+ ?9 [: r* x
複製代碼 , O' S+ _' ~. k7 a |9 ]+ w! \
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
( f8 V$ U6 |* F* \7 `0 X- S
' d+ P* W2 b5 w
|
|