|
2 Q5 o+ G- Y* r9 F8 _" b0 C
- #include <Arduino.h>, a5 ]+ C$ {$ W N
- #include <MeAuriga.h>( Z5 u% u, ^) z% x. L
- #include "MeLineFollowerArray.h"# j N: h" ^. c7 T% k
- / t/ n6 I; n4 f+ a" C
- MeEncoderOnBoard Encoder_1(SLOT1);
# F8 U0 z6 {! w3 @5 | - MeEncoderOnBoard Encoder_2(SLOT2);& v3 [/ F/ ^9 {! E% K
- MeLightSensor lightsensor_1(12);
/ I' e. R% W& [5 T' ^0 H* g# S - MeLightSensor lightsensor_2(11);
6 d, Q; I0 |$ n0 {9 s - MeBuzzer buzzer;
$ f5 q% ~' N0 | - MeLineFollowerArray linefollower(PORT_6);
+ z# \- W; u) y( k: \7 p% I: L - 1 A2 K+ \1 W0 a* A2 q8 \
- #define Error1 12 G8 N- j) h/ X) j F# j% B
- #define Error2 2
7 H' I( m* y/ P$ c7 w+ O - #define Error3 3
8 ]0 @& n9 h3 R) A) D& r - 8 Z2 D! G. W3 I* w+ B1 {2 L
- #define Kp 15
9 q6 U# m# u, R5 k0 G - #define Ki 0.15
' Z) I2 d( x7 F' { - #define Kd 0.03
3 ~8 q: B' y( H5 f- t" l
T) z( Q0 r5 s( l- uint8_t sData;" _% q6 t2 x3 K0 s o- I
- uint8_t D1;
# N. f7 _% H, S5 |7 ] - uint8_t D2;1 L1 L1 G7 o5 U
- uint8_t D3;
* \" ^8 ]5 H1 m6 x - uint8_t D4;# J4 i! }7 T3 o2 z& b
- uint8_t D5;. j6 @3 Z j+ T- ^1 {( P3 U
- uint8_t D6;
! \/ ]5 C4 `; l/ g9 o! r
0 t! d: ]( a; \- float previous_error = 0;. J! B4 j# k7 W; j; l( R/ w, S" o
- float integral = 0;
* _9 U. e- j1 J( J - float derivative = 0;! v! C U+ ]. d7 t) ?
- int Speed = 160;* B; U4 M; @* T5 l8 e u
- float output;
) ?$ @# g0 M0 F) F - 8 F j/ m# o& m& v* v8 L
- byte Left;
1 ^3 A# b- Z4 H9 ~" b7 T+ c" s
# Q/ S% G0 v5 g/ T' l" j) A4 n& n- void setup() " O2 i+ S0 e. |3 c
- {
" Z/ S) [3 }3 g$ I5 {1 W$ h - //Set PWM 8KHz
) U4 p8 |) g$ i( m% @1 _: } - TCCR1A = _BV(WGM10);$ S) a7 K6 `* W3 j. _- ^
- TCCR1B = _BV(CS11) | _BV(WGM12);3 q1 b6 P- R2 v
- TCCR2A = _BV(WGM21) | _BV(WGM20);) m' m& T$ _) O1 i% d/ j: p
- TCCR2B = _BV(CS21);. Y6 L' H9 X' n" V$ m; `
- Serial.begin(9600);: Z3 s: f! N9 Z
- buzzer.setpin(45);) b9 j/ }6 v# Z5 u
- }2 x) n% n! g, K4 a0 y* ~' ~ ~
8 J5 d) D- N1 u( V$ u- void loop()" j/ T% \, J' K& E. Z/ J
- {: G7 g# w" h! N3 `. U
- while(!((lightsensor_1.read()) < (10)));
; }7 m7 {/ d0 a% a6 S6 V - buzzer.tone(1047, 500);
5 U% J5 l2 c2 j. f - delay(500);
. c$ Q1 z) R3 l - do
# E4 s; |2 m! Q0 S- U! I- N5 o* Q/ T! } - {/ U7 G& j9 L2 v# N1 N
- int Err = getErr();/ h) g- n" Z! Q
- if(D1 == 1)/ ?) {% K( S7 W' I" ^+ E( `1 Y
- {
7 X4 Q w u$ P/ M5 E5 \ - Left = 1;
9 i2 q8 _: a) O, [1 F! E - }
+ ^$ ^, @7 \" ], a: n - if(D6 == 1)
! m: B+ |- M" t) w$ |- \ - {6 p8 ?6 [& j. R* M3 e
- Left = 0;
( b4 _, |6 S, s% i - }0 ^/ Z5 D E& _8 L8 z: _% D/ y, b
- if(Err == 99)
# v# K" o3 y7 B0 c2 }7 C$ i - {' i- x% D: x! b
- if(Left == 1), {6 M& O8 D, m4 N. P
- {
- Y# s6 W9 z4 F- x. a9 Z - Speed -= 5;
: t1 @+ O8 b2 {; e0 `! M - moto(0,Speed);% q/ U Z$ }1 i3 W1 D: Y- O9 z
- do
) l: x, H9 Z& _, v2 Z+ E) C - {( O: p2 o+ |# |1 {$ e! M2 H C
- Err = getErr();7 o, g, b1 ?- C3 c
- }while((D1+D6) == 0);
7 H' X9 b% `% Z# F - }
0 e' K) w5 l- i - else
3 A% Q" L$ ?. E4 r: X5 H0 J* s2 R - {9 N& I8 ^. ^) I% _/ N
- Speed -= 5;
/ I5 k4 ?* v: y& J+ y" a - moto(Speed,0);
& |! W2 p8 ]5 d2 m. V- E - do- g' L! [% H& l, y- z
- {
2 A. w, |5 }) B0 n G7 r4 h - Err = getErr();8 E9 F: e0 S3 H& {+ h) m A$ j
- }while((D1+D6) == 0);
3 D" h6 D0 U5 U! M9 M - }
+ u- L$ d& a# O+ q - }/ l. s/ f( k; c: c. h
- else
: H, ]4 B/ [/ ^. M+ S - {
/ `- c5 r+ Z& A* Q: H - if((Speed < 160) & (Err < 2)) Speed+=1; ' [) O; ~, W" o& t [0 u9 D
- if((Speed > 100) & (Err > 2)) Speed-=2; 5 y9 E* `% S+ |9 l
- integral = integral + Err;, D6 ]( i# a% h8 R9 l
- derivative = Err - previous_error;1 T% |1 e, p: U7 I
- output = Kp*Err + Ki*integral + Kd*derivative;! c: f7 _ ]4 {. e# y% q
- moto(int(Speed-output),int(Speed+output));
$ c. f4 D6 |: p# f - previous_error = Err;
# v n2 j/ f, ?9 m# @/ d - }1 X- }0 z, \& P% k J
- }while(!((lightsensor_2.read()) < (10)));; c! e* G! H6 Q6 K* I9 a8 P6 N
- moto(0,0);
3 M. v. T9 q' Z" T - delay(500);) I0 p0 o0 h- R1 Z' w5 V
- buzzer.tone(262, 500);7 a$ i+ F: ~* j+ V+ X: t3 d) n
- }
: |: U! H' N( p9 W5 W - ) k7 M" j1 i, @- ^& R# R# B
- int getErr()4 J! p' M. E4 B% u4 j b
- { 8 T( x M! u/ z) O0 W+ [& ?% S
- sData = linefollower.getValue();( q0 L" T) E/ R7 @1 g1 `
- D1 = ~(sData>>0)&1;
1 q a9 x' x' r8 R/ j! i: I: [. T9 X - D2 = ~(sData>>1)&1;2 Q/ q" E- S; |5 _
- D3 = ~(sData>>2)&1;
" Q7 p. M$ a: ~4 R* u% d6 I - D4 = ~(sData>>3)&1;6 D: e2 X+ ?, F# M8 P8 F4 D
- D5 = ~(sData>>4)&1;: ?! `+ C! C, A7 X
- D6 = ~(sData>>5)&1;- l2 o& ]4 A: a; Z5 \- k, m
- int downD = D1+D2+D3+D4+D5+D6;
' R! o5 U9 ^/ q1 ?9 N( U - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);0 N' B) W6 V6 @. O
- if(downD == 0)) W. ]! ?' A8 J; e% G
- {: X% }( Z: F! |) q/ S1 c
- return 99;
/ O- [5 V' M8 P' I) U - }7 U( b; ^9 e4 y5 a7 Z
- else
$ f% ]+ A* G: W8 i! }2 L - {
2 t! t, C- p. m/ T5 W1 D9 V - return upD/downD;
1 J! o+ }0 m2 X' O4 }7 x - }2 `: z$ H0 T3 r! R0 v/ g- R0 S
- }# ]7 e' u9 j$ O: x+ X/ @# q" O
- * R4 o' u( f, Z: Q: m9 v
- void moto(int Speed_L,int Speed_R)
1 R0 ^9 ^& L( e* {4 N( N$ R - {1 \- Y, V, D. \2 v; V# o
- Encoder_2.setMotorPwm(Speed_L);
( r* u2 X/ ^( @ f - Encoder_1.setMotorPwm(-Speed_R);
3 T' c) W. Q* c, a+ o) ^8 f" A - }
複製代碼
+ Q( f2 y- j' m$ P; @MeLineFollowerArray.cpp4 h- ^( j( m8 E3 B
- #include "MeLineFollowerArray.h"
, C, H# x0 {. d+ K# i% n - * q5 g8 K$ v4 V; q( E& E7 M
- #ifdef ME_PORT_DEFINED
j; Z3 M0 E4 t# \1 X4 y& r0 c - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)* L( O _+ H5 c0 I) g+ y) R+ V
- {$ z# V$ A6 \" I
- + [2 Z$ A6 Z, j* {, H" k
- }
& h0 i2 b5 Z8 G% j8 U9 l - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
: E8 ~9 t; y- C+ k; Q - {
& g; ]% n. n! j Z& s - _DataPin = mePort[port].s2;
. `1 w; }( X1 H; W. \9 W! E - pinMode(_DataPin, OUTPUT); " _8 @0 D6 Z% V" b. O- l1 |% M2 `$ q1 s
- digitalWrite(_DataPin, HIGH);! g$ z1 E2 A! W. C' N0 q
- }
3 y: K* Q3 V, Z. ~* z) l$ j# ^" U - #else // ME_PORT_DEFINED2 Z9 ~* Z8 m$ u
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)% X# z, h9 B+ A4 I; B( C
- {1 Z- G* K; s& o" A( m9 G; g8 O, ^ N
- _DataPin = pin;
( {- T' H" \9 E$ } \ - pinMode(_DataPin, OUTPUT); 6 [5 p5 y4 N9 u, x
- digitalWrite(_DataPin, HIGH);
2 f; B0 p2 H' C5 n4 m& z9 t0 x - }
. Z# @( c8 r4 K) C; F - #endif // ME_PORT_DEFINED5 a; S$ M; ~7 a# X0 \* J& C5 K
- 2 |: g* ^3 L6 E0 C; ^8 f$ m# u( D
- 3 ^6 }9 w7 Q+ i! c/ W! Q
- void MeLineFollowerArray::setpin(uint8_t pin)# L+ f5 J* _4 D/ ]6 W; s" D8 l' ^. A
- {- Z* W- i! L% p- I- S3 `3 [4 C, X
- _DataPin = pin;9 E" I. V$ ], n7 K
- pinMode(_DataPin, OUTPUT);
8 m3 w( A6 X( g - digitalWrite(_DataPin, HIGH);3 S0 R/ \5 l! B. w. _- s1 R
+ W& q4 _& O8 B# y- #ifdef ME_PORT_DEFINED
/ S2 b9 u! N" b, e - s2 = pin;
7 Y- B% C$ j% [, K( K+ |$ b; ~' W - #endif
8 L' J$ B1 e( \) L - }
# o1 B" V8 v1 ^2 K! @ y. M; b2 B
0 _( N3 @' M) V" W$ q/ `9 E! F- uint8_t MeLineFollowerArray::getValue()! J/ V9 i- V1 e( S
- {& S$ X f, U% ?# y1 N
- uint32_t LOW_level_read_time;: y. c# J7 B8 v
- uint32_t HIGH_level_read_time;, _% `4 u9 o5 c- z. ~; |
- uint32_t time_out_flag;+ N8 N$ t4 F/ j% Z- O
- uint8_t Sensor_Data[3];
# _4 x& e; g) n- {' [ - static uint8_t old_data = 0xff;8 |9 W4 o3 H1 X' [$ k
- # [ a W* F- ~! W& I+ M- m6 J+ x
- pinMode(_DataPin, OUTPUT);
/ L6 E& U" [6 o$ V/ f+ W5 o - digitalWrite(_DataPin, LOW);
2 Y# g% w2 Z: i- A' b& p# X - delayMicroseconds(980);
& {3 b% Y" l9 E1 ?5 f4 m9 Y% R" U, a - digitalWrite(_DataPin, HIGH);4 @9 c$ J6 F8 J) ?6 s& k! f R; ^
- $ J) ~+ \( Q9 F8 W- a) | r
- pinMode(_DataPin, INPUT_PULLUP);( C4 Q; H% d" J$ _& \4 b9 B
- delayMicroseconds(10);. i7 w5 j( ?7 t; Z. o7 o
- 3 D' `( l' y) Z1 r4 \
- time_out_flag = millis();: p& f+ X5 ~0 z' F; g1 J
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );! m5 J7 d' j. v3 X$ E! X0 m
2 I/ h, e1 b3 R% n% ~3 \2 A) M- LOW_level_read_time = micros();
, H- d( A9 Z; l7 M - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 g1 M. V' w3 J! K' R Z: w - {
1 r5 O$ q8 m: m1 B" c$ S& p! f - return 0xff;
, P( r$ L$ M2 M% } - }
( }. [: P& X& @. K- I
# ]) Q( m; e1 }: V$ j5 x+ N' y& R" I- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 v t" ~0 l) }6 v( Q7 c$ d3 M - ( }' E: v7 j: \
- HIGH_level_read_time = micros();% G* h: M4 w% l9 {
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level8 l' z/ C, {+ h. t, O
8 }* X7 M6 `) a- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out+ j5 g$ O4 B* J8 P5 l7 U
- {" p8 }# R( k* f
- return 0xff;
O1 U* [5 R' r1 b+ ~% t3 C - }& o A' l- W" C W1 i" S0 L; k! S
7 g% m1 m) z x8 n3 g- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))$ U3 u/ s X1 H% u# ?) U+ k
- {3 R, | W3 ^; y2 O: f0 t3 u0 q
- return 0xff;$ ?8 U& l8 ~- M9 u5 O8 t
- }- ^- M7 R8 R+ q8 ]$ Y
- 9 T& \* ]5 Z' G; M8 e
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
) x, h" K- v( y: A - LOW_level_read_time = micros();" C% e+ ?3 |' q+ E# |! B
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level/ O& l$ e e6 M6 A- V! ^
- . g$ i: I2 G( t7 a) F6 v7 V
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 c7 M1 c3 h' y# i4 G5 _4 o9 \
- {
- J6 Z. w" E- `+ T, _ - return 0xff;% L) B6 y* p* h3 _
- }
% e$ A' ^ w6 D/ \6 ?, H - , i1 k% H# u# Q
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))" s, v5 }+ u) m) P2 J6 J" d
- {
% j% q& L% c/ q- u - return 0xff;
% {/ p0 B" T7 {. t) w+ \+ | - }9 H& }1 \* b6 `& k8 H* ?
3 C! ?2 ?8 j& Q" A# o- for(uint8_t k=0; k<3; k++). h$ f9 a$ B' D" k* {0 K
- {7 b6 B( `& \' y7 _2 t E* w2 _
- Sensor_Data[k] = 0x00;
% j; u+ k w9 l# x
?: p. C, k) {: h+ Q1 f- for(uint8_t i=0;i<8;i++)
h3 O3 v& h# d, P1 s: ] - {/ \! i" P8 P' O' m' L' @- b! v+ X
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
7 h: T. y) T( A) ^' h: |4 L1 x- | - HIGH_level_read_time = micros();4 w! R9 m9 A6 q/ ^5 X0 p
- LOW_level_read_time = micros() - LOW_level_read_time;6 F# b& L5 Z/ J$ j% M- L
# j; z8 [2 ~$ R# _: P& K" i- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )6 k5 ?0 p* S! |+ t
- {" ?7 N+ s2 p1 S
- return 0xff;: N$ A( J: b' P
- }. c& u7 T' H* u( J v& P1 h W! E
- 8 h1 I( p6 o' X) o5 x9 P
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" _5 G! u7 L2 k; r6 v4 m5 x6 J$ r; q0 R
- LOW_level_read_time = micros();! @: K7 d, |: R& L0 N7 _, A8 `
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level$ c' l0 y" o% q. x" K" v0 G
) Q! I; {! G2 Q0 @- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1' R* [4 W3 @9 [+ Y( ?* R, W2 G' n
- {- h7 g9 |; T7 F5 N- ^
- Sensor_Data[k] |= (0x80 >> i);3 R4 A: ]+ C0 f) u- h' K* g
- }
4 K/ A+ E; U; w5 E - else if(HIGH_level_read_time >= 100)
$ [- q0 D$ H9 \ e - {
c* `/ g) o q- j' s' M - return 0xff;7 [# H) L; M* E: l- G5 M$ `& ^' P
- }; _3 f1 ~# e! L( L
- 0 k/ }3 ?5 y& x8 d4 t% T) X% @4 u
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)/ u! Q& [4 \4 L7 \" ~
- {
! b4 @, _" B- C _6 {" E5 U - return 0xff;
: ~9 z, g6 }9 X* S: F* @$ ^0 i - }1 L8 q9 \! N7 ~! u3 l
- }) h$ k3 @" |7 f; @1 ]8 O( K
- }
U |+ W: Z1 g2 E - & W* h: C2 R4 G8 O' C z0 N
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
, @8 P& t/ w [6 i5 I) c5 B - HIGH_level_read_time = micros();
0 p9 g. Y: G& q2 s0 \ - LOW_level_read_time = micros() - LOW_level_read_time; s* _5 y) i+ B2 N s- J5 K6 P% A# P& n
- W4 L' X+ f6 U* D& Q. [+ j! M; y- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )6 ]) y8 V5 J& X2 k% J, D% @" h
- { c* k; E' g) G @1 t2 n# N
- return 0xff;8 i/ \: o q0 s5 O. M, ?, y
- }* C) F* B: @3 ?7 Y5 L$ W. i
4 j. F4 c' O0 G6 r4 F- pinMode(_DataPin, OUTPUT);
( q( g/ N) m# V& n - digitalWrite(_DataPin, HIGH);
& d& m3 w9 s$ n( ^
6 Y# y0 t- J' D3 o- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
# K" v0 O0 M* v, S - {2 A' \! U( t, u
- old_data = Sensor_Data[0];
# U, C, ?& ?" A! q - return Sensor_Data[0];
1 e6 H7 }3 u4 R: Z! F3 N. \' j - }! i" m9 j) p( v# O2 @
- else
6 Q; F" F% h( Z0 W - {- y, A8 J2 o* B P# l/ m5 G2 V
- return old_data;
- i8 [5 K o, T) j4 g6 ` - }
, ^ H* t" x+ t7 }9 I7 O8 r - }
0 _; r& }) C) H4 [$ a9 t4 F1 z
複製代碼
. W" [# J& o% p8 i! B+ w% ]( W' pMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
4 X& M8 s; v8 b
. S/ u7 m% Q! ~6 o: ^9 Z4 t; F
|
|