|
( @. l( }/ J9 O
- #include <Arduino.h>/ \/ }% f1 |- L
- #include <MeAuriga.h>' x. Z) E+ F1 I; U c
- #include "MeLineFollowerArray.h"
! Z6 O) o& h$ o& m, x3 D/ B
- J% N2 \/ `4 N1 U9 o0 o- MeEncoderOnBoard Encoder_1(SLOT1);
! a5 }- c) }; w9 ] - MeEncoderOnBoard Encoder_2(SLOT2);# v l2 X% P. K! d2 k! _2 J
- MeLightSensor lightsensor_1(12);
8 ] ]) f# d/ J& J1 C7 T, e" s# ` - MeLightSensor lightsensor_2(11);+ \, g5 y( v: U
- MeBuzzer buzzer;
" G3 g& V& M! n6 E" C7 E$ d* L" ? - MeLineFollowerArray linefollower(PORT_6);
: D6 l/ z# j# r0 Y) u1 Y
6 H) U x5 D* r* U; q- #define Error1 1% h" L9 A- W0 n7 c0 _
- #define Error2 2. n9 B, b* ^4 r% D u
- #define Error3 3+ r, E' I7 U) U& o- J9 b$ _
2 {9 r9 R( l+ y- #define Kp 15
. @* D$ l- O" T, O& N5 f8 g3 v/ w - #define Ki 0.15
- Q; M ~' Z; s; V8 l" S7 C - #define Kd 0.03
) c' V( f/ @; `7 O
) x" l$ d( O3 f6 f- uint8_t sData;& f0 X2 e. V, V) g
- uint8_t D1;2 Y- Y9 R* _% t' W% J9 r5 [! l
- uint8_t D2;
0 j* ` p) w3 N1 R( R& M - uint8_t D3;
! B2 O7 A* k+ t0 V) { p/ a! g - uint8_t D4;
9 R! m: E9 g+ n# A - uint8_t D5;
% U, [( y8 i& k$ G5 u% p - uint8_t D6;: ]! |2 V5 P5 I) o" V. T/ q
- ( y7 T( }& j) A% ?
- float previous_error = 0;+ i$ e* @0 n* B( y. v$ i" x
- float integral = 0;+ [& q- n0 Q' l8 r6 k9 y8 {5 m/ {
- float derivative = 0;
' O* f8 w8 P: R/ m7 N - int Speed = 160;
. Y7 {7 z- v1 l8 ]$ Q - float output;
- \2 n2 A- s7 o+ @# b- O
- D+ r) j7 O$ k! F4 h5 b- byte Left;
4 ^" x9 `2 }8 h0 L0 U6 o I
' x% V9 T) _3 b, x1 X- void setup()
" v9 N4 T3 W) L5 P' W& F) a$ u - {: K2 Q, q) \& |5 z/ [2 c; i
- //Set PWM 8KHz
4 I; G: l+ O$ R. z - TCCR1A = _BV(WGM10);1 K0 o5 R z! j
- TCCR1B = _BV(CS11) | _BV(WGM12);. T5 c8 z2 _) C1 n9 u5 N B
- TCCR2A = _BV(WGM21) | _BV(WGM20);" i5 U% D( S w' I
- TCCR2B = _BV(CS21);1 h5 i6 v6 Q. O: w( H. h. F
- Serial.begin(9600);
; V: \) k# D% {+ K8 K7 V - buzzer.setpin(45);( x; O* z. t5 c( y0 g% G' p
- }
+ R# P$ A8 z' G& a% F
8 O4 Z2 [& r2 P7 L, C- void loop()
# S0 [" o' _' U/ c% O - {' M$ f/ W9 S$ q8 I
- while(!((lightsensor_1.read()) < (10)));
4 F( K7 j* B' ] - buzzer.tone(1047, 500);
1 o1 f) {" n% g8 M; l# V - delay(500);
" l5 E2 f" n- C E - do: V3 s+ P' O0 c9 n% u' i+ O( w
- {
4 x% ?, F1 J; d3 U6 j - int Err = getErr();
1 j, S0 h, C+ c - if(D1 == 1)
' i0 \. o6 [& @2 x - {
* Q }/ y- R. t- p, h2 H6 p - Left = 1;
B0 h7 g1 h/ g, h - }
" M$ P4 U) @7 L7 P. i/ X - if(D6 == 1)4 J/ l+ l+ o: i0 X
- {$ Y$ v v4 E$ Y5 z+ P; D
- Left = 0;
$ v b9 V; t; p" O: z - }: |0 Y9 X; b$ T4 J- Q
- if(Err == 99)
# R; b. v. `" b! e - {
8 R8 J! o& o+ z1 N% l) o) n - if(Left == 1)- w7 [3 j+ S7 [0 i4 g1 X# Z
- {
5 d1 r8 ^* z3 i - Speed -= 5;3 J, v$ y; {: I! u/ E* H) [
- moto(0,Speed);
1 m# h+ @; T7 S& N - do
a% a) G3 C2 h6 Q! O$ J0 F - {
4 Z3 v; K( ~5 A8 D7 U4 D - Err = getErr();- ?/ ^, u4 N$ B# J2 m
- }while((D1+D6) == 0);( |# T" k& M* k
- }+ h2 A# ?- U0 \' ~9 q! w, v; I
- else
/ `" N& l0 Q$ ?3 Y0 t% { - {
3 H5 [2 c& @1 Q1 {, F7 x - Speed -= 5;
+ h, h a0 e# p! l - moto(Speed,0); 9 P, X, s8 ?# w
- do
9 h' n9 ~0 }' m/ n$ O9 K& B5 f" s - {' u& \7 D! V+ n0 c. ?
- Err = getErr();2 R' W* \2 y( R9 a
- }while((D1+D6) == 0);
! [4 f; ?! B: F: h, P8 H2 i - }
X) s+ w0 `; G/ Q, k2 }" h - }
7 y) X6 k8 i/ z* w5 w - else
6 u- u* ]- L1 d2 V+ a' J - {
: L0 g% X, C6 r! I. N* D - if((Speed < 160) & (Err < 2)) Speed+=1;
6 z) n" `6 P l* \$ s% f - if((Speed > 100) & (Err > 2)) Speed-=2;
, R$ B N0 ?1 c! ]* \* \+ S - integral = integral + Err;( i# k; [8 O0 z$ a
- derivative = Err - previous_error;
0 o5 \( @8 C& Y/ n$ O) N) w0 x - output = Kp*Err + Ki*integral + Kd*derivative;1 m; m# o. T8 ?: u+ \: |3 r7 v
- moto(int(Speed-output),int(Speed+output));* e# ^, n% w/ a) [/ K% f/ O
- previous_error = Err;) L2 I$ n- B; N# n: }
- }6 A- {) j9 E9 C* d0 ?; r
- }while(!((lightsensor_2.read()) < (10)));
* ]# N$ ^7 S, X' O - moto(0,0);
3 R2 d" p/ [8 x6 ~ - delay(500);6 W! r5 ?6 w- A& z7 h# J
- buzzer.tone(262, 500);9 Y$ [# P# q; Z, a0 z
- }
( ^. f V1 |( l% X* A. o* B2 z - 5 P. J3 M* J+ i* R6 K& H
- int getErr()9 \* C# b' Z: F, H- x
- {
- m3 o W: _1 c$ `' A/ r5 A - sData = linefollower.getValue();
. V, U5 N% D a4 k Q - D1 = ~(sData>>0)&1;+ m6 ]; c+ M" i% `+ L
- D2 = ~(sData>>1)&1;
9 I% y( q2 h0 M4 R9 m. T, O - D3 = ~(sData>>2)&1;. ~! [6 n1 G/ w1 q/ ~+ s( t
- D4 = ~(sData>>3)&1;* v; p6 e: w( y& c
- D5 = ~(sData>>4)&1;6 F$ A" Y: Y- f. F
- D6 = ~(sData>>5)&1;
# X9 d5 N* K4 C9 L - int downD = D1+D2+D3+D4+D5+D6;) e( l4 W' c+ t! J* U
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);5 u; Z. [, n! U* n. ~
- if(downD == 0)! \. F/ \ w* n/ }
- {! v/ y3 J- [7 |6 H& ?3 }+ f
- return 99;& d: A2 {$ i) e+ O+ g! |
- }
" a# ]5 V' R% o - else+ {2 z. s; v0 e, F* N& a8 x2 c
- {
' s0 X( G7 y$ u. b* Z' F: U - return upD/downD;
" b+ |, G6 L3 H- E. m - }
; g: ~* P3 w. a" H4 l2 c - }$ `' p9 W' q* P9 b4 n& p" G0 d% ~
- 1 I; h, U# f/ }
- void moto(int Speed_L,int Speed_R)4 _+ ^3 {% G i8 C: H0 S
- {
8 W% A( Q, c8 w* e1 S0 T6 y" I - Encoder_2.setMotorPwm(Speed_L);- z* C; l1 `( M; O" V) _
- Encoder_1.setMotorPwm(-Speed_R);2 }- w) v% o3 @1 J+ Y2 k4 G
- }
複製代碼
$ }# }$ h% j. W2 VMeLineFollowerArray.cpp
' w% `( |, K9 T2 l% {% l8 X- #include "MeLineFollowerArray.h"
% K$ u) L0 C p% t- C _$ ?9 ~& ` - ; s% L b$ K: V8 E1 F
- #ifdef ME_PORT_DEFINED
7 ?3 A6 W* Z0 Y& w - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0), ^% Y; e3 s# e
- {
' H! }) n; m c7 P U - 4 ^- r. F$ \) S
- }
. o3 q" u% w; ?& W - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
; i q3 [- l5 c# D" A3 x1 P - {6 y- d+ ?4 D5 \9 U
- _DataPin = mePort[port].s2;
) N! X! Q8 Y. ^" r; p - pinMode(_DataPin, OUTPUT);
J$ p; a, Q# P4 Z - digitalWrite(_DataPin, HIGH);( V0 W% W- I4 B
- }
; N) z) f* f! V$ k% @% p - #else // ME_PORT_DEFINED
5 `* H( y5 `3 @9 ]# p7 C - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
* I( @: V/ J5 |. x1 n, a - {
9 b, a$ ^, }+ H7 H# E" B0 F - _DataPin = pin;7 N- e1 s8 Z* Z# P& |% j; L! v
- pinMode(_DataPin, OUTPUT);
$ E3 e7 p6 Y2 c3 U5 u- ] - digitalWrite(_DataPin, HIGH);! `! \0 z+ j0 H' r; k1 ?
- }7 i8 T9 G% E6 L( n0 U
- #endif // ME_PORT_DEFINED
K. U/ e/ { j0 f H - ! O; i$ h" e7 v4 F3 D$ c {5 ~- E
. O" l( M$ w8 v& y! |- void MeLineFollowerArray::setpin(uint8_t pin)3 }7 w2 s' v# g6 i2 G" T
- {
( y. n! |5 j, q& h& Y - _DataPin = pin;% C, s6 b! N: Y1 t7 D/ m' r
- pinMode(_DataPin, OUTPUT);
& z3 `9 D d5 ] - digitalWrite(_DataPin, HIGH);
. ]' [1 K7 p6 s5 z - & l% L; w/ `/ I6 Z O# Y
- #ifdef ME_PORT_DEFINED6 s. F* Q1 B3 F6 | z p* P. e
- s2 = pin;8 E( ?' A& W; l2 S7 V
- #endif' T( N8 E( {/ j% W) W7 D
- }8 |* b* {: p& a
2 K+ k/ g6 B8 x- ?- [- uint8_t MeLineFollowerArray::getValue()6 z8 B3 q; _3 l) m6 }
- {; b, g+ P- u8 I* ]. e
- uint32_t LOW_level_read_time;
8 h. d( K: G" m2 y9 @ - uint32_t HIGH_level_read_time;' L1 j- t: x8 w3 h* D
- uint32_t time_out_flag;& P- n0 E: ]/ y) z _' Z6 n- b* G
- uint8_t Sensor_Data[3];" ?& ~% x- U" m; C# c% Z
- static uint8_t old_data = 0xff;& Y# u# ^8 k+ m4 v* }' J
- $ o, r; K4 `" n
- pinMode(_DataPin, OUTPUT);
9 s& k$ k- }5 N2 c. M& V7 _ - digitalWrite(_DataPin, LOW);
! } y+ N; y G5 M - delayMicroseconds(980);
* ~. f: i. K; F- [" X - digitalWrite(_DataPin, HIGH);* |+ i- F. C9 Y3 V- s- E
6 Y* N' {; g* }8 c- pinMode(_DataPin, INPUT_PULLUP);
7 S' Y) v/ z* Y9 w - delayMicroseconds(10);
" G& f( q& H$ z3 \) ~: X5 K. ] - 0 N4 e; F9 e/ L
- time_out_flag = millis();" c% }/ |8 b. S
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 g. }9 E' r3 e* Z; ^8 k, B0 ] - ( q- F6 |: D) d e8 }9 n& {" z4 j7 h
- LOW_level_read_time = micros();+ c: B9 E% `9 n" j/ B k8 {) s) ~
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 Q; P; _9 u5 n, |' Z) [* b
- {
, V2 J/ ]3 B0 e. w - return 0xff;1 v* R2 m8 f, V. J
- }
8 g5 J6 i# T0 F# \( f
# V% U d! M# `- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );3 c* N* f# e0 ^! w1 q
- - H' {1 K& v+ d! O2 s0 x- X
- HIGH_level_read_time = micros();
( f' f7 e6 {' W& L u! q! W9 q! A - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level# s- \1 _: [+ B; ~
: L! h+ G! S5 d2 A- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
K0 {- v+ p4 x6 z$ I - {
% }/ p t0 L" q4 N - return 0xff;
; R, |" _3 {1 U6 C$ N2 N! W# z" S - }. D% h' Q+ @! s) n7 Y' K/ N5 U
- * `, ~ v$ P1 }" B* Q
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
$ |3 r1 H) N" H7 ]( M+ B - {; s( }- m8 C. `9 s" N
- return 0xff;! F" w1 t/ O! h! @# N; N2 | n! V
- }
2 F8 c( z/ x+ }8 Y - 2 G: ^) t9 h5 f. F) A& C
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ [+ K$ n- q- J( r! b) r - LOW_level_read_time = micros();
. |; Z+ j6 g: H& I - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level; J# a0 z! E# I: u; r
1 @; |, ^7 D+ M/ i3 W- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
- H1 V7 T; t& f6 H/ d - {5 {% S7 `1 G. o$ _7 H
- return 0xff;( d/ t- X9 u/ y) Y& _9 {- b. U
- }; M r" p/ E3 ~& ^" K* d
' B2 h- K6 v: d. |5 e- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
, ^( r5 U0 F: K# w; W* j+ ^ - {$ C2 _6 H8 o G2 B$ M1 o% F0 H
- return 0xff;6 o/ O& O3 u5 P V4 i0 B
- }
; ?. V O! u) A% _3 l, k& r- X
4 J# [ L- G- V/ G+ G1 L6 x7 t- for(uint8_t k=0; k<3; k++)& i; H r/ l- V [6 N
- {
d* |* r, B r, c0 h& K' K) e - Sensor_Data[k] = 0x00;$ `1 O; H0 ?9 F7 g8 R1 j/ g
' M7 j1 U( S$ {4 v* g6 v* ^" \2 I- for(uint8_t i=0;i<8;i++)1 t0 d2 K8 L0 ], h, Q% k
- {
% ?2 O4 N2 B/ i: K7 ^ - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level& M# g8 R0 I1 z, k6 u! ~9 a
- HIGH_level_read_time = micros();
0 h6 n0 N$ T9 P9 q- m9 G& k! G - LOW_level_read_time = micros() - LOW_level_read_time;1 R6 ]# b3 h7 r/ j# N/ @1 N1 B( c
# V# t, `/ A) \1 s# t, f' m- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
8 H8 R# ~1 S$ A; Y* R5 [& P - {
7 p4 ?, C* k! E+ T - return 0xff;
8 X/ ~9 A; f9 ]; f7 }8 E - }
& y/ a. G8 J; _& P [ - : a- Z7 B' M3 y6 q
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% u* x% \, I3 v7 q" m9 K - LOW_level_read_time = micros();! f' v# A# j" ?4 ~$ W6 p. j4 c' L
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level" u+ {# f) y6 V$ S8 G& m' T
5 E3 X0 p# p, I7 u- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 12 l" k. q$ R( q! C% B( u# ]1 \
- {# T: z7 G, t5 p& H! \; e# m0 u
- Sensor_Data[k] |= (0x80 >> i);
+ O, H$ `# l3 w! D- |8 Z+ y - }3 V! D# e2 j, z" _7 k
- else if(HIGH_level_read_time >= 100)
" j/ A" Q9 X+ V/ X l8 f( c - {
) f3 M7 x, [6 {; q9 R& {2 n - return 0xff;, v5 D# R' A. l" F
- }4 ?1 f) h, D) @* g* R3 U$ H9 h' |% {
: V7 m# m$ [5 S2 S- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
2 [! H0 l3 P* I& X; `' [7 M - {* S& }1 c7 @/ P( W* v
- return 0xff;
* ^3 V- R3 @4 U; W - }
; w5 {9 |, a3 G ^ - }
9 j* E; i" K) x, @ - }9 H; {. z: s) P8 ?8 a' d4 ~
3 s. i$ f. `4 l; g, X' F- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level$ c; M- M T) S; _5 p
- HIGH_level_read_time = micros();
1 ^$ x0 _( y, ?( ` A; Q - LOW_level_read_time = micros() - LOW_level_read_time;# ~3 m2 m) K5 D1 v5 U
- ; B) L5 W4 c& }5 n8 [5 K* T$ Y7 T
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
3 d) Q* m8 e4 O2 [ {2 P+ X - {2 U* f8 }( Q6 U
- return 0xff;
/ v. L4 Q0 K$ I6 H% M B% O$ u7 G4 R3 H U - }
6 N- \* t1 N+ P0 N- z& Y- D) j
3 K+ j* G+ X/ `9 V1 o# p5 X5 r# C- pinMode(_DataPin, OUTPUT);0 ]2 q( ~9 s3 P- \
- digitalWrite(_DataPin, HIGH);
4 m8 @4 I+ c( U6 k0 J0 A - ' ~+ v+ A5 [( X% Y% k( ?2 \
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))4 ^2 p9 _) f3 s8 ~ s# X
- {
$ [ y/ r& k9 w* Z3 ^: U! u - old_data = Sensor_Data[0];
) h1 f1 |2 ]& V) W - return Sensor_Data[0];
( U/ \* ~9 N/ D p. H - }% P' {& M0 L& \4 N" L" a
- else5 C& p+ s- B& L
- {$ O$ L6 m' m0 f! q0 h& l
- return old_data;# e! K7 F: M5 O( g8 r* f
- }
3 V" X3 s* u- f- _. Z, u8 F - }
% x+ p3 I/ R3 n% U7 T% e4 K3 E
複製代碼
, _2 R. |9 D! RMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
7 N, k, g" B! W- ]' c. B& V. m
. h/ K8 A* F- A; M: d% @! ~ |
|