|
|
$ S, b( E3 o. K. D- #include <Arduino.h>
! B3 ~8 N* {8 Z1 `( v n. x5 D - #include <MeAuriga.h>
( \: A2 X$ b; S - #include "MeLineFollowerArray.h"
' R$ W/ X, `; J( B6 j0 z/ F8 Q - - g/ W! a1 l- U4 ` v5 c0 Z
- MeEncoderOnBoard Encoder_1(SLOT1);# @( |6 F1 b, U
- MeEncoderOnBoard Encoder_2(SLOT2);
# @8 I2 C+ j$ C7 W5 ~1 W - MeLightSensor lightsensor_1(12);
' c5 ~6 o! d3 Q# E - MeLightSensor lightsensor_2(11);/ a( B5 i$ G( B( |
- MeBuzzer buzzer;
$ I# e8 ^6 i- \. T& q& U1 n0 @ - MeLineFollowerArray linefollower(PORT_6);) O& z& x# J: X, y3 G7 Q
- e, ^- V0 D2 [
- #define Error1 1
+ u. e. G) m: j" B- r& T7 s - #define Error2 2
" Y W" @- W* |6 d1 o0 r! ` - #define Error3 3
9 ^, Q4 ]- T8 h0 t9 o/ l# e4 }
) F; H+ Q) p: `6 s& J- #define Kp 15
+ c S/ X L1 ?4 y - #define Ki 0.15
: l3 L4 _1 w3 b* W8 i6 N - #define Kd 0.03
4 J5 b/ v# t$ G" M* S0 ^
9 v* H. Y5 r) ^! u: o# F- m \9 b- uint8_t sData;! q+ j% P$ |0 `& _" l9 E, ]. u( R
- uint8_t D1;
% H8 }+ g: G& k0 N! @8 v+ O& O - uint8_t D2;
, \4 E7 P, O! f0 q& d - uint8_t D3;
+ Q P5 U! \6 D$ p0 H - uint8_t D4;
: `7 t: G; ~: Z4 J# } - uint8_t D5;
* [5 {4 R2 A% g - uint8_t D6;
/ n+ l6 y2 x8 z% ^ - ! S5 L- l% y: t
- float previous_error = 0;4 w6 D( j4 I) f' }( v: @
- float integral = 0;
9 M# ] g. e% |) f9 m - float derivative = 0;
0 [$ C$ I* ]# }! N0 b - int Speed = 160;3 y v$ D) h% I6 n2 m4 b
- float output;
) V% Y: v, ~: |" O
2 N7 e* G M+ E& \: q, a9 n- byte Left;
3 H$ p. B! {3 g" C3 Y! w - 9 v7 N6 Z7 `1 O3 o* q" o9 W6 f
- void setup()
1 n! x- C k3 V! { - {, G% x+ Y; N4 \+ `2 B5 B9 V
- //Set PWM 8KHz5 J* P7 _. {; I# t( o' P) W
- TCCR1A = _BV(WGM10);
( @* I! B) Z6 J @ - TCCR1B = _BV(CS11) | _BV(WGM12);
8 x0 y& ^+ S; e6 v - TCCR2A = _BV(WGM21) | _BV(WGM20);
: Z1 ~& R, n1 a/ B/ k) Z+ W) I - TCCR2B = _BV(CS21);
( l) ~/ J* [+ {2 h! u; a+ i3 a - Serial.begin(9600);" y: h. `& k5 b$ e! j
- buzzer.setpin(45);/ U: U) }2 d: F' q$ G! o' l5 j
- }# `# k3 N; S ^* | d
- 1 y2 |4 c0 j5 O8 @! {% r
- void loop()4 K! H. J9 D% o' c
- {3 T( v& ]* z5 m" Z2 i7 j$ `3 Z
- while(!((lightsensor_1.read()) < (10)));
7 z. {3 Y2 v% q+ X - buzzer.tone(1047, 500);
% S/ @& u' o3 D; Q# d, c - delay(500);
. r, E8 ^, D9 M" q1 H$ i - do
1 _0 y$ m5 g, W# t" `) A# X" t - {
2 _% s9 i! X/ Z2 R% D8 H - int Err = getErr();
- h! `( M5 ~# x( u1 v$ @3 } - if(D1 == 1). _4 ^2 M. y% u( X f
- {
3 ~4 v8 Q; q# i6 m9 @3 s - Left = 1;% p$ _, l7 U* T
- }% E+ }2 ?3 p. }' K
- if(D6 == 1)/ p7 O, E; Q1 O: o( y# y
- {
. O( L0 w) y" F* V% y. [9 W& Z7 S - Left = 0;
- t7 j) m6 W: X0 N# B9 I - }% L N% D( s" s. p5 `6 e
- if(Err == 99)
: w* ^* T+ l' X/ u - {
" |1 [# q9 s4 C+ R9 ^" B2 M - if(Left == 1)
+ z+ g P5 m! w" S( V. ]9 T - {/ I% r1 i% `5 [& Z+ w$ ^8 L
- Speed -= 5;
2 l- H5 `! U4 j2 y( G - moto(0,Speed);. W. O& u1 k7 }- X# ? L& A% ~! V
- do2 x4 |& W# Q/ v! T0 F. ]/ R+ ~
- {' W% `5 e' D9 k6 E) P0 [+ A
- Err = getErr();
R6 }) D8 l# Y D - }while((D1+D6) == 0);0 n* p' t3 M+ x% U6 v% r
- }
0 V. j8 L3 J9 o5 f' r4 q( n - else9 n- q8 S$ p) e8 u8 V9 ^4 R7 c8 k
- {& n9 F7 u+ _7 k4 M
- Speed -= 5;0 u z1 t1 K3 o- @* R
- moto(Speed,0);
2 z) {+ i; Q/ k0 ], q% a - do
, V9 K' Z- C1 z2 H - {2 ]6 |( i+ x' Z
- Err = getErr();9 u" C4 @# \9 h7 f) \, c' V ^
- }while((D1+D6) == 0);0 W/ F. q: F n% G% L
- }" e3 R' d! R9 r' l, y) }, U
- }. @: x# Q* |0 C5 `+ B3 P+ K
- else# U8 K- c2 C. @" ]
- {
( b9 i" X. s, q% M/ [. b$ k - if((Speed < 160) & (Err < 2)) Speed+=1; ) T% ?2 T9 ^1 K$ A, Y3 x
- if((Speed > 100) & (Err > 2)) Speed-=2;
$ O1 a4 g# X7 U1 A5 i0 M" V& M3 } - integral = integral + Err;5 U( u( T& ?: _! q
- derivative = Err - previous_error;
( m0 R [1 J3 x8 i5 v9 N( e - output = Kp*Err + Ki*integral + Kd*derivative;& k) ?3 j7 V. q7 g" F
- moto(int(Speed-output),int(Speed+output));
1 g* W1 P) r z+ A! M3 N" ?3 K! r - previous_error = Err;6 Q4 [: W0 H7 l9 S, j- x2 N% Q
- }' `3 ~: F; D/ _4 T0 l' w
- }while(!((lightsensor_2.read()) < (10)));
. J4 C& a! F% d - moto(0,0);
' Z. q$ H8 l% U0 n - delay(500);: p2 |5 x! |/ g' }& R) D! _
- buzzer.tone(262, 500);
+ C" s. v# C' ?6 @ - }
2 p! a9 S4 n2 @# C - 6 A' }3 Z$ m- r* `
- int getErr()
1 N- {# J c+ c- N2 y$ n - { 4 C5 |: @; Y- V1 h" T3 o
- sData = linefollower.getValue();
/ d/ i0 r* I k: r - D1 = ~(sData>>0)&1;
. e6 d- P; d$ J% ?, l2 i3 [ - D2 = ~(sData>>1)&1;9 P9 t4 A# `% r/ u! I) U+ r' z
- D3 = ~(sData>>2)&1;
. G1 z$ i7 C- p8 U6 z* a- T - D4 = ~(sData>>3)&1;: @) h# u# z, K6 z1 I
- D5 = ~(sData>>4)&1;
& |; ~: J+ G& ?8 d$ j* Z - D6 = ~(sData>>5)&1;
5 u7 q Z2 y9 P( s) O4 ?" @ - int downD = D1+D2+D3+D4+D5+D6;
: ^. z4 b8 a I9 m7 y+ @ - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);8 Q8 U. {" ?$ W6 i
- if(downD == 0)
* t [7 r" a1 o8 ^5 h - {
+ x1 e; [, z- Q" k$ n& F" `" F, A - return 99;* m# f, I2 b3 a
- }
5 W8 P* U- r( T- r - else% Z2 q, u/ g5 Y
- {1 x* t6 P# v0 f) @. M d( j
- return upD/downD;: `# D& o3 w% \% [- c) ~
- }
/ d6 s0 a$ e6 b+ N4 ~ - }" V' k, p8 d, Z! y
- & T2 N3 l! t" ?- I3 g" i
- void moto(int Speed_L,int Speed_R)# z" S1 c2 r$ [% j6 k; T0 ?
- {
9 @$ M' X$ M1 {* ^ [ - Encoder_2.setMotorPwm(Speed_L);
1 k( M1 h& b" z: V% \1 k - Encoder_1.setMotorPwm(-Speed_R);
. Y) n) A( G; b9 O' ^ - }
複製代碼 ' q7 {* B' X/ \
MeLineFollowerArray.cpp$ M# ]" N' a4 |! K' G
- #include "MeLineFollowerArray.h"2 j# K" h. R7 N
5 g5 _) [& U+ X4 ^8 m8 B( a9 d- #ifdef ME_PORT_DEFINED
$ `8 F6 p) U k. C( W - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)! J2 i4 a8 q' T; _) ~
- {
; L. g5 F5 H% Y0 X
& o, g- d; e; N- }
6 q* Q7 R! ]2 A2 h/ G5 g2 ` - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port). e. [. \6 Q5 r" H
- {
( y/ Q$ d6 H3 T$ ? - _DataPin = mePort[port].s2;
% [1 ]( ^* x) y5 w/ W, q" x - pinMode(_DataPin, OUTPUT);
A% E- O: Q4 t - digitalWrite(_DataPin, HIGH);
9 g7 L9 \0 B3 U7 s - }
& x- v) u4 s8 m, V* M8 E, Z - #else // ME_PORT_DEFINED
2 j5 U; m& M" O0 _5 E1 h - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)/ h3 L2 N# q8 z4 ~: _9 H
- {) ~2 w" {* w6 ]
- _DataPin = pin;' t& F9 @6 y1 W6 v. v
- pinMode(_DataPin, OUTPUT); 7 ]8 f: Q( P. u+ R( V3 a8 ^
- digitalWrite(_DataPin, HIGH);' c5 T/ `8 \& P
- }
5 T! P- J- w/ H9 K: u& F - #endif // ME_PORT_DEFINED
8 A7 A( \/ }" S3 H" A. U. y - {+ Q' a) {: ?+ }
2 h$ l8 t T p- e, X2 _4 p; N- void MeLineFollowerArray::setpin(uint8_t pin)
: b! H) [ x' Y$ ^ - {( v$ H: W, u5 M# E, w( ^# D
- _DataPin = pin;
; A, U& u* q1 q# F7 i/ d - pinMode(_DataPin, OUTPUT); 8 M- a; U- }* \2 ?& x. P- N) n1 C
- digitalWrite(_DataPin, HIGH);/ C/ g1 e6 p. ]# ^) W
- 0 G# Q s& N; T+ h
- #ifdef ME_PORT_DEFINED
/ m& I$ K1 I4 P$ n - s2 = pin;
. A+ `! y' {) }" ] - #endif
! t/ u# l# D4 n% e( v8 R& ] - }
! B' S1 |$ a1 h7 h/ w# S9 n. W l( E - . I; L2 A* p- x! M" ^' R$ Z( n
- uint8_t MeLineFollowerArray::getValue()- t- P" b j1 s* d9 H9 G: K0 Z! y
- {9 C& J1 l6 s. ?( r, v: I2 }0 x% @
- uint32_t LOW_level_read_time;/ u4 R1 Z" S! G+ l' y) m
- uint32_t HIGH_level_read_time;0 x; M7 n& b0 Y- H2 H8 F) S5 b6 a
- uint32_t time_out_flag;
4 i' l- h9 X/ |$ a% W# r6 {! O. y, U' O - uint8_t Sensor_Data[3];* O$ U( _6 r3 x9 I: U5 R! u
- static uint8_t old_data = 0xff;& M" C1 o5 c3 g
- , g6 R* n& ]5 q) m0 x" G! t% `9 A
- pinMode(_DataPin, OUTPUT);
& n6 N. k6 f3 @. q( e - digitalWrite(_DataPin, LOW);
0 A3 f' T0 w ~) g9 K - delayMicroseconds(980);
% y; W. I/ v% \4 L* p1 i - digitalWrite(_DataPin, HIGH);$ j y9 F0 I, L6 Q' c' c2 n3 z
- 4 R8 E. B6 C, F6 x/ ^# C1 T
- pinMode(_DataPin, INPUT_PULLUP);
- R- t3 _4 C' ^3 ?: w& ?) H1 k - delayMicroseconds(10);5 D$ o0 L8 \0 |/ j3 N
- ' l# }, u7 k0 K( N" V
- time_out_flag = millis();
7 o) p+ k8 W& N. a5 s! q# I; N - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
2 ]/ T8 {$ S5 Y4 }; V0 { c - . L* v1 {" k6 o0 j4 o$ I% D+ C
- LOW_level_read_time = micros();
/ x$ ^8 W A+ Q- h* k1 n - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
+ y5 A, m$ U) h* j- h# A1 C; W - {' N+ \; @, u( h1 B* j
- return 0xff;( A) A- L7 Z8 G: m' B( p$ K$ U
- }
$ j9 r Y5 F* h* t' {
' x1 n J% d: m* b8 a- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 r5 M* J7 R' Z$ e6 _ - $ f5 P4 w. d/ i6 ~4 e
- HIGH_level_read_time = micros();
5 F; G/ u' z" `* f$ q) D - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
; ~0 W7 A) O3 E; v9 l" D
$ X6 N7 Q1 L7 w1 [% |- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 p" K! H' Y1 f1 _2 Y- b4 e% ]7 } - {# l3 z2 I2 Q7 ]
- return 0xff;
7 z, D2 `9 x! T+ |8 p: H. W - }2 f2 r5 p: B$ e9 D0 R
- 3 m) f5 Z* Z3 g+ V2 }' u
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))% Q6 i+ C$ o6 f9 j* x
- {( c% g! u9 c) k1 U6 X- a
- return 0xff;
0 r$ R! t. a9 f& }3 F* j) p - }
( R1 t, |4 c; V. Y7 @% c
( n7 {5 T/ ~9 W- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
" t3 J( H5 D( Y' [2 v0 w - LOW_level_read_time = micros();7 {, W; I# H' h5 Y6 }2 x
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
. E& }+ [* P; m - 8 }0 y! G5 m3 u& f
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out: R; ?1 E2 k# e1 t9 ?/ E' f
- {$ D1 L7 e% f- V* G
- return 0xff;" n+ k4 k1 g& ~; {, i0 q+ ?
- }9 N0 u2 d9 Q! r8 \. F' V8 U) }) B
{; I. h, u6 d- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
9 j( D5 D' `! J) h- q& e9 _$ j1 j2 c - {
% P2 S( E0 ]3 A' j+ {* t5 |6 B6 o - return 0xff;# r# Q; C1 Y$ ~/ ]/ H8 J1 y
- }4 e3 U% j' w4 Z/ Y1 _2 x
- + D7 R) H" ]. j
- for(uint8_t k=0; k<3; k++)
7 u0 P0 f' m0 D - {: S$ }: L: f2 j
- Sensor_Data[k] = 0x00;2 p+ j) [! w4 S/ ?$ E) m7 e+ Y
- ( X- T6 W# r1 S( O9 d5 V9 a: K0 S
- for(uint8_t i=0;i<8;i++). g7 S6 W$ e3 ^/ Z- @2 P; y$ i' h
- {) d1 B! Z: F* _) E q: \8 c6 |
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
" S9 ^; X) ?: b) {: P5 u - HIGH_level_read_time = micros();
8 v! T+ a. y5 e9 k7 ~- ]2 _' z - LOW_level_read_time = micros() - LOW_level_read_time;
$ A& s- D: Q% L+ P; Q - 3 T, K) }/ m& v
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )8 t' ~* _7 c" o' n3 v
- {0 O! D% F' s; K! T! x( v% n
- return 0xff;' b% w; B( y. m+ X1 ], W. O8 n8 C/ D# U
- }# h1 d* R0 y% D4 e
) o9 C2 }7 L. e- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
" f- Y1 d+ M% @ - LOW_level_read_time = micros();& `3 G) P/ \) Y4 J% _7 D; i
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
. t: F+ M# }4 U ^ w1 @7 _ - 6 m6 ]9 v4 {+ ]0 E; T6 t1 a- J
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1$ b1 I: t0 F9 S, o5 A6 h% B0 v
- {
% h: S- D$ D; ]6 Q }5 Y - Sensor_Data[k] |= (0x80 >> i);
$ v' A' O9 ^' B: ~/ a! L# ~: N! r - } r7 L1 j+ `* B) |1 u% A F. C V
- else if(HIGH_level_read_time >= 100)1 H. i6 Z7 X1 a& ?3 y1 I
- {9 d+ j3 @; t) t S% D ?: U
- return 0xff;
/ n1 |& g2 |* T% G' N - }
6 }8 l' S9 O. [$ ^* g% x
" H* q' z2 J$ z% I" J- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)" r0 Y1 Q4 F$ n; D) F6 l
- {. J+ y3 }3 i) }% g4 M. M
- return 0xff;
( Z5 v, [( [. W$ j - }
+ j0 Z6 C% C7 v7 c5 e - }
, F, j1 i; w( T - }
# ?5 N6 ^+ R2 c/ G$ R! F - , `0 j3 X! s" a$ _
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level$ [/ G% p# _5 C6 f1 ]- r
- HIGH_level_read_time = micros();5 C0 y6 h# t* @# I/ Q1 Z
- LOW_level_read_time = micros() - LOW_level_read_time;5 O8 @: R- q/ a8 C/ B1 f
- 6 V' I F: z0 r9 D8 P* Z% m
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )6 d& K+ |/ u' d% ^) w
- {( d7 r$ D$ i# |7 h6 \
- return 0xff;. g/ p6 R9 I2 A4 X$ b
- }# A/ N) g$ N: X0 T
- ' Q7 C; ~8 e! m h
- pinMode(_DataPin, OUTPUT);$ _" Z* r. |6 r1 U
- digitalWrite(_DataPin, HIGH);6 R. E, `8 ?% I4 |/ f! V3 P
- b' I* i: M" x4 R/ p4 P* a- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))- W# h# {: R# \
- {8 d2 A6 O9 c, y. ]1 e9 D! v
- old_data = Sensor_Data[0];" d9 P4 Z8 @* q F( `
- return Sensor_Data[0];3 y. ^( C/ e4 q# d: V3 n& P
- }
; K8 N i# d6 |! U" Z# H% \ - else
8 E& S4 A, q& Q% x ?9 R - {. m3 T1 s5 B) W& o ~
- return old_data;
7 ]. V; _( L& ?- W2 L' U) ? - }
) \ G L- E# a - }; l# s G! S4 u0 ?# b. x
複製代碼 - x( F/ V+ `5 F, K' K
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
4 p! H% _8 R# x- J
0 f' m1 q4 I4 T3 `' G( x) N; d4 } |
|