|
|
% o! T! v9 Q, }( {9 J
- #include <Arduino.h>- Y5 |0 n3 D d* Z
- #include <MeAuriga.h>
r" V; C" {8 i: i - #include "MeLineFollowerArray.h". z* V) X6 f! J0 L# K
! j& p( U: [" @: Q% t. L8 z0 y- MeEncoderOnBoard Encoder_1(SLOT1);! E. O6 B6 ]% Z8 Z+ \+ [; L
- MeEncoderOnBoard Encoder_2(SLOT2);9 Q9 O. v. D( v* Q" V. o
- MeLightSensor lightsensor_1(12);
/ d1 ~+ O9 T* m6 U/ y; s - MeLightSensor lightsensor_2(11);
4 G) m9 @+ N7 e2 _& w8 V - MeBuzzer buzzer;
?7 R8 q+ g( R; H9 B: g - MeLineFollowerArray linefollower(PORT_6);
0 B" S) r$ B# }9 X% @3 c9 g+ v
5 r" t5 m* q: w# c& I- #define Error1 1
1 \2 c2 B7 y; K% ]; n7 D - #define Error2 2# S# [# ?4 R+ p# Q
- #define Error3 3
! V% V( r. V: ~6 w6 R$ r
0 I1 n/ B$ E4 x5 S$ ?- #define Kp 15
2 d9 t2 y7 ?' t2 N! A1 L - #define Ki 0.15
" G; k/ o' s5 `* F; t7 [, A! r - #define Kd 0.03
$ c1 C- M/ c3 f! j
; f6 X9 K4 A7 L/ M! V/ [6 S- uint8_t sData;
1 q4 }5 ~% P; M7 L8 Q: o - uint8_t D1;
% k: b; k+ R9 ` - uint8_t D2;
; m* h& O+ o: j' U6 Y/ B! w - uint8_t D3;9 T- f# `+ A/ i) O
- uint8_t D4;
, ~. T- `% Z' V* a - uint8_t D5;
' R; C- q4 w2 \9 X, m& j, w8 G& d - uint8_t D6;
5 L; g8 A4 q( d0 t* X
. M" A. h7 ^( b- float previous_error = 0;6 F9 W5 \6 F; t; v: c) Q
- float integral = 0;
& r/ C9 \8 A) q- r6 B1 C- E2 I# A - float derivative = 0;
( m8 E5 a s/ e2 ] - int Speed = 160;
7 G/ q8 `3 Q% _5 |6 ? - float output;3 }% E8 k3 P5 W9 @2 b
/ g# A, G: a; n- r- byte Left;
( J6 Y5 T d5 U$ l$ X' H
1 O+ Y# _" N. e- void setup()
0 S: C2 e* [% a6 F - {5 D7 k5 m- g# n& P
- //Set PWM 8KHz# h& b, [9 j: ^' C3 P6 R
- TCCR1A = _BV(WGM10); J- x% ~! z1 J! k* V+ M
- TCCR1B = _BV(CS11) | _BV(WGM12);" k& l' W! o" c/ ]+ w. v* q
- TCCR2A = _BV(WGM21) | _BV(WGM20);7 W. g9 ]1 v2 p8 j
- TCCR2B = _BV(CS21);
9 h3 ?7 o' q3 r* L! j# B l- D - Serial.begin(9600);
2 r6 O: R; U# }& J6 m - buzzer.setpin(45);# t( T, x Q; O
- }
$ F; K- E4 l& y6 x; A+ Z( o4 \
) H. f* c6 N* ~$ r, x- void loop()% j& y6 P; p! d4 `9 e, H
- {
/ k: P% z/ `$ K - while(!((lightsensor_1.read()) < (10)));3 H) m( }, K" |% w0 L5 U3 W
- buzzer.tone(1047, 500);; l4 P8 P/ g) e2 Z/ F6 U1 |. ~ H
- delay(500);, H' I3 v) A% P) {+ I
- do
' S, }8 E9 b5 f5 b( I3 J+ h - {9 _1 T7 u' g6 g$ K0 W
- int Err = getErr();& |( P0 H, o2 [: V2 B
- if(D1 == 1)
8 h* A" W ^' @, E# H; @0 K - {4 `2 J# S+ d0 E! K( g
- Left = 1;
7 v# }, q! Z% k" a - }
) f3 o4 @5 y5 ]. r. W8 V1 j) u - if(D6 == 1)5 W: c1 W5 Z9 o' X: ?
- {
9 p! u$ @, _8 f& F. Y( d! g; X9 ]+ S - Left = 0;! |2 n, P! ]6 `4 O w1 J
- }5 d! o+ U1 n7 Q& _' r9 M9 {
- if(Err == 99)
0 c. R7 @+ J6 ]" D% t - {1 a- t* ?; K- U, C$ T$ W
- if(Left == 1)* ?) O; A4 A$ S6 z+ p
- {
5 o. x _5 F: ^' [7 ?5 N - Speed -= 5;
8 d4 x- i8 n$ K( i* C# @ - moto(0,Speed);
6 S; h! Y& _+ z) J' |& f - do l/ B4 |6 i" n2 ?
- {8 O( C: v# T" Q$ \
- Err = getErr();
3 @$ R+ s3 L2 T# K' k2 c: ]; p' @ - }while((D1+D6) == 0);
E0 ^1 Z3 a% z) G9 {6 P9 v" ~ - }, U7 b7 u. t0 i: q
- else1 [& M. p: r5 r! B- a" m2 h
- {
* @- j5 |/ f0 l; u" D O - Speed -= 5;
5 s5 c6 G& }, t1 ~8 x& v+ ?( I: Q - moto(Speed,0);
( {0 V P: \# S. W1 m8 X+ Z! p - do! ]- K7 W* }' r
- {
5 a, m/ i* o) s - Err = getErr();7 F$ A4 G: X! H1 G+ F
- }while((D1+D6) == 0);
5 n9 X# l$ W% W h - }
+ K, @% H+ h& ~# i( N' x - }3 g, R3 x6 o5 l; a9 V
- else
+ P3 P, y5 m( M) _8 s - {
# |4 Q6 z- c: |. J3 w - if((Speed < 160) & (Err < 2)) Speed+=1; ( j" B% ?9 F8 B% A6 ^
- if((Speed > 100) & (Err > 2)) Speed-=2; 4 }1 F+ G0 u" w( D
- integral = integral + Err;
; `4 l' x! c9 E - derivative = Err - previous_error;
; `% g: b+ e/ s. O+ `2 ] - output = Kp*Err + Ki*integral + Kd*derivative;3 ]( Y1 s- V) v' U
- moto(int(Speed-output),int(Speed+output));& \3 h8 j5 c4 m9 [! h# ~
- previous_error = Err;
" {4 O7 M- y, D: I( H8 M: u6 S - }
2 z# A1 O6 A: F4 D# ] - }while(!((lightsensor_2.read()) < (10)));5 e T! @" W& l& @6 @9 p
- moto(0,0);
% H1 C$ t1 o7 ^( h1 _1 b# y0 c - delay(500);
7 s9 F8 l3 J6 V9 ?( Z - buzzer.tone(262, 500);
$ Z0 k4 N7 Z% A* ^' h/ c y - }
3 n% c2 I4 \1 J) F$ v7 o
: w3 k6 q5 m9 E$ G- int getErr()
, n$ R) E P1 i1 N4 S, _2 w - { ' y, A+ r2 ]( s y1 W
- sData = linefollower.getValue();
3 G( |' w1 f, @! k - D1 = ~(sData>>0)&1; J. \1 ` U- f% C+ K
- D2 = ~(sData>>1)&1;
+ t/ y. j4 I* Y" I# t - D3 = ~(sData>>2)&1;9 `5 X' x7 Z% t
- D4 = ~(sData>>3)&1;
' I% Z. T1 Y4 p, Q( Z: I5 F* Z5 ?, ~ - D5 = ~(sData>>4)&1;
, a4 \6 l1 i5 C) A4 i - D6 = ~(sData>>5)&1;
$ R- A1 r) c9 I - int downD = D1+D2+D3+D4+D5+D6;( m. [' h/ R2 c9 A3 W' |* C
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
0 |% l) }6 l7 x: w. ]2 r - if(downD == 0)
& S* i* V, h# I& ` - {
) [7 Q4 [2 z! u: W; {4 U/ t - return 99;' Y) Z3 G: Z9 t0 D# g, x5 p2 U4 x
- }
/ g2 w( @! J+ l# F3 g# h3 Y - else
2 L1 Z O( G k% a" Q( t - {8 I" q' C! o; v' a$ m% T5 X
- return upD/downD;7 ~" U- y. s5 Z! q, E( @
- }
, W5 {0 i/ S7 H1 |! \7 t9 |" ~, H - }$ }( K) n4 v6 n" b# E) ] t! G: Z
; w# f3 X, N. ?6 P* h( B% b- void moto(int Speed_L,int Speed_R)1 |: q& e+ { f) p7 b" N. h. Q6 y6 h5 l
- {
1 i! X* ^' `) x, I1 { - Encoder_2.setMotorPwm(Speed_L);% M7 T3 A6 B) j; X
- Encoder_1.setMotorPwm(-Speed_R);
6 r, z* I) u/ h5 z5 v1 { j - }
複製代碼 # J0 P v, W+ d# W9 f& m0 s
MeLineFollowerArray.cpp, ^8 G% G6 J: I* Y( K1 M
- #include "MeLineFollowerArray.h"
2 l+ f I( L3 W - 1 K* e: R8 H$ Q2 N
- #ifdef ME_PORT_DEFINED
$ B$ T* I: P8 {5 Y2 V2 d0 a! ` - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)5 A8 [8 Y( N# c; U
- {
4 ]9 C7 q0 H3 j6 I
4 w4 x( d" {& Z' J8 t' o$ V- }* |' P0 E( G% `" o& ]5 \8 b ~
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)- u2 O: H0 ~, V8 k: o4 ~
- {
9 F# }3 g6 r% o c - _DataPin = mePort[port].s2;# y' p& P7 h! R0 [' N: q! n
- pinMode(_DataPin, OUTPUT); 7 B) Q9 j9 O( z& |6 K' a! e
- digitalWrite(_DataPin, HIGH);
9 F4 k1 E3 H8 p9 z7 u - }& M- b& C7 a. i* y* `
- #else // ME_PORT_DEFINED
; f) i3 m6 ~6 T( c+ ~" | - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)* I/ M: B5 e! `! G# n s. z
- {
8 _+ [, b0 |0 p3 D2 M2 |* P - _DataPin = pin;
- m( t; _3 ?4 `1 O( C1 d - pinMode(_DataPin, OUTPUT);
4 ^' p2 q& c9 |9 { - digitalWrite(_DataPin, HIGH);) L% Q, D7 o' ]. c6 p7 x s! c
- }
3 F7 i$ [! s' J5 U( a. V) l - #endif // ME_PORT_DEFINED% J% V, ~1 a5 T' \
- 5 j3 F$ Z) B- d5 b9 Q+ }9 @" P
, B& U) k, P) O- void MeLineFollowerArray::setpin(uint8_t pin)
8 T) {+ N- m1 p q- j - {- v2 c( A, F8 A/ Z
- _DataPin = pin;
+ c; t* y: h" ^9 U) h. o7 q - pinMode(_DataPin, OUTPUT);
0 Y, F0 J) c; C7 W/ e, z; Z - digitalWrite(_DataPin, HIGH);, R" A1 W6 b a. ^- l- c! I
" t' U, `: {* d- #ifdef ME_PORT_DEFINED
2 O5 l9 |% {2 y, N. h - s2 = pin; v( ]2 v$ @. F; o9 F2 {, j
- #endif
/ M& u& w& ~+ O# J6 ~ - }
* I. C% q, I3 C1 i2 s" R1 {
: ~3 _2 l$ J0 A$ O9 o9 g6 S4 {: t- uint8_t MeLineFollowerArray::getValue()4 ^5 e$ a- n/ a. \
- {
( ^5 V/ ?. ~! q2 a- Y/ Y - uint32_t LOW_level_read_time;- I4 L' ~9 S! z* c
- uint32_t HIGH_level_read_time;
- w2 R6 q: d% T - uint32_t time_out_flag;
2 \ @ r: X1 r e: L3 H - uint8_t Sensor_Data[3];
5 u! t# [, d+ Z9 J8 w% A - static uint8_t old_data = 0xff;
, {& L4 [( ]" n/ v \
+ y( d8 V( c4 k* L4 I% D. M- pinMode(_DataPin, OUTPUT);
7 r1 r' z' e. z - digitalWrite(_DataPin, LOW);
2 K% v) T9 [2 }+ U - delayMicroseconds(980);
- M9 j" r/ G" v6 ^ - digitalWrite(_DataPin, HIGH);5 N: ]9 @. ~) v, v& x; |
- 1 K- w5 m5 _" J% p& ]
- pinMode(_DataPin, INPUT_PULLUP);
4 O. N' Y- C$ | - delayMicroseconds(10); O1 z& V. C" I; D$ O
5 \" U) I& I% w- time_out_flag = millis();$ J# f' }# w" ]9 q
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- h5 O, n4 v9 i& G! b8 F - : t1 b6 r0 g. f
- LOW_level_read_time = micros();6 s! n; J1 }, _& O7 a: \$ ?8 F
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 O, V; B1 J) O/ Q - {8 Y6 r! ~7 D+ g% D) |( }4 g
- return 0xff;
5 e7 K2 i1 X$ \+ j - } Q1 d3 ^1 W# M8 x! o. Z
- $ R% V) N: [5 V, ]
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" E! q o( B2 h; ^8 n2 f4 N& Y
% G, Y. Z$ Q- ~+ b/ O- HIGH_level_read_time = micros();) S$ R8 T- D% w3 c4 q1 o& |3 ?" X
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level7 h! R) u7 n+ F5 v: M' L
- ; m3 m( x: G4 K4 O; _
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ s6 g2 Q0 K* O- J
- {8 t* d8 o9 H" E7 c
- return 0xff;' H& L8 N: t, b$ j/ ^1 Q9 ?
- }
0 _# b6 \ }: Z4 r. i7 j9 R
9 Z& C# r. X* Z S- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))" E* }! ~* w5 L2 _: D
- {
! ~! ?" L8 `! A- C" H - return 0xff; \# m8 T/ U& L, _
- }
+ `# `7 N" L$ H; M2 ^- W - - U9 s! c2 x x$ `6 Y3 q
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
/ X6 X0 U0 \) r - LOW_level_read_time = micros();
# f, R% c8 H/ M" X) ]) q% P8 { - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
& Q) P) p1 @/ d6 R3 g. a# s - ) G8 ~$ V! K" ]/ ?0 v+ k
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
8 E& U( K: l0 j$ A# o2 q: v' B - {
' X. b7 V" y# x - return 0xff;; p3 |. }7 F# w, A
- }3 d6 `0 W1 _3 o) y# {: Q
: {5 v& @0 {( b- i6 }5 Q: K$ Z& d- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
* _# h/ n) G6 ?' @" B. X o - {
0 B6 U, {% [! O8 ]2 |' [% q - return 0xff;) i6 e! h1 r+ l. V) l2 f
- }
' d* X# b8 V X% T, V; c* f
. l# B1 {$ Z9 r- for(uint8_t k=0; k<3; k++)
1 W2 F6 c# ~! _' r$ w6 [" Y, { - {
& ]7 j" D! Q# `- f$ X- r/ P - Sensor_Data[k] = 0x00;
9 O3 b3 t! A8 M: O3 F - 6 A; t1 s# [9 ^+ W& V3 w2 Z1 | r$ H; h- N
- for(uint8_t i=0;i<8;i++)
1 B2 V- T) d$ @3 i - {
2 U E6 t9 [+ D/ A- d - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
5 S) D M7 D8 S5 x( k( l$ x/ U4 ^9 p - HIGH_level_read_time = micros();
6 G4 _9 \$ S5 `. z, j - LOW_level_read_time = micros() - LOW_level_read_time;
, x$ i5 U; u; `
: f: X% Q# P T) D- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
6 ~1 t* j5 o+ W; p2 B) R - {
8 @# o L- Y& {2 a8 l0 x - return 0xff;! W& [3 n5 b7 e0 g7 M% \$ I* h
- }: j" Q( R# j9 H: ^1 }
- . }, k9 R0 _0 u
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ l. O( y* b6 a, U; c# p/ v# w
- LOW_level_read_time = micros();1 y4 u" j6 V1 E4 l0 m; ^
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
' ?; R% d' a. f4 b+ z! [ - : ?, B; `5 Y1 \' ^3 ]- J/ @0 f2 U
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
0 S" j$ J3 q) c& I4 w j' B - { e: b1 D& O# V/ o: M6 v! |
- Sensor_Data[k] |= (0x80 >> i);' Y o0 N$ W; @$ U
- }" J" G) Q' n6 T$ B$ v7 I1 {/ I- v, n
- else if(HIGH_level_read_time >= 100)
& G0 ?9 [, h) [/ [4 c c2 T# Q - {
7 u) U1 x5 P6 M/ G( ~/ }& d1 U' E - return 0xff;7 R$ J' ^* E$ ?
- }3 y4 { ^; w+ ~0 y# g
- & M8 J- t3 v0 d
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)& \3 j- F* _$ w7 L
- {
7 B5 L5 I( _. P# H( g. a - return 0xff;8 N) y! S/ h) O: M- }3 S/ X+ W
- }& A4 { Q9 C3 k8 R& }
- }
0 v4 m8 a: ]& _8 n" G7 ~ - }, h1 `! f' B" B" g
- 2 F ] r/ ~6 j P& ] w9 I2 I$ q4 {5 Y
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level% k4 e4 e/ S1 P2 U
- HIGH_level_read_time = micros();
; v7 j4 f- R# H4 z - LOW_level_read_time = micros() - LOW_level_read_time;
* k E* }$ t% C5 N+ y+ X9 K - - k, ~1 W& U |$ X- H# F5 P
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )# K; w/ e5 B1 d* E4 L3 a
- {* |5 |3 I# J2 t9 t1 p
- return 0xff;
9 J# V" ~6 O1 d: e" d2 N - }. b$ X9 `: }3 q1 g z6 U$ K
- ) i( A# L9 O/ b; r! p; z0 ^( G
- pinMode(_DataPin, OUTPUT);
8 t% _& j; T6 M$ K. H5 @ - digitalWrite(_DataPin, HIGH);
$ q0 a+ ]: P* Y: ]3 Z; x2 `* J
0 Z$ X, h7 f/ _- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
( _0 B* h& i M: W7 V; [- @% G - {
) Y) {1 S2 i4 Q& g4 } - old_data = Sensor_Data[0]; Q4 N* c8 D' O. p
- return Sensor_Data[0];
6 ], i1 W+ w5 E3 q- R) h) { - }- ^7 k( y2 T" H+ [ A% e8 D! J1 `" D
- else% N( j% V3 N8 R: }& G
- {
3 w; N' D6 @7 I0 z `, ?, Q2 v - return old_data;; @" G4 i K- ]& j' r& S2 O' Q
- }5 m* \ p9 z0 c) C h( A
- }
* l( Z5 R" B5 w: i, ~
複製代碼 ) | L! E5 v! M7 M/ j1 v
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
) W! M1 c- b. {* z z
: ^6 ^$ g# ?# E) }1 n& i* d' t |
|