|
|
' ]0 {+ ], R7 [" N ]$ v) y- #include <Arduino.h>
# ]) L/ d5 H' {0 T - #include <MeAuriga.h>
4 B) Z6 ]1 e I7 f) {) Z7 ? - #include "MeLineFollowerArray.h", t# R0 v6 z/ P& g3 t
, b( e) j7 a& G8 M( x9 Z- MeEncoderOnBoard Encoder_1(SLOT1);
4 p) `9 A9 ^7 l0 k- h, ? - MeEncoderOnBoard Encoder_2(SLOT2);
+ j* _ i9 N! H; b - MeLightSensor lightsensor_1(12);
, [8 M$ L ]. H( P/ L - MeLightSensor lightsensor_2(11);
# y$ y0 A& W0 c: O0 K, f - MeBuzzer buzzer;9 A8 v) W8 M$ D& _5 P
- MeLineFollowerArray linefollower(PORT_6);
0 g# a* d( r3 `/ L! S+ N+ S/ K
2 }' E, V2 Q" R) R9 T9 y/ a9 }- #define Error1 1' X; a6 t8 ?& w$ G+ t+ h
- #define Error2 2
8 ]9 i3 a- L2 f: ]% h3 s7 \ t0 a$ C - #define Error3 31 c2 R8 P- f3 g" s/ Y2 G
9 {) |2 n" Q5 F( s- e- y/ p- B- #define Kp 156 { S" w: @( D; m, E, I7 _; k
- #define Ki 0.15
+ c& C0 n+ y5 C0 ^& r6 g - #define Kd 0.03
- c, N1 O6 w0 X
. V0 T- A/ G$ V0 M2 j* c- uint8_t sData;
. r4 S3 T% S) Q- ^ - uint8_t D1;5 I: q) B+ e+ T
- uint8_t D2;6 z' p D" D& M
- uint8_t D3;
3 u0 P% G. k4 N: P - uint8_t D4;/ x& S* ~( Z+ f @8 e5 I1 C* u) d
- uint8_t D5;- S) M' M* _8 d( d* Z% Y
- uint8_t D6;' O o: j! [4 ?. T
- 9 u' v4 N b f6 _5 K
- float previous_error = 0;
+ |/ U( w8 G5 c! l8 _ p" N - float integral = 0;* v" q& o( K6 K4 F9 |" \
- float derivative = 0;
- ^9 a) m+ ^: Y- m - int Speed = 160;
: M2 E+ s# w& o! Z9 _1 Z - float output;3 G$ Z# F, X. n; e2 a3 } h. A. ~
- ~) s! C* M3 d+ \3 t6 U2 P- byte Left;) r: o$ R$ l8 F8 Q L! ?: V/ [; H; g
- 5 ~4 C L; Q3 H# b% l
- void setup() - r- y; t$ l0 v4 W% r
- {' N3 P: `5 x2 Q1 F) K
- //Set PWM 8KHz
' }+ c3 S$ k0 |* V - TCCR1A = _BV(WGM10);
7 } g3 \5 D# ^7 ~. U9 h6 Y - TCCR1B = _BV(CS11) | _BV(WGM12);
4 K J8 `" B8 K, |- Z. v - TCCR2A = _BV(WGM21) | _BV(WGM20);; w3 Q4 M* l. I
- TCCR2B = _BV(CS21);5 c: O5 h2 X$ m- G( b
- Serial.begin(9600);0 U. M* D4 P% ^7 n
- buzzer.setpin(45);- T' l0 m1 z3 f( z& B) P; M
- }
1 D% `! F! |9 Q; [2 n - 2 z" Z' Q& _0 O" \1 P+ X; F" p
- void loop()
0 W$ n! I7 c9 R( Z$ B - {$ N. u- ~6 N' F
- while(!((lightsensor_1.read()) < (10)));
5 c5 k) A0 x* q% {( I# x v$ N - buzzer.tone(1047, 500);
, e# n, D$ {8 X, P) U - delay(500);
) h) l) X" e1 G& p. u - do! N7 K+ r7 B* A; z7 C8 n
- {
$ `& _: o d5 {' @4 i3 q8 n: V* g - int Err = getErr();) K; ]3 F6 ?# j+ w [: ]! C
- if(D1 == 1)* c( F# f2 D* I7 E4 S! J
- {
. h/ _7 V$ ^+ d' d: A) \- V - Left = 1;
/ X: Q! [( E. J+ T/ E% r - }
( U3 I7 k3 `& t6 x& r1 A- j9 z. ~ - if(D6 == 1)' ~" T0 U8 E9 Q S; a9 D2 Q- X$ N
- {+ z+ t6 G' K* l
- Left = 0;
9 V2 v; ?# s y7 o2 p - }
( @2 ]3 V* V6 I. `7 q; P4 g' d - if(Err == 99)
3 d& c4 t. @/ O6 j1 T8 G - {
2 A8 x* W* p; ]. n3 I - if(Left == 1)
" n& h, f: B ^6 W - {
5 `- n6 Q4 f' k( G) l - Speed -= 5;7 e$ A) l; U' S0 M
- moto(0,Speed);# B" H2 H6 z% F' G( |% z
- do
- Y* {; c, Q; N j' W - {* [! J2 ]" L7 A$ J. t2 N z9 D
- Err = getErr();( ]) q6 P1 ?, {" Y# v
- }while((D1+D6) == 0);
- u5 G7 q# }# w- Z, Y# T0 ~ - }& `' x6 r* R! \. {* v
- else! g& Y" r2 ]& a; c
- {3 A6 H" B; m) a# h& a+ N5 ]
- Speed -= 5;
; N) F/ C& _2 b3 Q8 O$ l9 N! U - moto(Speed,0); ( C6 B7 R1 b! d
- do- @0 s. w% W+ |- W
- {
7 A) m6 f$ K* L0 u - Err = getErr();9 C9 |& Q% V8 h( U8 V3 D: N9 B7 f
- }while((D1+D6) == 0);- o8 x1 \9 Z3 f. L
- }
( r. ?2 c; Y1 a( f. I - }
, u2 y8 e3 x) S8 s1 d2 m! `7 \ - else2 H% ]4 k5 `; t! [/ e( ^+ A
- {
) B+ U" ?4 \8 H4 I" `7 [$ x+ L4 R - if((Speed < 160) & (Err < 2)) Speed+=1;
7 A& D$ q$ l- e; m# A$ W, ? - if((Speed > 100) & (Err > 2)) Speed-=2; 8 c0 N; |* ~0 H7 [
- integral = integral + Err;2 A2 V; A+ o2 K8 k
- derivative = Err - previous_error;
( e: a+ Q, P" V5 B - output = Kp*Err + Ki*integral + Kd*derivative;: S" i. P1 b( q
- moto(int(Speed-output),int(Speed+output));
5 N: D; X9 x' m4 j+ r; Q* P, N3 B - previous_error = Err;
9 j" h% @, Z: K n1 a o - }
' S- u! S. v# U" E# m - }while(!((lightsensor_2.read()) < (10)));
7 V4 a6 ?- e6 W" t - moto(0,0);
( \2 G7 `4 w, T" _ - delay(500);% f! I; F* Y2 l- Y O \& ?: S1 g
- buzzer.tone(262, 500);! Z7 [; I+ u( K0 c# S
- }
6 C/ y# g$ T9 T' f% d+ f! Y5 _ - 1 e* a4 t% D4 U! v
- int getErr()# K! R+ v5 j3 E& {) A
- {
6 i3 l+ T* B: n8 p) H! | - sData = linefollower.getValue();; ~* O- r& Z$ g
- D1 = ~(sData>>0)&1;! E( r- I& Q( O$ E+ O
- D2 = ~(sData>>1)&1;- E1 A. H* n8 A
- D3 = ~(sData>>2)&1;' P C Z3 R- ~1 W' d8 o; D Z% B
- D4 = ~(sData>>3)&1;3 L6 u) C% }' @( h1 b9 u* ~
- D5 = ~(sData>>4)&1;
& V. p5 B S; A- B' t& Y& z! I - D6 = ~(sData>>5)&1;
) W- q! [, D- L% ^2 Q - int downD = D1+D2+D3+D4+D5+D6;# l% x6 e# J; R% k2 Z$ U
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);% F. _4 \: [! L9 y
- if(downD == 0)
" [% l# J* U+ r6 d4 l/ V. k$ B u1 J+ @ - {- s: C% I0 Y: m. C* J$ E) a4 J2 Q* U9 D
- return 99;' A7 w: P* n! s8 M$ a9 @ Q" T
- }
+ H$ i1 F. E6 Q/ D z - else: U5 K/ s7 J0 ] j0 ^7 q
- {0 ~- e4 \2 ~* E
- return upD/downD;8 B# I& j! ]% H& P, @8 T
- }) W5 _) l4 _( i: w& d
- }7 j: L! u. ? P4 I% M( ?$ U
! X- |: o7 y; n! T- void moto(int Speed_L,int Speed_R)& l1 j. \9 C+ i3 z, y4 I
- {3 E1 h% c4 q& @9 H
- Encoder_2.setMotorPwm(Speed_L);
0 B) s1 z- p/ ^" k. X; \9 a- C - Encoder_1.setMotorPwm(-Speed_R);
+ l& F5 | Q% m( e% h9 x$ B3 N+ G - }
複製代碼
- c+ N, Y6 N& `' H3 l' {6 oMeLineFollowerArray.cpp! q7 S- F% Q* [: W# r9 ?4 m" O
- #include "MeLineFollowerArray.h"
9 n- C; b7 O% N' d0 }3 Y" P
9 b9 }) l; r1 z0 L- #ifdef ME_PORT_DEFINED! p0 p% D. l9 H6 G
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
( l8 w1 s6 G$ m- F* \$ H - {9 G% A$ l) s5 I% c
- & C8 f4 o1 ?9 O% b( p
- }* U% y# T3 v# u2 A' m0 f) l
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
4 B- {- f3 N( Q! d+ \ - {
3 F+ r: @( K9 Q0 j6 a# ^ - _DataPin = mePort[port].s2;& l7 b, j* i8 T. ^
- pinMode(_DataPin, OUTPUT);
" g9 a; }# U8 t# L, h- t/ x1 w - digitalWrite(_DataPin, HIGH);
& }$ p! S% S$ T$ M. t& b+ u2 R - }
' s6 P, n/ m; J7 I' h7 t+ s - #else // ME_PORT_DEFINED
7 v! h; K! f. S, V+ s$ M X - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)6 D5 {: O# G) K! f! s A
- {
4 E: x1 {( X( J& ] - _DataPin = pin;
! \4 ~' K1 V& w8 @& h8 \( X - pinMode(_DataPin, OUTPUT); 8 F0 a5 u1 R' J8 B( a
- digitalWrite(_DataPin, HIGH);" K+ U# O0 `) h# {9 _- P( j" g* }
- }4 A$ ?$ p# g. N7 o C3 V
- #endif // ME_PORT_DEFINED- ?6 r% t3 K: t& V
, I4 ?8 i# Y2 r, S1 i1 K: e% x
: w1 [5 q8 t7 T$ }" ?; x& p- void MeLineFollowerArray::setpin(uint8_t pin). }* I( `( M0 m( p. ~
- {4 v; i6 c6 h7 f; \$ X3 V0 t" @) E
- _DataPin = pin;4 A8 g I2 ?1 C- b5 Y1 A# N
- pinMode(_DataPin, OUTPUT); . R/ X6 Q% r: D; q& z `% }6 U
- digitalWrite(_DataPin, HIGH);
) _) c- r* k) P" b, |8 n2 ]/ ~ - ! ?! k% J: s L. V6 Y: H' A
- #ifdef ME_PORT_DEFINED
0 ?; g2 Z# {* k p d# N - s2 = pin;
4 L' Y2 ~$ Y" p* J; s7 B9 i - #endif" y( N" Y3 _2 ~( q+ {. E
- }) h& Q I% ]# z5 n7 P9 { J$ m u
# t& K6 i% G0 E, W0 Y/ g- uint8_t MeLineFollowerArray::getValue()
; p$ j! z' O( f' A - {# |: H. p/ {) s- e, d( e+ X a; L
- uint32_t LOW_level_read_time;6 U' J6 `/ M( o# S6 \
- uint32_t HIGH_level_read_time;, {- u. k1 V& B4 O
- uint32_t time_out_flag;! o. J: o' [& C, {/ x
- uint8_t Sensor_Data[3];( U/ e# E- z# j! C( m
- static uint8_t old_data = 0xff;
, ?) s6 q, P+ A* j8 g0 g
) c, c$ _8 ]/ E9 H, L; O- pinMode(_DataPin, OUTPUT);
6 P# Y8 e3 L" @+ c - digitalWrite(_DataPin, LOW);
; D4 C$ s" p* s - delayMicroseconds(980);: }( ?) `4 z, l# k7 [! L8 q8 V
- digitalWrite(_DataPin, HIGH);* J# g" q! C+ @6 @9 [) T& Z
! B" x S: \- G' i8 k' \- pinMode(_DataPin, INPUT_PULLUP);
, _1 X( h! r) R) U5 r: n - delayMicroseconds(10);! h/ I' R. M5 j# \) U$ W, l4 l( X5 x
- . }+ a; S) Z' R4 m9 S7 O9 K* Y7 f B
- time_out_flag = millis();4 d" r0 Y+ r3 L- s) n. l- j
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); r i/ `9 \3 U: _* V; b
- / e+ G5 t5 E9 [" v7 h0 k
- LOW_level_read_time = micros();" b' |( F% n$ |/ v6 I2 F
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
: k2 l+ M% d+ L7 J# e - {
$ x" R! ^) O# T% }( T A - return 0xff;
m8 [" O& c: p$ y0 v' e1 _ - }, [ m& }! n" u$ ?' n
- ! B# l* @, G9 B$ t
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
, L1 d( l% I' p3 S6 ?7 m+ r6 A
8 O2 d2 P! d' b6 i- HIGH_level_read_time = micros();
* z, r5 V6 z ^" w& V& N - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
2 l. k5 [/ m, D% K
3 Z+ L5 b* e& h8 w& ?8 R" f/ j- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
$ R8 ^5 O. c8 C, k - {1 h, M U! `& f! D) |) |
- return 0xff;
! Y$ N, `4 F- q3 F - }
9 f. C$ q2 x! p! r
/ p- G( \: H* X! a6 z- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))% N+ ?& M# x8 ?+ G/ z% N
- {) [1 \! I9 E L+ ?0 Y# N- G
- return 0xff;
" g- E/ ?: g# v6 H. C+ m* `, c, u - }
! Z. _/ d3 f& b, z
' K/ N; i; f' l! d) l- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ W5 }3 H: s* I ^* e) \$ \& \ - LOW_level_read_time = micros(); ^7 b# \5 h, M: U( ~/ }' [( ]" N
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level6 z; P# V: S D( o- ~$ R; n4 F8 J
4 }1 n" Q2 |2 {& q t" w% @% @; t- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
7 p4 q1 s6 Q; ] - {7 C8 {6 s4 V( t3 W& p1 S# C
- return 0xff;+ f& B) K, G2 g% k5 I, [, r6 o
- }9 i3 K8 I* x! E* N, r n$ ]
- / e' @9 t/ E5 h( j7 t6 Z/ T, X
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
. @- n2 Q/ u) Z9 B2 ^4 k - {% f4 v2 G/ g) S9 Q" @
- return 0xff;7 j% @! ?8 E" }2 t9 O
- }
" s- q/ _0 C l2 f% ? - : A4 J- `( k/ z/ h% M0 h0 G
- for(uint8_t k=0; k<3; k++)
4 w. [# f$ ]' b G0 r% |$ a - {
: [4 w+ |2 U2 |$ Q/ z - Sensor_Data[k] = 0x00;
! ?$ H% U" O0 P( Z) q6 k) ] - 3 P* m. o9 P( Q+ _7 _3 U1 C6 `
- for(uint8_t i=0;i<8;i++)2 x) `, T3 Y, w% F# }7 {1 h, Y
- {3 B) M) W6 W y l3 R# J, _
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
2 Y2 R! ^3 h0 Y, m, B8 T - HIGH_level_read_time = micros();' }8 k5 F; r6 V8 `+ S A
- LOW_level_read_time = micros() - LOW_level_read_time;
: z: W9 X& \! l
! E7 X$ U# I1 n- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
) I, A; @2 \/ g, K6 |6 W - {3 N3 C$ O U; W+ e
- return 0xff;* e: s% p1 R. r4 {. @6 T
- } T5 Y, g6 ?& u
- 9 b: b$ ^; ]; V1 u* ~4 n
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ a, l4 |- f1 Y4 S. j8 S* d. B$ C
- LOW_level_read_time = micros();$ m# }( @8 D ?+ i
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
- S( t! @6 W% U4 x, P0 s- \6 Z; z& v
6 G; L: ^8 [9 l6 X- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 18 T+ d; l( A+ f7 F! g6 @4 e1 S. Q; d
- {
4 Q3 a7 N& Q9 I) s5 T# L - Sensor_Data[k] |= (0x80 >> i);$ E0 {% j. E$ C* D; d0 w% U: H8 U
- }. `5 ?6 L2 _7 r+ t+ P
- else if(HIGH_level_read_time >= 100)# O" p6 h$ F, X+ A* H3 s3 b
- {5 @/ d [( g4 |4 C" o
- return 0xff;- N% p- M7 v& Z+ ]5 y$ {; J: I
- }
' E6 K6 r4 {, ^+ Y - 0 ` x1 h5 q( h! l+ |
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
' x% c3 I& k. W3 y/ V! z - {
$ b9 u9 z9 d! ~, K' D - return 0xff;, B9 d# C5 m( K2 u, R' f
- }
: I# f7 F4 X$ v( k/ j3 \) d0 R - }! S0 c4 ~+ j+ U$ Q0 B
- }
6 t: @( C: X- @" Q6 q# y) p
8 [) ^* B/ M4 T' X0 T6 ^- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level4 k3 l, j' v# r- y
- HIGH_level_read_time = micros();
' W, P. N0 G0 T# H0 M6 f - LOW_level_read_time = micros() - LOW_level_read_time;
) z A$ G+ h3 W0 E1 h* i - 3 F& y5 j9 P& |7 K& e
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )" b0 l, u- O: m5 S3 V
- {' W4 |7 q+ f& j' U- _+ ]1 A
- return 0xff;
1 u1 T4 U/ y: W6 V! n2 Y" C - }
& R% x' D7 c0 P6 X - / q0 t* N# q1 d: Z5 n
- pinMode(_DataPin, OUTPUT);: E" P$ K* w; ?1 s
- digitalWrite(_DataPin, HIGH);
' |# S- W0 P& R/ c' q
Q% X8 k8 }1 c) X- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0]))): v. C2 D7 B5 l) L' |
- {: k' O* B$ S% Q, ]2 t
- old_data = Sensor_Data[0];. r' j0 X" Z: o d, @9 ~/ f
- return Sensor_Data[0];
% |0 k, A& o/ \ - }5 q5 a- r R( k% Z
- else+ |7 V; `4 D# |% n: d* e. |) b
- {& N; |0 u8 w i) a: l3 g4 }2 z
- return old_data;! c+ l; y; o9 T" o
- }5 {% w! b: B& p ~8 u: L
- }* |/ x8 R+ o, ], c2 U$ v
複製代碼
0 V% U [" X# P' B& A7 E* H ZMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
: G% U/ I8 r' d
" u O8 o2 A9 u4 J- `3 j |
|