|
|
' @# q9 w0 e+ S3 t0 I7 T6 h9 z- #include <Arduino.h>
! `' I& f f6 p- _& s - #include <MeAuriga.h>& k2 S& G! t: `5 ^2 ?
- #include "MeLineFollowerArray.h"
+ P3 C# M, U. I) l0 L/ h8 D0 d, y - 7 B0 h9 e( @" h( ~2 X$ x) j
- MeEncoderOnBoard Encoder_1(SLOT1); @4 p2 x! j% ?" [" z N( r0 z
- MeEncoderOnBoard Encoder_2(SLOT2);
( V6 ^' S/ `; S6 b% k# v! I$ _ - MeLightSensor lightsensor_1(12);$ Q1 r0 y& E. |$ Q; d
- MeLightSensor lightsensor_2(11);& P; x6 k! @8 S! {3 R$ Z
- MeBuzzer buzzer;/ D4 G3 a( m! ^) ?& d
- MeLineFollowerArray linefollower(PORT_6);* _- X. c# ~6 ?1 n! }
( B' o \( r: J# `0 ^2 }5 p- #define Error1 1& t+ ]: W1 C/ [1 a/ @
- #define Error2 2
5 r' U* N4 g9 g6 U/ b - #define Error3 3: n- ~* ]8 `/ ~: r( V* N0 e
$ G& I; L& t3 E/ M. j0 B- #define Kp 15
( z% u! Q8 a" B n' m - #define Ki 0.15
! V& t" o$ p! f9 C) Z - #define Kd 0.035 m* {+ J0 j' v
- 2 i% _. F w% n1 a. K0 C- ^. ~, s
- uint8_t sData;
: Q( B3 ^( z4 K# O$ z8 t - uint8_t D1;
( D( s6 D2 }+ Q, `5 ^9 p - uint8_t D2;$ Q) M1 L1 E8 F& m3 a* l; q4 W
- uint8_t D3;/ R# Y* l( C1 m1 w6 O5 I3 {3 ]
- uint8_t D4;
! t$ O0 A. f3 d/ x# ^) h1 x - uint8_t D5;4 i( h& q" Q: G4 g7 g- n
- uint8_t D6;% G" \ {! j+ v" X; c
/ A: f4 G6 l9 O' U3 t7 t" r& x- _- float previous_error = 0; J. s: _9 `& O& X
- float integral = 0;+ w1 U; }/ l! q, ?+ J0 C% n! ?- G
- float derivative = 0;. j5 p9 n6 Z) Q) n) w7 ~
- int Speed = 160;
, m9 u8 n6 p" P! h% V4 N# s4 C - float output;
( p' v/ L/ B7 H8 |$ R4 } - , Y7 T% y: T0 S h1 W
- byte Left;
7 d! n( @( {* _8 D" H& v
# E g/ M) ~, K" q- void setup() ; [+ B3 o+ m9 u w3 C4 I- H1 x
- {0 m# U; Q1 y' E9 J( h0 w/ q
- //Set PWM 8KHz
1 q1 J# b- c% B2 A9 l7 o5 } - TCCR1A = _BV(WGM10);
6 ^) ~' D7 p% w1 X& R2 p; z# W6 Z( j - TCCR1B = _BV(CS11) | _BV(WGM12);6 P; p+ ^- v2 s6 e6 Q/ Y8 j8 G' W
- TCCR2A = _BV(WGM21) | _BV(WGM20);' I3 y2 o+ d1 m
- TCCR2B = _BV(CS21);
& w! I- s8 K: w% Q2 m E3 n9 ~: c$ h - Serial.begin(9600);
# k, I' q" }7 _$ t8 @: o/ L0 p - buzzer.setpin(45);7 N& N# @2 f7 P% l
- }
/ c R* d! L7 l* [
+ B+ [+ X- ^8 `4 b" X- void loop()# E9 A; J5 \ g
- {; h0 Q2 m% C. ]7 |/ I
- while(!((lightsensor_1.read()) < (10)));
- K3 M+ [+ C8 W' z- ` - buzzer.tone(1047, 500);
x- Z t7 O0 p9 r* T2 [ - delay(500);
" |& }4 ]; `1 J+ U: T - do' |& o ~+ A+ o6 N( T
- {6 C, {0 T W9 ^. T( s( O& C
- int Err = getErr();
0 y' E/ _- @6 } - if(D1 == 1)5 G2 U" D- R2 e5 x6 n/ ^$ H# S5 L
- {
! p. ~) ^ f( s - Left = 1;3 @: {6 n3 a1 w1 P+ w8 V
- }
0 }6 B) V0 g9 k, p. y - if(D6 == 1)
; h. @8 C, E3 w- ~ K4 O$ H - {) h. O& u" C2 Q1 |* p. _8 ]
- Left = 0;0 Y" Q& ]; H T0 _+ h' k3 Z4 ?
- }# } V4 n; h( }* E
- if(Err == 99)# S. ]9 a0 h' k& [; y
- {2 _- }! r3 p) v' g _
- if(Left == 1)- Y! x- |% e/ u, j. [, k% ~
- {/ y- W/ q9 L8 a: \# ]3 I! I
- Speed -= 5;
3 S \ g4 r* D" K0 \ - moto(0,Speed);
2 N8 k* X* ^5 h7 Z. P! s. t - do
4 j1 F! Z( A5 H# E1 |9 x/ C - {: @) x1 z4 \: d9 m
- Err = getErr();
& c& j# t0 u) k+ \; j - }while((D1+D6) == 0);
/ ^. P' [! B# K$ p- X - }( r( C2 S. M* l1 g4 O
- else: L2 G% z4 n, j0 R. b7 }3 ^
- {0 H( |. E/ n+ W, b0 r% j. N
- Speed -= 5;
% N& A: @. ^0 | - moto(Speed,0); $ |$ W: ~. q" |7 b1 W
- do
# M0 u+ S& o0 e - {7 L* K( D0 H! D( ]9 A5 Q4 A
- Err = getErr();8 f$ b+ R$ c$ a" P& N
- }while((D1+D6) == 0);
# A" e: D5 z4 O( d - }
% u1 d4 L. Q3 d S1 X - }% H0 D {7 f& O( e! Z; w* X
- else
$ F) F* N1 w, v, ^4 l: E8 _ - {3 P. A0 |% `: D( f* t
- if((Speed < 160) & (Err < 2)) Speed+=1; * n8 H3 l1 U9 u; j' @
- if((Speed > 100) & (Err > 2)) Speed-=2;
* R8 G% j6 m/ p; n( X# d: p - integral = integral + Err;$ n9 h# _8 `+ `* q) ~, u7 l
- derivative = Err - previous_error;& l9 i0 u% \; t5 c! P* F2 H
- output = Kp*Err + Ki*integral + Kd*derivative;5 g( @/ i0 w( l0 l' }0 ^. a
- moto(int(Speed-output),int(Speed+output));+ E% R, @- n& R7 Z* m
- previous_error = Err;
& s0 d! P' R# d# Z9 \) Z- R7 J - }; _- I* z4 |& C j2 h B
- }while(!((lightsensor_2.read()) < (10)));
. a; B q0 i+ D1 t% D! D* E - moto(0,0);) a5 }2 y, w- v: q( i6 y
- delay(500);
% d/ f5 K; V2 ~4 Y0 z( A - buzzer.tone(262, 500);& H7 ] g. L- T2 d
- } ?# n! o% t0 h, N0 R& q
& x, n1 o% u; q# a! I4 a2 z- int getErr()9 f; M% F4 ?; C2 ~ V# q
- {
- B+ p- k0 t" d8 e- F; { - sData = linefollower.getValue();
6 |" p {/ @& L2 K - D1 = ~(sData>>0)&1;
0 Q; i2 L. ]5 X' d6 v3 _ - D2 = ~(sData>>1)&1;
- X* k4 d1 Q; V- k& U* j+ ~# H' O9 E - D3 = ~(sData>>2)&1;6 v1 W- T, r+ c2 M
- D4 = ~(sData>>3)&1;
% X3 A- T9 J! ]4 r3 N8 d: ^ - D5 = ~(sData>>4)&1;$ i/ ~$ m' p; N& N
- D6 = ~(sData>>5)&1;1 k- h) j' i2 X) t+ J
- int downD = D1+D2+D3+D4+D5+D6;
3 g& m" O: o- h* p - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
, M4 a% N: ^/ ?% P1 @# N% A - if(downD == 0)" ]$ Z$ g# k* e2 c
- {
, `* p# E! Z- L" ~* q" B) _ - return 99;* _/ E9 g9 N. p$ T$ E |
- }1 M6 C) M) P( T5 P3 B
- else1 c. ] e! N1 a. ~9 b, W; i t
- {
7 p: P6 V$ |. |7 K" T% ?! H - return upD/downD;- |' O: ~. Y, ?! A& f
- }
$ `) D7 \, y+ L7 F( f5 h) b5 } - }- g0 a8 T2 | v& e
- ; Z' w/ e, B+ G. j* F
- void moto(int Speed_L,int Speed_R)
: j" L, p- }/ ?9 T+ O& J7 g - {4 E$ Y* D4 k. v3 c4 j; ~
- Encoder_2.setMotorPwm(Speed_L);" [. O N- e' C9 `6 Z2 h* c: B
- Encoder_1.setMotorPwm(-Speed_R);
8 k7 D! E+ H5 L3 c' r! { - }
複製代碼 $ H& H) ^4 N& Q* Q: H
MeLineFollowerArray.cpp
4 m! T% |$ y! o. O- #include "MeLineFollowerArray.h"( d1 E' @' |* T, S
- + s) v9 j, c/ H/ I- L+ ]
- #ifdef ME_PORT_DEFINED6 [% y6 ?- ?2 p. c* w9 ?: X
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)* U# G: A5 N3 {7 ]3 s9 a
- {2 c2 o$ N& S/ e6 y6 t
- ! r& K# R8 z, g i' F, ]
- }5 s8 L; O. {* F' e" `3 ]% Z
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
3 n' U6 D8 q. E M) O - {" N. ]) @% G/ {
- _DataPin = mePort[port].s2;. J/ o4 ]" _$ P7 C1 N0 l5 u/ @2 @
- pinMode(_DataPin, OUTPUT);
4 N- G2 _+ R! u$ C; ?7 H - digitalWrite(_DataPin, HIGH);
; V0 n* Y+ ^5 O+ f - }
4 l, M- E9 ?* D9 C; O K - #else // ME_PORT_DEFINED
4 j+ k$ C% r# V3 j - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)1 k7 ]& X( H2 [ Z0 Q4 b; x. _& j7 k
- {7 W' n, \9 l( \' N. I) v+ h
- _DataPin = pin;1 D: V/ F2 J) K$ A3 ?0 H) b7 a5 _- U+ V
- pinMode(_DataPin, OUTPUT); " \, E- O# ?4 C3 W6 c H) R
- digitalWrite(_DataPin, HIGH);
1 C. P% `, a( a* {; J - }+ Q: j$ k0 I+ ~1 U2 z
- #endif // ME_PORT_DEFINED
7 V6 r) x0 |& L4 i
Y7 ^2 w, T* \- I( h) V2 ?
* }. V. U0 x: P9 ?- A- void MeLineFollowerArray::setpin(uint8_t pin)" u4 \/ H. q" X0 A
- {
3 U6 ^1 L. I1 W3 m' g4 a7 P1 Q - _DataPin = pin;
* {8 q, @4 O" {# `+ @# o) f - pinMode(_DataPin, OUTPUT);
" z& U% r+ H/ K h% Z5 z - digitalWrite(_DataPin, HIGH);: w/ W( U# A( L% z/ o2 P
4 e" h, E* p6 @$ p* l$ t# [! s* }- #ifdef ME_PORT_DEFINED
) x6 A+ v# |- N) @0 W6 u/ X) D - s2 = pin;
% J; i- W: F! X1 E: L+ b1 I: G - #endif) ?1 Z0 Y& N. J) E" I
- }
; y; J3 w2 \% c3 L# v$ F% I - " N- [4 P- U: D) q7 m; o8 M O. d
- uint8_t MeLineFollowerArray::getValue()
: C2 B8 {* ^6 K, N! L4 K! O - {$ M) I6 K+ H: ^, i. e3 c
- uint32_t LOW_level_read_time;
- x4 K& g* y3 c" O - uint32_t HIGH_level_read_time;
: Z1 t" }8 J0 U( U5 ^3 ]1 S& u1 Q - uint32_t time_out_flag;) F6 B6 @. X; i8 |$ V& O
- uint8_t Sensor_Data[3];
# ]+ ]- I2 E' n - static uint8_t old_data = 0xff;) J) n# X6 u) Z, |: c4 k/ M
7 y. Z5 R# J& X/ G- pinMode(_DataPin, OUTPUT);5 h1 b: l' o" c: }
- digitalWrite(_DataPin, LOW);# l# ^. i2 d/ G1 s4 `: T1 O! o
- delayMicroseconds(980);2 ?9 h4 s6 ]+ z4 A0 u" K' b9 Z
- digitalWrite(_DataPin, HIGH);1 g( r/ `1 S9 Z. `$ ?
- 0 x: [' I% Y4 P% z W b5 \( \
- pinMode(_DataPin, INPUT_PULLUP);
1 j1 y8 q5 ^* i% O. M) C! u1 y4 \ - delayMicroseconds(10);: H- h' G# y& C6 X' F
- % y5 {" n9 Z$ M
- time_out_flag = millis();5 m" c: }2 ~0 F
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );5 T2 F/ a0 x" w: `' E2 ~! Y
- & y/ t* g" F( L+ f
- LOW_level_read_time = micros();
- n* V8 J& \2 z7 b7 z - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out9 t% N5 ~, W2 I" N! Z$ \
- {6 U# z1 c4 v# G
- return 0xff; k6 i, a6 {3 z$ B1 Q4 k
- }
# |+ \/ w+ F3 m9 V- g- v: y - / v' @6 |7 D) w6 V2 E
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
/ s, Y! y* d- W- v) I$ ^2 N
8 }9 y( L* c$ r, D7 y2 |8 i7 v! G- HIGH_level_read_time = micros();
& {% W" e8 l" F- i+ w/ @# X - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level9 E. z; m7 C" S. R2 a! `2 f
6 {( ?0 A# p& o' S1 C- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out; t8 L- P# R: F5 w
- {) n, `. A; J3 T4 X+ A6 a: g
- return 0xff;
* |- i5 X$ b- s; v- {3 m6 t - }
. e z. k& z4 k& H1 Y
, `- V& Q! n8 I+ O* P7 b- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
% B8 V+ R2 }" v6 h1 A - {) ^1 g1 d9 j; ~
- return 0xff;6 r+ V: K: H7 p {9 {! [& g
- }
6 d/ F5 Q7 J: E5 Q( c
- ]; P& t; K6 a3 q2 ]- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
, U- f4 A' R% \ - LOW_level_read_time = micros();
* D ~( C. g" c* y7 `1 F; l) H - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
9 N; W9 \: w$ r" Y' U+ M7 ` x
9 M: Y& T# Q) `) X5 {, G& q7 f5 Y6 \- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
* z5 H1 U$ z5 _# d: k% v2 ^7 o - {/ H# p6 i+ `' U
- return 0xff;; z+ Q; E# a, y/ ^ I! Z5 K
- }
3 F5 b' m+ [, B1 T - 2 \$ T7 ~9 S8 k1 W2 S6 ^
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))% }1 b; |- ~" ?) K8 v5 E+ {& Q, @( C
- {
! S, R% t5 M3 E3 q3 Q9 ` - return 0xff;' `4 j# n& J9 \6 \
- }1 ?! T- x7 C" O* T8 U
, B, o' D. E9 _2 Y3 S# Q- for(uint8_t k=0; k<3; k++)
! Y8 [+ t$ {4 k+ g9 {( ? - {
" x* m0 Q5 r; `# m0 ]) z - Sensor_Data[k] = 0x00;. v+ O, g. P$ c0 J6 f
- e8 Z5 L( O! K4 a* I4 L- for(uint8_t i=0;i<8;i++)$ @; H' O) |3 k V0 L% y' N6 u! F
- {
7 I, M6 i1 w6 B' ~$ [3 R - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level; P0 M) h p3 \3 q6 w: r5 j) V
- HIGH_level_read_time = micros();
& I+ i9 u8 }5 ], z - LOW_level_read_time = micros() - LOW_level_read_time;
" T9 o9 n, b/ W% `& S - # h3 W s3 h4 k# I- i. O
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
$ ]/ T$ E! _0 {7 {% [ - {- V: o2 b# G8 }, P# f
- return 0xff;! n9 d& j5 h1 O- l+ c3 w0 j
- }
# E2 K# y% t% D7 J9 k$ ^% h7 V - 7 u+ b& M+ } q9 X- T, ^) N$ g% m
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( H$ V* N! m' c - LOW_level_read_time = micros();, G4 ]' M- ^0 t0 r$ @8 ]3 ]
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
0 j: }) D' H& s! Z( \2 ^$ z
8 h) \2 A9 m* A& ^, O b, C- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
+ `9 V; e5 K4 c* D - {/ N7 w% d7 m9 x8 f
- Sensor_Data[k] |= (0x80 >> i);$ I+ L! w: [! ~% O
- }
; t% s5 Q8 P: F( V5 H5 l/ z - else if(HIGH_level_read_time >= 100)8 r0 |* K6 T. O. r( M* f' x
- {
6 N7 q2 f+ Q/ s3 Z4 b i u3 [ - return 0xff;. U+ j0 d9 T+ K1 a- \
- }
9 ^6 h; A0 i1 g) s" v: p* E - 1 ^/ I/ u2 o7 d8 B4 z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
+ l8 z$ k; C2 r+ g1 Q - {3 d2 {9 ~9 @6 E& x# T
- return 0xff;
9 g0 N6 ]2 y6 h. I9 u- \ - }: E) W9 h! p) j2 k. ^0 C: o% M
- }
5 | U, }# }9 |! d# U% {2 P - }' f6 k; @3 W* R# J6 D
- 3 l; A |" N* W8 V! A; j: s/ A
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level' z+ F1 y+ |* o% L0 x
- HIGH_level_read_time = micros();5 ?( @3 e" l+ ^: L" M
- LOW_level_read_time = micros() - LOW_level_read_time;' _3 w( E) e) h# s7 {3 T. x# a1 x
$ B. ^* y+ Z: c. H4 W- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
* l7 w* Y/ u6 R9 I& a* R6 A9 X - {. [/ w: L3 M1 w8 }
- return 0xff;$ {% a+ }; }( P
- }
~! U. L; S8 j; n - & T; H6 s+ e2 T9 M0 X2 I
- pinMode(_DataPin, OUTPUT);9 G1 I6 a5 ] ]( u) a5 f3 e
- digitalWrite(_DataPin, HIGH);
' O1 B7 h! Z- `& n
5 n6 L" f6 n4 @; F2 W. y0 W/ [- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))0 B" N. h e' o
- {% [" j5 Z0 F& l7 I+ O+ T* Q# n; P% f
- old_data = Sensor_Data[0];4 w/ N; K& z8 R% A- `9 N" d2 @6 n
- return Sensor_Data[0];
& S5 u7 f r/ l0 _2 r7 N - }. W) `: q; y# d
- else6 w$ F5 `# L, w; ^! B& ?
- {6 r& e! Y' @' ^- d$ C
- return old_data;
2 |: y3 i+ y( E9 V0 ^! X! a - }/ O+ L1 k) a+ H1 M/ `
- }+ ]; @8 k9 m) k" u1 \2 ~: \/ ~
複製代碼
% Z- ]# l- W- d1 @8 v, }MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
* s8 A: f! K! q j! ~6 B
- C2 u1 p9 n% c& f
|
|