|
|
* h0 @0 M' V0 ]: q5 ~
- #include <Arduino.h>, t) o: Y* a4 l$ {% D
- #include <MeAuriga.h>% F" ?: d1 r. H" U8 z* E7 q+ o. `
- #include "MeLineFollowerArray.h"
]( Z1 G1 D* W) E: r1 \9 g - ) w c6 c- h$ |1 ^; s; q+ d2 @' {- R
- MeEncoderOnBoard Encoder_1(SLOT1);
9 d$ J: m* r, i( T0 S8 }. Y6 i - MeEncoderOnBoard Encoder_2(SLOT2);
, Z% j3 b3 I; ]% ~ - MeLightSensor lightsensor_1(12);4 [, {4 l+ a/ d1 ~ H
- MeLightSensor lightsensor_2(11);
' F0 k7 ]0 \2 ?% ]( c T1 K3 K4 ` - MeBuzzer buzzer;
$ K) R: s5 H! z# I4 Z0 p# V - MeLineFollowerArray linefollower(PORT_6);7 }, T/ W4 A; }( I9 s
2 \0 l9 z& ^6 d5 T4 `8 ^- #define Error1 1; W9 ^' j. X$ N: X: {
- #define Error2 2
3 y3 J; Y0 [4 T: x - #define Error3 3
# Z' d/ e0 z: R: H% m - 1 V1 |' E1 i0 _/ N) D
- #define Kp 15* {8 b$ M, b* p- U# n( O" D$ g/ k
- #define Ki 0.15& Z1 L. e) C0 h% v1 a4 Z
- #define Kd 0.03
0 e; Z( C, u6 X: C$ {) \ - 7 O; K) K" K* ]2 ~4 b9 ^7 O
- uint8_t sData;
! M* B" X, {# C) ]. J: g - uint8_t D1;
: f$ t3 Y$ _& p a* m4 ^ - uint8_t D2;
- C$ E9 B: z. {# C0 V - uint8_t D3;$ ^+ w: W/ ]2 \2 e
- uint8_t D4;
1 P( T/ {, T8 e& l- {# u. j - uint8_t D5;8 A* v1 L4 n/ r: O3 h) C% s8 B
- uint8_t D6;
# _" G# r6 M- N4 }7 ?4 j; b - * I3 o S! m U
- float previous_error = 0;
- x7 ?. e/ ~' b4 J- d8 ^6 A8 M# D - float integral = 0;
1 g0 W ^- p2 O' B* T+ G( x, i; K - float derivative = 0;
, q% ?& f) M4 z: s- e - int Speed = 160;
/ _( p8 Z5 M& N5 x8 q2 [6 p - float output;$ q( ^. v4 l9 `) I. B
- E0 K) {7 f! I p% U* v# g- byte Left;: ]7 D f# o9 Z! ^7 E2 Z W4 C
. \9 p9 n! A( _7 z. j' I2 j7 u. c- void setup() ! P6 h" f% O# D8 U4 v7 R
- {
# K( o4 v* O( _* t# o/ \& L - //Set PWM 8KHz
3 y; a9 Z. N! P! x s1 T1 m; S - TCCR1A = _BV(WGM10);3 {* n+ E5 n3 z! ?; ?. s7 z w; }
- TCCR1B = _BV(CS11) | _BV(WGM12);5 I& ?/ Q; {1 Z1 |0 d, L: @
- TCCR2A = _BV(WGM21) | _BV(WGM20);+ b$ ^% ~& n, l& j4 y" C5 o
- TCCR2B = _BV(CS21);
1 C" J# ]* f$ e7 g+ R6 ?5 X; I% \ - Serial.begin(9600);
2 `5 Q( {4 ?4 ]1 I# E, A: Y - buzzer.setpin(45);
- B* Q- s# L7 P5 y6 d( Q - }6 E) ^& w/ F* p# S% q& h
6 V8 {1 b* t5 f3 y- void loop()$ P& F7 W$ ?+ @0 w s
- {* S0 v6 {" z2 k# j" _. c' P' R
- while(!((lightsensor_1.read()) < (10)));
0 {# E! d+ ?- x+ E, t - buzzer.tone(1047, 500);6 R9 K+ A3 B9 V- Q0 E/ ~" `4 _& ~
- delay(500);1 O5 F3 o7 J5 ]% @3 }+ O& K6 j! B' C* u
- do) L' o- ~+ [. i9 R" v0 [, q7 J) m
- {
7 G! G6 g" O' j8 S) n9 a - int Err = getErr();/ m, l% `0 a% j8 }! ~
- if(D1 == 1). @! q0 m! q" C- w1 R
- {
0 L" e* k$ r# k* e- u. d2 o - Left = 1;! S/ }/ p8 r4 B/ E
- }
$ b& h0 ~7 ^. m- |; V$ K$ G - if(D6 == 1)8 f# T$ [" g& K+ T' y4 p
- {6 Q) }) X* ]: o) G7 }
- Left = 0;
+ p/ T4 n+ b6 H/ e, _8 N - }
6 Y$ z( o0 N' Y. r" T, b, Z8 A - if(Err == 99)8 K7 \" V" j; W2 k
- { t- J# A, {; f2 _$ Y, X
- if(Left == 1)
% K# A& D" b8 O7 D - {/ t7 C* l- ]) E7 |3 ]- @* A3 H
- Speed -= 5;
5 U. l0 ?6 s. L9 ? - moto(0,Speed);. w4 c: ^1 i+ B6 E9 F! d3 r; _
- do
- c6 b4 y V( u. N$ X7 n - {2 c& e6 W& {% ]/ L9 U5 @
- Err = getErr();
4 w2 }! o/ |6 [& G/ A - }while((D1+D6) == 0);. r) }$ O$ H, S; q7 Q
- }
" M7 K2 q' h2 ~) O" V7 h0 p# S' x - else% V( I y i2 z+ e$ x; |1 o: J
- {' ~ i# I! E/ {1 b, R$ S, Q4 ~
- Speed -= 5;- U( D5 W' W d, _$ w
- moto(Speed,0); ; X0 l: l7 l! C5 J5 w) k
- do6 d" u# Y7 a9 h& `
- {
; R; Q" \% d$ U) w - Err = getErr();
8 L# x+ u6 n# T2 O - }while((D1+D6) == 0);
: C# J( ^: B+ E( e* }" D/ v1 J - }
# C7 u" A+ Y* A$ i5 {1 Z0 e - }& r7 q" r% v; K
- else
0 P/ S% F/ x$ q( `" \) G9 ] - {9 @3 e$ z2 g3 x2 p8 l) }' i' Z
- if((Speed < 160) & (Err < 2)) Speed+=1;
3 f% m7 U6 u3 m4 F - if((Speed > 100) & (Err > 2)) Speed-=2;
2 `: l2 p a" z4 g3 B1 O- E+ B - integral = integral + Err;+ f2 Y2 J5 E) i. R
- derivative = Err - previous_error;3 j3 g" G' x" r7 f5 q7 @1 w0 y' _2 o
- output = Kp*Err + Ki*integral + Kd*derivative;+ Y( X2 {/ D1 i/ H
- moto(int(Speed-output),int(Speed+output));7 Y. l2 f x/ `! ~% l5 F f7 }: }6 R
- previous_error = Err;
& f+ {& d3 b5 v- z; i( g - }
" j2 f# P N2 M3 v V. |4 i - }while(!((lightsensor_2.read()) < (10)));' l8 |+ R: b% J' ?0 {
- moto(0,0);- l5 m; _4 a# ?- N- b. I2 n
- delay(500);/ l4 s! \# l3 s! m% N* h% R
- buzzer.tone(262, 500);
8 s. p+ ?* V( F - }
! Z% X t9 d" m/ g: D
# C; s) E+ K. l W- `- int getErr(); b9 T" S$ ^9 k- M
- {
/ e! q( Q: f. T - sData = linefollower.getValue();
3 Q5 ?! ?) w4 ?. n9 F H" E1 n2 ] - D1 = ~(sData>>0)&1;
7 K4 n2 O) {; @ P5 ?0 A; a+ Q' v - D2 = ~(sData>>1)&1;. W& w& I" O7 Z7 V
- D3 = ~(sData>>2)&1;
+ ]7 W8 H, V" [) g$ j9 u( S - D4 = ~(sData>>3)&1;
) T. a% n( C# g. H& y% g4 v# x - D5 = ~(sData>>4)&1;
: S6 u/ A5 h9 Q4 T - D6 = ~(sData>>5)&1;
: T% Q7 H& ?9 L5 U) K2 O - int downD = D1+D2+D3+D4+D5+D6;
# a5 Q+ k, F6 o2 w5 J2 r$ O - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);& a3 a$ U, h* I3 N3 p6 x! ~# z8 K
- if(downD == 0)
6 t# u* J- C' T) V7 ~ - {
2 d6 j6 U( r* Z2 C - return 99;
8 ^. S* k6 f- @6 M1 R - }6 g% k S* S* @# ]+ G4 X
- else* S! N( k4 ]: Y" { E' M
- {
: v, }' J2 ]* X - return upD/downD;' e$ P5 W) `+ k. l/ ]3 B6 @
- }% x' x3 t3 g& z6 h6 H
- } ^+ j' j2 P: ~0 b8 A M! W' F
- 1 c! U. }7 M0 L9 F8 l2 q" @
- void moto(int Speed_L,int Speed_R)8 A6 m$ H7 Q, y2 }7 {
- {1 t5 W2 S5 w; a( U4 o: W# Z
- Encoder_2.setMotorPwm(Speed_L);- }) S) f+ H ~# G0 Q% f. h* D
- Encoder_1.setMotorPwm(-Speed_R);
5 ~: f9 [0 e0 |+ O( M* h# _ - }
複製代碼
7 M, v! \% y$ j7 L. j, f) eMeLineFollowerArray.cpp
; E0 F4 J1 w4 N( r! c6 l- #include "MeLineFollowerArray.h"9 k3 p2 E0 Y6 F! N- w9 E3 d: `
6 f- L. ^ V" A" B+ h8 g- #ifdef ME_PORT_DEFINED
/ w+ w6 b% |) v0 t - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)' g" Z" W, K0 e2 `, v, G, `, j
- {
/ {, N. K! }3 u1 \; c+ Y1 w' o* J
6 C) J5 y5 x6 b$ m# l- }
/ ^ o8 i" g4 `* H - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)( a6 J, C: J/ h
- {. [1 c; z2 ~( ]
- _DataPin = mePort[port].s2;6 f3 v# r! n1 q$ U( ^- V- ~+ r8 ~' f
- pinMode(_DataPin, OUTPUT);
2 T3 q+ {- F R" K - digitalWrite(_DataPin, HIGH);8 T/ S. [+ L6 I& m0 X0 K
- }" ?- |( c4 i# g7 O
- #else // ME_PORT_DEFINED
7 c1 f. Y1 q$ Z0 C - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
4 x! a7 k% }1 X3 J/ I - {: y; |! c Q: b
- _DataPin = pin;, [/ O1 W6 k0 r) N3 }& Z/ c
- pinMode(_DataPin, OUTPUT);
4 K! W" \' M1 C z) R6 X - digitalWrite(_DataPin, HIGH);
! \# j% `, N% g+ o) A/ V - }
; a4 `: I/ o) l6 P! e - #endif // ME_PORT_DEFINED
+ [% H9 z z, Q4 m1 g$ B - & q3 G' h% {0 l2 l2 L5 S
3 g4 c: c- J. v9 g2 L- void MeLineFollowerArray::setpin(uint8_t pin)
! z1 ^$ R/ T3 B6 b$ q% @- \5 y - {
; W( o0 }( B* z- o9 X3 e2 b. Z( u$ |' Q - _DataPin = pin;
0 S9 R5 j8 R9 `% U: A, } - pinMode(_DataPin, OUTPUT);
/ e# A0 ~. a, b5 {% @. A' N+ v - digitalWrite(_DataPin, HIGH);8 s, \9 G: K6 |1 r& v3 `' d; ]
- 2 p" z; q- d+ ]9 B. j
- #ifdef ME_PORT_DEFINED8 j* ^8 h8 M/ W9 y# g' c
- s2 = pin;
! [/ `- X6 c1 e$ h - #endif
9 L- X/ N; G# T- l1 m: {+ R - }8 Y' W- G- D9 g5 b4 j
- # C+ G- l' p( T" ~) L
- uint8_t MeLineFollowerArray::getValue()
( S: `$ D! P' v# i' C2 t - { b/ T+ n# `% M
- uint32_t LOW_level_read_time;$ N( o! H1 B( ]3 |
- uint32_t HIGH_level_read_time;
( f. x0 k+ N0 u6 g1 A - uint32_t time_out_flag;
0 ^; D$ U: e: n - uint8_t Sensor_Data[3];; u+ g |2 b1 E- L! E
- static uint8_t old_data = 0xff;
& {$ z' q3 {& U M/ ?0 F
- x# B3 u7 `, [: t) x- pinMode(_DataPin, OUTPUT);
& K) t {3 M0 P; K' `( T( U7 x - digitalWrite(_DataPin, LOW);
0 K2 k p: C7 z3 l, U9 F$ I - delayMicroseconds(980);
- o( Z; X0 ^( Z% d - digitalWrite(_DataPin, HIGH);
4 i& ]- e, L6 C- v; r
$ W0 j( y2 H0 C5 g- pinMode(_DataPin, INPUT_PULLUP);) P v: l2 r: f; n0 M
- delayMicroseconds(10);5 g6 P* p5 O, f+ v, j
- + n3 N: K( v1 ?
- time_out_flag = millis();
& ~0 k0 r0 }# i4 B - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: {5 d$ Z. _; Q, l
$ M$ z L( l' r, y7 g/ l- LOW_level_read_time = micros();
2 U( t; W# [# `5 B( o+ x5 v! ]: k- O6 n - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 I6 }8 X& b& Z( |5 [! L( g1 c - { c9 V( M8 E: _$ p3 U4 W7 P+ n
- return 0xff;2 y6 h$ g9 c2 P% T( f
- }
' @$ k" Q( f' A/ N( C) W - / b# z0 U9 _$ H. g% }" b
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 m3 k2 ^ e$ ?" h
- L3 u+ m$ {2 _ j3 L& J9 s2 L! i( _- HIGH_level_read_time = micros();
5 F9 C8 \' V; W% Z8 E" R# S - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
7 U1 ]7 G2 A7 a4 O' K a - 3 o1 R c; _5 Z& m
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
- t' T7 A+ w( d5 k9 Z - {/ [* z8 y, p; b, _6 Y
- return 0xff;6 e/ R2 q1 T% H( ` J. G; l& s
- }
! ~3 t4 @& F# m" r - ; ~7 A+ c7 @/ b0 e# U. }: o2 l
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
+ i% M ~6 r6 |6 N - {
6 S8 }, q$ z# G% z - return 0xff;" M/ ~! p- v s9 A
- }
. u. W& F1 j$ P
, C/ a$ `# }6 q- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 j/ W, P8 u2 o5 h - LOW_level_read_time = micros();! u0 e, r' B2 \$ p. F, i
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
. g Q; a% E& o+ L# x! I8 s - 0 w) j+ r! X$ Z( U
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
* @$ Y0 m3 T8 E) E7 Q - {2 L3 W3 r0 k' n# Z" H' A& i) B
- return 0xff;
) K7 t) L4 d8 q9 T! w - }
+ k5 |. T4 \, c+ o( y3 g' ?. `6 c5 N - $ d$ ?! z' q# N6 R( _) k: q! c
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
& f: z9 q) d3 w( v/ O - {
G8 Z6 W1 C! Q6 Z - return 0xff;$ u! ^: F1 M0 E3 I4 k) S7 p9 r
- }
# f. r6 M b9 h2 f% V8 F. |5 i: r7 ~
! v3 Q: [5 X+ f) w- for(uint8_t k=0; k<3; k++)! e' @1 F) K4 p( v3 A
- {* u/ s& X( n; ]+ H" l, p
- Sensor_Data[k] = 0x00;
% V( `# I9 `3 Z* w - ; w# \; `: c7 F
- for(uint8_t i=0;i<8;i++)
0 [' p# E3 t2 _" J: E6 E5 r - {
# _7 p% _( m8 g( ]$ }* J- E" ] - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level+ r0 l1 R; n1 Z* y, T5 k3 N
- HIGH_level_read_time = micros();+ _$ E) `7 ~9 Y8 k6 q! p( J& u
- LOW_level_read_time = micros() - LOW_level_read_time;* F1 x1 b( `1 R: b( J8 y
& b: h8 |0 E" m- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
& e, Z( Z" p( y4 A - {3 E( j1 \$ W9 [$ @7 L
- return 0xff;
6 S6 n" Q" C( k2 \0 I( k9 u' S! P - }
. Z* ]( r1 d! P f5 |6 x - & R: y* G; A9 K
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );! [+ a/ _9 a0 N6 @
- LOW_level_read_time = micros();
, L$ ]# z* h' `# d - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
4 n1 ^* M' Z8 \- S* ~ - 6 v3 B8 z" Q2 Q! C, }% }7 C0 c
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1' l4 U6 L# ]) ]2 e) ]. [
- {
8 y' \+ Q6 ^4 Y1 M - Sensor_Data[k] |= (0x80 >> i);% w1 ^# W, {! b. X2 p( g
- }$ X# B4 v# F! A/ l# P3 m$ a) E7 {
- else if(HIGH_level_read_time >= 100)" e M k5 U. m$ p, c
- {* `: v" o6 T( O# \
- return 0xff;
( f* G5 p$ a- N5 M R# B - }( N7 U: u9 t- [2 a5 I6 f
- 3 O/ r5 R$ {* g) w
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)# B: f; J5 N5 p* C `4 `/ w5 R$ W0 x
- {
$ ]: a+ W7 D$ Z4 V( |) e; P+ \7 G/ m, X - return 0xff;
: ~- [. d6 T+ `- r a5 w. O; N - }* {) ~6 p2 V* _
- }3 c: Z) N: t/ ~
- }
2 a& V1 m/ m6 _7 a2 ` - & s- A8 O3 C) v8 ?1 j
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level8 ?+ @5 _* B8 r! w
- HIGH_level_read_time = micros();+ O6 r; E# h* p! C
- LOW_level_read_time = micros() - LOW_level_read_time;: O& i& k( b; Z9 Q5 I2 C/ L5 H3 G+ N
- ) n4 V6 z4 i& P3 I5 a: h" [ o
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ). h" a* h; V6 E4 Z' M7 `' M
- {
. n- _+ s- |6 q, U) \ - return 0xff;
0 ~+ z8 a3 r/ k" x - }
" h, E- A; P+ x
. H$ G' f3 L- n$ }9 p+ K- pinMode(_DataPin, OUTPUT);9 B# P8 x2 C9 M
- digitalWrite(_DataPin, HIGH);
/ n. I0 ~. x& X# e3 K - 5 P; o& X2 V# E' B+ @ ] q
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
' `5 g) ~, n$ X$ M) R5 n; J - {% V/ l6 E5 g+ `
- old_data = Sensor_Data[0];
: a0 G( O9 k' p$ ^( [ - return Sensor_Data[0];
: l! l. ?2 V' s5 @2 l - }
& y: T! ]) I; w3 @# k% ] - else
8 B8 b5 C+ }: H* l; m) i6 q - {$ {, L/ d \+ j; Q( F
- return old_data;9 _8 ~8 ?* I: f$ u9 X7 h
- }
+ R o v& q1 {! h# H7 `! t - }
7 M; ~1 l$ h* K; t- \# e
複製代碼 $ t# X2 l$ |+ a$ B% Y
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
. a8 w- r7 s( ^
/ `/ E2 g# y% S) [ a# f% E |
|