|
8 n+ Q V6 S$ H
- #include <Arduino.h>
0 p" n8 L; I% N0 z b - #include <MeAuriga.h>8 ^+ _; g) i9 N$ l
- #include "MeLineFollowerArray.h"
" R# p* x( ~) f% N - # Y; e+ P8 j/ F1 s# n
- MeEncoderOnBoard Encoder_1(SLOT1);
# T4 _/ U9 l% z0 p o - MeEncoderOnBoard Encoder_2(SLOT2);4 @4 [7 ?1 Z* c, C
- MeLightSensor lightsensor_1(12);& n: q7 I8 g2 o7 {* w* a- a
- MeLightSensor lightsensor_2(11);
4 w* E( e8 ?1 `9 o% u+ {/ V( g - MeBuzzer buzzer;
% E; S1 S# D( G4 ?6 P - MeLineFollowerArray linefollower(PORT_6);
9 k6 h/ n& D& h0 B* N: J4 }( [
" a7 I) L4 J" }, j+ R7 D8 `5 f- N- #define Error1 1
8 @, g% ]; d, t9 a( I7 t - #define Error2 23 L* }- t& S% F- H7 g
- #define Error3 32 A: B/ g4 T! }& {
; i J" e& |+ X; N% G4 {- #define Kp 15
1 t$ t# E2 Z' O* V! V - #define Ki 0.15
1 W5 q. X7 N% c$ t5 X - #define Kd 0.038 `, d$ C# _. p
- " g I m5 y: |2 R; k c
- uint8_t sData;
: R0 j- u" N) @/ X ?' r2 U - uint8_t D1;7 b/ P4 ]4 R0 `: A8 S
- uint8_t D2;
0 _" v$ r+ z: b+ j z - uint8_t D3;
9 z$ v$ i/ o, ] - uint8_t D4;
/ D/ x5 E# R" y! d8 e - uint8_t D5;
0 W! R: P$ p$ K% T - uint8_t D6;6 b, }% L" r& a
- ' r) a5 ~* V' }( @# U" B$ d
- float previous_error = 0;) L' ~5 y4 ^( `( z
- float integral = 0;
: i0 q; j: d2 n# o# R& B - float derivative = 0;/ ?6 m+ a" f K6 c
- int Speed = 160;$ l7 ^6 I2 L/ c8 B" f; b( A- F7 c
- float output;% ? m- o9 ?9 N. a4 r5 Z" L- u
$ H( K' y& d. r7 g$ l- byte Left;
$ o& |1 ?6 R3 o" p8 e
0 b- D% V4 T8 S+ `- void setup() ; I S; r6 M q2 C$ D: e6 m( D/ ]
- {
$ u6 o* I9 P5 w - //Set PWM 8KHz, h+ Y. W3 Z6 R
- TCCR1A = _BV(WGM10);6 y! U, Q0 Y. e5 u+ ?
- TCCR1B = _BV(CS11) | _BV(WGM12);
E& B, y) m: E# m3 W1 K# K - TCCR2A = _BV(WGM21) | _BV(WGM20);
: C2 R2 r* W. W5 e6 w$ f( Q$ G. ] - TCCR2B = _BV(CS21);9 |) h R8 \9 |, p. e1 x
- Serial.begin(9600);# |; b. C v* Q9 n3 v
- buzzer.setpin(45);& z2 i3 B! D3 Z9 F. t6 G
- }. k& \' u2 p/ N: w) M
7 r$ _: n$ T# a2 {6 @. R" E- void loop()
) L9 E% {0 v& \# [ - {
# l' b& m# x V% Z7 M - while(!((lightsensor_1.read()) < (10)));$ S1 u. G/ a( T+ J" F& h: k
- buzzer.tone(1047, 500);
( ?2 H6 d! }2 l3 L' B ~; Z - delay(500);
u( C5 p2 e3 ]9 t6 B5 l - do$ G" d1 n/ P, T
- {1 k8 P* `" t9 _5 W4 l- q, `. s6 h
- int Err = getErr();& G% ]5 H9 e. [" m, _' R
- if(D1 == 1)
+ Y2 S" T& n1 a# X# E Q3 G# G - {( y$ A# c% n' m! `
- Left = 1;
+ u U( u9 b/ ?' S6 W" A, V - }3 n! v% e: |7 n0 X0 u8 v
- if(D6 == 1)
/ v4 H& y" \' Q9 i$ `9 X - {) l! s( ~; I9 v
- Left = 0;
. Y. V1 Q; M7 k# A+ D+ o! Q& ? - }
, Z8 t# q- @8 U* G - if(Err == 99)
. y6 p' b( x- z: g - {7 ^( \+ I4 _) S; ~8 {0 B
- if(Left == 1)& {) f& a3 A W, K _# [( I
- {7 q) F+ u% i# o
- Speed -= 5;( m4 K" h- n3 f$ @
- moto(0,Speed);: w" s- `3 R% v' u) I& J
- do
9 O9 B7 u6 E0 i% Z0 k - {8 `' {' \7 Z2 `/ J
- Err = getErr();7 @" K8 ^5 R( ~+ R# t4 E8 h
- }while((D1+D6) == 0);. c V$ k$ G6 `
- }' t" i' s" S+ d* P" q3 v7 R7 K
- else8 W0 g& S# j8 D" g" a5 _
- {
: b; `- L) ~1 e, e - Speed -= 5;. P! m% \% C: f, S: [5 w2 I
- moto(Speed,0);
. k) |. R% G" G - do" ^ ~$ o& ]; L9 r" f" H5 o
- {
: i; f4 R9 d; i% ]0 P( {3 V* ~ - Err = getErr();; p- r. W6 ^7 G/ E* n4 r+ K* o2 z
- }while((D1+D6) == 0); w& _3 B0 B+ ^6 e
- }
& D/ b5 F* p, y( }. l! l - }
) A$ k) J5 I) c' ` T2 R - else1 w8 k- Y" T1 I$ D# P
- {
4 Y! z0 Z! z. W8 C! C - if((Speed < 160) & (Err < 2)) Speed+=1;
7 I" t7 ]: v0 E- T' q2 V T3 b* M - if((Speed > 100) & (Err > 2)) Speed-=2; 4 r' M2 \( Q- t2 @5 r+ k0 y, c, W
- integral = integral + Err;
( \ d& U$ _: W - derivative = Err - previous_error;
9 P; e7 [4 g& p1 { - output = Kp*Err + Ki*integral + Kd*derivative;
3 X3 A. n6 B4 M6 C* s! a2 q/ _ - moto(int(Speed-output),int(Speed+output));
5 N/ }0 P! M+ k( |* x - previous_error = Err;
/ O& [1 U7 `2 Y - }1 `% |$ L" I( p, R7 } C% I
- }while(!((lightsensor_2.read()) < (10)));
, x$ l S: L- @9 j7 n - moto(0,0);3 K5 R% L" X) q, K
- delay(500); N. `) S# F) _9 C K8 ?
- buzzer.tone(262, 500);# j* t& a7 U/ i" X
- }* r- c# \5 _6 x4 E3 \
% {; m$ h `$ K' U3 x- int getErr()! d+ @* W1 _2 N6 u% M- W
- {
/ A7 ]8 g! w ~$ p; v# d0 B6 x( @ - sData = linefollower.getValue();7 V/ |* m1 p' I
- D1 = ~(sData>>0)&1;
7 S! B5 f7 W1 v( X - D2 = ~(sData>>1)&1;
% l& N; V0 O( [$ w0 M; A# k - D3 = ~(sData>>2)&1;
7 S0 \6 W: `. [ ^: v' Q P9 N - D4 = ~(sData>>3)&1;4 D4 v( x; P, Q6 p/ b* F+ m
- D5 = ~(sData>>4)&1;
. [; C. d i+ E' f - D6 = ~(sData>>5)&1;3 Z5 O p; A1 T$ @0 b j% m! @1 z
- int downD = D1+D2+D3+D4+D5+D6;
& z5 m4 R: U% q! a' X0 G - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);( u, [9 ]' |, G& V
- if(downD == 0), ?0 M( Q! o* b: `* p2 `$ }
- {) Z9 x5 s/ d& e1 K
- return 99;
' @" T! f" F: G7 `# f5 M - }
) ~9 r, Q8 N$ Z$ }, u - else: B, q1 x8 c* x
- {
d# x1 W7 _& X$ O3 H - return upD/downD;! M; c' h7 P( z/ i" g) p1 K
- }' U, y6 N) ~: y7 F' J, F+ C5 M; a
- }
- z1 l/ f9 U% D - ' N, L4 p5 ^) L: i) F6 m4 S ~
- void moto(int Speed_L,int Speed_R)
+ d- \7 q o% f" T# @5 |5 F - {
3 {, Z/ X0 M9 z1 U2 h - Encoder_2.setMotorPwm(Speed_L);% \2 G" X: `; \" L! d4 J
- Encoder_1.setMotorPwm(-Speed_R);
. o# _% F, W: ? - }
複製代碼 & O8 \% V# P& Y- ?, P4 g+ R
MeLineFollowerArray.cpp; r7 H/ i: Q+ F) S, ~8 G( s7 k W
- #include "MeLineFollowerArray.h"( u6 y& C: o* K3 }* i, e8 r. {
- " [- x% S* f% y, }, |, Q" L* \9 i
- #ifdef ME_PORT_DEFINED
% h7 A+ d) x! y& ? - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
$ L1 U, b$ z/ l, Q% n7 N - {4 C+ C' A% r$ [ A- @
- 2 l7 t7 H K6 o2 v; u1 A4 c, |
- }& ^! H% V% ?* R# m9 x
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
8 b& Q7 Y* l$ F+ d6 f& {" A - {# p/ E/ ^8 p8 `( w* {! N9 P5 x
- _DataPin = mePort[port].s2;; v* V3 G' m0 c( u' o$ N7 i
- pinMode(_DataPin, OUTPUT);
, d9 A9 P7 l/ o) N8 h% x - digitalWrite(_DataPin, HIGH); m* ^" h% P9 Y: g4 E7 O, h4 I
- }
% M- t/ ~& R, e3 {0 W - #else // ME_PORT_DEFINED2 k8 _, {& a) T, h2 i
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
p# P8 A; F2 y& }( z. O9 j5 M8 |' S. [ - {
; W. {6 I7 B8 _" }0 g) i - _DataPin = pin;
* B$ o/ r" M8 S - pinMode(_DataPin, OUTPUT);
" a3 ^2 u0 S7 y2 [. x) B - digitalWrite(_DataPin, HIGH);
4 x4 k& Y+ Q2 x# y$ E# K - }
: ]4 x& W- Q8 l0 ^ - #endif // ME_PORT_DEFINED, E$ }& Y' M5 z! v" M* ~+ M. |
- 7 ~8 h9 s( c/ a7 `: U; N' |
- 7 f3 V3 w4 K" k( A- O
- void MeLineFollowerArray::setpin(uint8_t pin)
! o9 m4 r: G2 t# a5 i - {+ q* m9 O4 Y8 c7 R% ^% u3 k- p
- _DataPin = pin;
% e$ i' w% B1 @7 x3 ^ - pinMode(_DataPin, OUTPUT); . E7 r( D9 v, Y6 M* Z. R5 m
- digitalWrite(_DataPin, HIGH);
3 h; K) j! l9 j2 q2 U* u
" j8 z/ [5 j; n! l& @% B, B7 R- #ifdef ME_PORT_DEFINED
5 l& ]% |' L5 a3 d( A, c - s2 = pin;% Q- p, ? O$ I) @$ M( }. k. B
- #endif* o7 t c9 r, F( ^
- }
* m) F9 @% k, d3 X. H o
, t9 u! g# v R+ W. ^" X- uint8_t MeLineFollowerArray::getValue()
% S9 Y1 _& ]( I; W$ a! q: r - {
/ X9 u9 G G0 e* q$ v* D0 ^5 ~ - uint32_t LOW_level_read_time;
8 s; }, H6 V1 z0 A' b - uint32_t HIGH_level_read_time;
3 Y+ y/ b* c( D4 C# y/ T - uint32_t time_out_flag;/ [4 ^! G+ m" w. O3 r7 l
- uint8_t Sensor_Data[3];
) a- [2 f" w6 F: w3 d - static uint8_t old_data = 0xff;
! u& P* k% g+ H, [/ A0 V
6 C" u) o8 }) b+ B1 Q' W& k) Z- pinMode(_DataPin, OUTPUT);
* h: k% C" V3 o( E - digitalWrite(_DataPin, LOW);, F3 {, H+ K; w# Q2 G$ Q* S
- delayMicroseconds(980);
' Y. r2 c& k1 W; {) ^( L2 v - digitalWrite(_DataPin, HIGH);
- L. b' R. v: K( r
8 m6 @+ U2 E9 @% l- pinMode(_DataPin, INPUT_PULLUP);
6 e. o- ]6 g/ c - delayMicroseconds(10);
5 _- T) P* ?7 j1 j' e - ( p) @" ?3 ]0 }+ P) z! E
- time_out_flag = millis();
3 k, d+ W. v$ L. |2 z - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ {2 u" U9 A/ ]. W& j
- F% R% ^+ Z3 C% p- LOW_level_read_time = micros();
+ V" d. q, ], _/ |3 Z- F4 H - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! r: O: X0 B. J& W2 `
- {0 S. h7 k' I' |1 P& r
- return 0xff;8 Y1 R- w7 x2 e* n+ X
- }
% U1 t" s* k ?# _1 I$ o6 z - ' n& f2 j% w% A" U: c; |
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); G& O7 S( r% e
/ R9 c, x+ K5 s! X: k3 E2 b" T: ^8 g- HIGH_level_read_time = micros();; ~3 x) o ^$ @$ q! T/ S! x
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
# \0 k2 J" W4 o" o, `& \2 i' Y - 9 M/ S8 a! Y q& p! h# K: M
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
' j* U2 a4 B; m5 f - {
- B6 \5 @& D4 J - return 0xff;6 t1 T# Y# l, c
- }
; |1 I0 u* p9 i: m. A - & I9 y, d6 p% m
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
6 a& C- N5 R! A- H6 `, g: `) c( L - {
4 f1 [1 X# u/ y |" j, k( ` - return 0xff;
2 |3 c1 r# S% e+ S6 U4 X& G9 y4 x4 } - }
. O4 N8 H$ V) Y; m' T
$ R. [' |6 ^) {. m+ ^8 V& q' l3 H- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
& l/ t2 \' h' Z2 k - LOW_level_read_time = micros();/ C4 [" q$ @ z
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
5 Y3 n4 T2 e8 G, k( N* Z - & J$ J9 l2 C3 `. T
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out, n3 _7 e/ B6 V3 f/ e/ C
- {
% w* M2 `. U( {" x - return 0xff;. D& U9 _; G/ W6 ^( N
- }+ w7 U- y+ ^3 l5 ]2 `1 T
- 2 ^7 g! Z2 F+ ^$ e& ]$ {) \
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
, i+ K7 [7 a0 ~' r+ b* L - {
* ?& y0 I3 u6 X. @2 a) g - return 0xff;* A/ R, Y( H0 K1 s2 g7 M
- }/ E, ?( s) @" v5 ]
- & T( W+ P, u9 ~' k
- for(uint8_t k=0; k<3; k++)
" f( k! \# p4 Z( B, \( M6 y8 D P - {: ?) Q5 W- D2 P- b& \7 l
- Sensor_Data[k] = 0x00;6 N' k2 ~0 i3 N0 v, ?. {
- 0 w0 i2 T& y3 i; {$ w
- for(uint8_t i=0;i<8;i++)
1 b( {3 c9 l7 K% D, a! H - {
# ?0 _6 G/ w3 |& K8 y6 K3 K ~ - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
' [8 w/ I& x" ~ - HIGH_level_read_time = micros();% H$ F3 y2 n& B7 L S7 h: A' G
- LOW_level_read_time = micros() - LOW_level_read_time;
/ s( O7 f* J1 r# q2 X - + @) z. w; F, I# N' I2 a! i3 j
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
7 v, _# D1 U/ `6 ], n" D) v% z - {
+ r- A) S* Y% P$ l - return 0xff;
3 O; X0 v$ B, F1 l L5 D' H - }: H! G- d" b/ t' u
" L [: G- E$ _' `# N3 I" ]- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* u. w# C; a3 r! e1 f& m
- LOW_level_read_time = micros();4 q) O3 H y6 v0 o6 p: p
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
+ o; ^ E! d8 X0 } - ! C" C5 E Y; K3 V. M9 R
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1" A( K; }9 o( t* v7 e6 w
- {. I9 b7 ?! o# c& {; T
- Sensor_Data[k] |= (0x80 >> i);/ z) x7 M( z) H& w* E
- }
) ^/ Z/ m# ~+ K" X: c4 y" ~ - else if(HIGH_level_read_time >= 100)
& A2 J. ~, P; U7 l V$ j Y - {1 {( ~4 I4 Q! Q$ j
- return 0xff;
" C" ]+ E6 s2 K# s) ?" s - }
- S- W( {3 Z+ J n) g) ]
( y- S) f4 T! K2 C8 O( g& |- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)/ k6 i0 w! g4 R* N4 \( E2 Z V( X
- {0 {3 T1 F! G) [8 ?+ n6 A* C" X
- return 0xff;( |& C! ^' E( k9 z$ [' ~* r, G
- }
2 K1 [+ o+ A$ r- ~$ a7 D$ S, a. ? - }
8 |8 O, e; d0 C( P - }
1 v0 |6 W$ X O( V' k' U5 m" d - 9 Q. p; {% F4 A& C' R; z4 \
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
. C I% F3 Q9 [9 l- b - HIGH_level_read_time = micros();
3 i* h+ [5 A( V" G+ S, B/ h. X/ X# ~ - LOW_level_read_time = micros() - LOW_level_read_time;
' X0 M/ F7 }7 p
% r( q: M( H6 E! s2 X& n7 O- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
" C7 }- P8 w2 K1 F! x. A - {
! t% l# L; y8 T; j- D - return 0xff;
( G/ \9 O3 ?0 {- i6 c3 J" P - }
% H+ E& O/ B5 z8 H7 m: {0 R - ; m8 E& z; {& e4 t5 o
- pinMode(_DataPin, OUTPUT);; M% N' l" O5 A, A
- digitalWrite(_DataPin, HIGH);
+ V w+ k% _: _4 ^+ [ - 9 o( @: f- _$ R; T/ Z7 F! n
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
0 f" ]& ^# }' F$ B6 d: ~# |! y - {. i7 r9 q0 r! ]* h( N( D3 \
- old_data = Sensor_Data[0];" W- e9 [6 p4 ^5 D( U, G
- return Sensor_Data[0];# j& H; J) j) K1 H% x* L" H' K& V
- }
1 j# S* r0 I! A7 {& Y! t9 E - else
: g2 x# N4 u: d- L - {3 W( T, m3 `- c
- return old_data;) A) g4 e6 |6 S' z; `3 b) }$ p
- }
7 s+ V3 l- ~$ K( p - }4 C$ {1 r) \3 ?6 i$ m4 a( h" U" u
複製代碼 ; a1 N, r* y+ U( I: P( g
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
! ^- L" i1 ?- R; O4 [: E2 l7 V9 @6 _ J1 q) k" K* o/ M; y
|
|