|
|
+ Q5 Y6 l5 P. q3 ]6 \- #include <Arduino.h>) _: N6 l: i! [+ s% |; f0 o
- #include <MeAuriga.h>: i% y& p) B$ a" A" i2 g
- #include "MeLineFollowerArray.h"
: K1 S! w- \2 T0 }4 y4 a! T0 G( E
5 ]1 j' z( Y/ a" D- MeEncoderOnBoard Encoder_1(SLOT1);
$ h1 Y: u) U! s( k9 ]& d2 t8 Q - MeEncoderOnBoard Encoder_2(SLOT2);6 n) K, ^" `6 B
- MeLightSensor lightsensor_1(12);
C6 Y8 T: g0 \/ `( }9 b - MeLightSensor lightsensor_2(11);0 L' b% d8 l9 v R, m7 ^
- MeBuzzer buzzer;
; |5 A/ m5 p" E* `$ ?& f4 Z - MeLineFollowerArray linefollower(PORT_6);
) R6 f) o+ k8 G! r8 ?
* E2 a2 i+ x) F- N& w+ @- #define Error1 1
8 Q/ B; g" [: n - #define Error2 22 O2 ~( L% F3 q% f; B* K8 r' W2 R. c0 k
- #define Error3 3
% Y3 a/ G: b3 v+ d: ]; l' p! F
; `9 \5 D1 Y& R" h- {- #define Kp 154 K1 ?2 H+ a9 l: Y5 X, K1 l
- #define Ki 0.15
# n8 [" P7 U) H5 \! _2 J8 G, J% ? - #define Kd 0.03/ J B2 T/ Y0 y
- " w$ K( G5 q6 ` M/ ^+ X {
- uint8_t sData;0 [9 F$ F- P& W( [6 ]. N% S
- uint8_t D1;
4 E$ @) m' s! D, s4 [ - uint8_t D2;
1 o( R/ {. ^# W* U1 G - uint8_t D3;
# e5 T' J+ K0 u4 c- o+ W# z# e - uint8_t D4;( w5 g6 c! w& b: `7 n* k! z6 I L
- uint8_t D5;8 b! j5 U3 ^2 o& Z$ Q4 h
- uint8_t D6;
- `3 U2 E. l) h
. j) {/ C. [( [ J8 ~- float previous_error = 0;
1 N/ n2 ]. `3 ]/ b+ k7 {6 J- j% I$ {2 D - float integral = 0;: j$ W4 u, M e3 A
- float derivative = 0;
* \( v) ]0 Y! @4 g1 O4 x5 a& h6 y0 H - int Speed = 160;% A" q% y4 E' D
- float output;" V% ^# {% q! m2 a" {& x5 D
; v. A* ]6 O* Q- A- byte Left;: l) l ]; I. q( R; z$ _3 K
- & u" e; d! o0 a. q+ @, i& F/ E
- void setup() 2 L! S+ D; @ u& y6 @& H2 r' a0 L
- {5 m4 I/ a+ E' b( ^8 m* b3 }; O" O
- //Set PWM 8KHz
1 K3 s& l& E7 g0 M - TCCR1A = _BV(WGM10);
' @/ a# W8 T; g - TCCR1B = _BV(CS11) | _BV(WGM12);
2 |+ e" C, G8 ?7 _4 N' K - TCCR2A = _BV(WGM21) | _BV(WGM20);
' t7 {1 c; P% ~8 K- c0 P, r; `' a4 B6 b - TCCR2B = _BV(CS21); F1 H; v. r5 g0 U/ a: s0 W
- Serial.begin(9600);
`6 n" T- e, ~- B+ L @6 e, u9 M - buzzer.setpin(45);
* ^3 m- Z& E# f' d - }$ w! L, C! o0 e
- , H5 e8 r2 g0 M4 ?
- void loop()
9 S* |% c+ {- { - {) r2 p- j7 g. \! ?
- while(!((lightsensor_1.read()) < (10)));
% ~* B* G. Z: m2 ~3 o' E - buzzer.tone(1047, 500);
- ? `( j/ `) H - delay(500);9 F( T' z7 b; N" _7 b; w" `: v/ v n) v+ z
- do
+ J# B. j0 V5 o' `8 ~5 y - {
5 Q: l2 a3 F4 x! i2 c - int Err = getErr();
( @8 o5 P4 e, @ - if(D1 == 1)/ G% p" X/ `9 Q1 q# y) }
- {
4 u( b" g" y4 N0 [$ E& D6 e - Left = 1;
9 {( m, ^. I* k- W - }
0 X' e4 _2 [) c8 G - if(D6 == 1)
; M7 ? K g# T* `% }5 e: t0 g: w; u+ r - {- V& V: w: {9 B2 `; g' W
- Left = 0;. I S, I8 F# f1 ?2 G( |, X0 _
- }* ]8 t C) M, Y9 F8 y, U" t3 X
- if(Err == 99)7 ^0 v$ R! ?; ?9 p* M1 L
- {4 g9 q. t. Y! v( i
- if(Left == 1)* u, y) B' M S7 X$ K1 k
- {7 |9 k7 w$ O( E: |0 p' N
- Speed -= 5;
* a9 S* n8 v+ J5 T9 F) U - moto(0,Speed);
) |* ~0 z; ]$ C - do Q3 \: G9 K3 R; {7 J4 T0 I/ {
- {
- y, o+ s# e/ h- i& G9 w- r - Err = getErr();
, K# ?9 I! i: T - }while((D1+D6) == 0);% p" I2 ?& U9 d3 j. N4 z$ G
- }8 X) U0 C2 `' \ K3 ]6 h4 l
- else
# B8 A$ l/ L: V, w. Q - {! f! l+ d5 L0 c" H+ p
- Speed -= 5;, \9 n+ y# K- ?# ?# X5 C
- moto(Speed,0); , E& E* p" y$ {1 Y/ Q& N; Q: O. f
- do
3 _7 N' g/ M8 U" x, H - {
3 B& s0 M2 l3 C: g% l - Err = getErr();' j; a$ l. M) C9 r2 i' Z" |
- }while((D1+D6) == 0);
2 h8 t; c8 f0 R9 h8 }# Z - }
8 w& I) U/ T5 e - }
4 n; X% g8 F( b% E! |# O8 \6 X - else3 E5 ~* ^+ _( G9 U
- {
2 |% N8 |' v+ N! w' n2 ` E9 d( D; E - if((Speed < 160) & (Err < 2)) Speed+=1; 7 K; S/ J& l0 Y5 g0 t: f8 V
- if((Speed > 100) & (Err > 2)) Speed-=2; " }4 [% }& Z" n% |
- integral = integral + Err;! n) \# Q; Z) v1 D
- derivative = Err - previous_error;9 p$ l# Y2 f- Q- |
- output = Kp*Err + Ki*integral + Kd*derivative;
5 ^3 H3 t2 }% S: s/ ` - moto(int(Speed-output),int(Speed+output));6 G3 t+ M2 I% P2 v. S
- previous_error = Err;
i9 G) F& r8 D9 z* _6 v( }3 Y! f - }
- J+ F0 p+ e! q$ q" E - }while(!((lightsensor_2.read()) < (10)));# d) ]+ V$ a- h, B: V( g4 s
- moto(0,0);
/ }0 z& X! q/ |# r - delay(500);# m4 X- R" \* K: ^/ H; f+ P) c
- buzzer.tone(262, 500);
$ I9 a) p; t# L# @ - }9 ~9 L( X! [; t& W
- ; A+ p: P3 K8 {8 Z C" U9 M& ^4 H
- int getErr()
' `# o" w- _1 ~ - { & P& P$ _6 l. M5 X
- sData = linefollower.getValue();! y _2 C, Y. e4 h! W3 k
- D1 = ~(sData>>0)&1;
5 r& g5 b! j$ \$ d. l - D2 = ~(sData>>1)&1;. p; w4 \; P( R) }/ x) l. q
- D3 = ~(sData>>2)&1;# v' u# i! d9 M/ M) x; k! z
- D4 = ~(sData>>3)&1;& _$ x0 B, v% ]% P" y
- D5 = ~(sData>>4)&1;' j. a$ L3 {, w7 Y6 T* C- O
- D6 = ~(sData>>5)&1;
7 Y; j7 C" t- y1 \( } - int downD = D1+D2+D3+D4+D5+D6;/ p$ V' }3 I" a
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);) S3 N. k i" c' N, f- r- y: L+ q
- if(downD == 0)
" U+ M- @) q2 D: c - {* D2 g, i2 ^. H) d4 g6 W8 h
- return 99;
% L4 D2 d0 `2 X - }( t5 K" B/ f- e9 a4 u) C
- else0 w' {! ~0 n$ J
- {" U0 e) o/ t2 r* f7 Q( S
- return upD/downD;5 V8 S6 P2 w1 X
- }
8 A* Y1 }& e; o( x1 B. l. B - }
* z1 }% b3 U/ q' b) K
0 A) E. H' A- ?. h! w- void moto(int Speed_L,int Speed_R): _( f7 c( X0 p! O; s( A
- {6 B' s* C" o! g% w9 ~- w$ d5 r
- Encoder_2.setMotorPwm(Speed_L);2 g! D2 r j; W" c% M
- Encoder_1.setMotorPwm(-Speed_R);5 | p9 i1 u# K* k
- }
複製代碼
+ D8 }2 ?/ n j- Y, zMeLineFollowerArray.cpp
% k/ C7 O6 q0 L6 d; H4 `, \( W- #include "MeLineFollowerArray.h"
5 B. }7 f8 _+ s
$ a4 X) Z% x/ e4 I. E" ?- P- #ifdef ME_PORT_DEFINED
' G% v& B9 V0 M: I2 Q$ W, W: a, ^! l - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)5 K; l. }6 X7 Q- E, M* e7 }
- {
2 d, s1 L) k3 i; F6 o5 u# W( w - $ P! n; q f& U/ D/ ]) y% X$ {
- }7 n1 G3 {6 R6 C% F6 |
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
: Q% J/ y$ p9 Q& V% x# V0 | - {
4 n* W3 C$ Y- ^( Z - _DataPin = mePort[port].s2;
! U4 O: _# C3 f1 z/ F2 ~. d1 m, B - pinMode(_DataPin, OUTPUT); 1 Z3 y; O! d: a* [7 X
- digitalWrite(_DataPin, HIGH);
5 Z8 b4 z2 }7 {3 |. x - }
( w. R. S. L- J+ w, x2 p - #else // ME_PORT_DEFINED+ e, I I" m4 m" g, B8 l9 J; V
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)* S( x. p- l, A& Z
- {
0 A/ f1 y) f& w8 y/ K3 j% M - _DataPin = pin;
( ]; m2 |% e: [* `* t# l. f9 y - pinMode(_DataPin, OUTPUT); 5 P, _* e' o a& w5 W
- digitalWrite(_DataPin, HIGH);3 H5 k8 Y2 I- G; A n
- }
9 V/ _* g# v* C- D/ Q/ S" ^ - #endif // ME_PORT_DEFINED a& `! F, R R' b$ t
- . P' V) F/ j9 }6 Z
% R# a3 m3 f7 I6 [- void MeLineFollowerArray::setpin(uint8_t pin)" l0 p/ T: ]3 ^: c
- {) c7 A6 ~! ? i7 G- `
- _DataPin = pin;' E. H% o9 e( Y1 _
- pinMode(_DataPin, OUTPUT); $ q: E/ J) u! D; o" ^
- digitalWrite(_DataPin, HIGH);$ N0 w( \% `0 J2 r' x) n
- 2 q; i7 S& F, K% d4 f" I( c
- #ifdef ME_PORT_DEFINED: i, e4 o5 w! e
- s2 = pin;
+ m+ i$ h9 b" s2 I - #endif9 o! r# _& h0 g* _' ?9 [& i# N
- }
4 \( v2 n; y+ Y, I - . S5 s8 t' P5 e
- uint8_t MeLineFollowerArray::getValue()3 r. h% N; b l& o. V
- { \2 L- T4 G' J5 @; @# D. L
- uint32_t LOW_level_read_time;( e. l8 \7 _! u" R- L
- uint32_t HIGH_level_read_time;( m* h v Z- A. H$ h7 A7 Z
- uint32_t time_out_flag;
& q/ K' K( U+ d# I# x - uint8_t Sensor_Data[3];
4 J& N/ t( G' I2 K ] - static uint8_t old_data = 0xff;
+ _7 O: P! {0 i- }) K. U6 u0 e - 9 R/ L. m6 Q4 z5 ^* M. J4 J
- pinMode(_DataPin, OUTPUT);
* F7 X5 ^' O7 I - digitalWrite(_DataPin, LOW);3 e" I" g6 n1 O
- delayMicroseconds(980);
5 j+ r- w+ [3 g P4 L% l6 ~$ f5 X - digitalWrite(_DataPin, HIGH);
4 E" @6 e1 x6 u2 S" @6 A - # Z8 R+ \# `' c$ R! K+ g; ?
- pinMode(_DataPin, INPUT_PULLUP);
8 C: b- k! I' l- L+ K$ P - delayMicroseconds(10);% I0 v5 E: D9 y2 D' Y2 H
, m! `+ G1 x" J2 ]- F3 y- time_out_flag = millis();
1 `# A! ~* v% V. ` - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% d8 \1 p% F5 T - : [( d" |" ~9 W& U9 U
- LOW_level_read_time = micros();# p* d) m4 b0 Q( x2 z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out1 H4 y' K# V% J. M8 x2 w+ _
- {5 L; ^, f# L9 B4 P' @7 v
- return 0xff;
/ i, _, Z: a J3 B" K# R' l - }
& N7 G& \( Q1 l; ~1 Y. {
( o/ o( f& i7 b# Y$ f" Y2 t- K2 x- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
8 a2 J$ e% H2 U/ }2 Q4 f
9 Y. O5 D3 K1 H6 M- HIGH_level_read_time = micros();2 S" M% f% V n2 U; S/ d) k; Y4 D/ J
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
9 I0 p) K0 F& N# J) D - 2 I+ O, i4 x( W2 B8 m; Y
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out' O. X m. `: S! g8 X
- {* G/ T Y( P" _3 a6 Z( b# {" v; I
- return 0xff;
/ S. ?, G$ ]% T; S6 B - }
8 Z6 K" X4 H) F$ X: F0 ]. x% g E7 G- `
9 C3 C+ t# Q" y7 C6 U7 r3 e- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)), f7 O6 O3 |, h
- {
6 X0 @9 u$ f& } N0 {" s) ]9 ]/ J - return 0xff;. K% o( X3 \1 }& g: q; |
- }
9 c3 {' f. u4 h' m0 @
2 V+ w+ `4 b6 ]- t& V' B7 Z- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); |6 j0 f' I; l$ C+ |$ _7 f
- LOW_level_read_time = micros();6 P; h& g. c8 w- C' S8 i- ~
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
, I' F, f* D1 c2 K) n5 X" O - i' u' f1 \: t2 i& j- ?# N" [
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out$ }, F+ l1 I {0 l& @: _0 O
- {
1 Z! A6 B) t$ b. ~! o - return 0xff;
i# o6 B7 m* I D2 }7 U - }
" Z: c' L7 r8 D( U7 q0 q# o& D - 9 Z7 M0 S# V y* g) P. M. D a
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
5 w: H `5 q/ l* o# ~( h( w - {
$ k/ Z3 M4 O$ f; Q; f: G. F - return 0xff;
@; |) J" |( r$ ?3 _2 C3 W+ B# d/ { - }
0 G, I# E" c. _# }& \2 L n
% J; Z$ o! n. e0 e. j' g- for(uint8_t k=0; k<3; k++)
# n( o6 a0 v! ]7 n9 h3 } - {
( y, _1 p: b4 l9 J - Sensor_Data[k] = 0x00;; d0 x0 K: g6 a, U* A5 ?. _, Y
7 G6 N1 {# c6 P' L/ K8 Y2 V' w- for(uint8_t i=0;i<8;i++)
/ L0 d6 p& U! G - {; I( X6 ^: _* U" R' r+ y- F) k
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level, W2 P+ F- X% B" d/ {4 ~8 O
- HIGH_level_read_time = micros();
* S2 r; W; T, }, W9 w% Y2 J7 E - LOW_level_read_time = micros() - LOW_level_read_time;7 {( [# v0 i# U) a
- ( o% o/ V3 x5 }- }) v5 H
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )0 I, D2 q, ~( ^6 E9 o
- {
! v4 U, G4 l7 m( g- h3 y" y& J3 ^+ e% G - return 0xff;
/ P' z! u( R( V0 N - }
9 P2 ]1 n' F; a* o7 P5 f0 ~$ ?! L. r - ) L1 Q8 Z# r, X, C3 I
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 Q; u% Z5 l$ S% _5 d: h8 ~7 E - LOW_level_read_time = micros();/ \! R2 |" d6 _' ~! w" p! |
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level% F; e8 e% l( U
- h. q, C8 q6 J1 |- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
) \/ O8 [9 f9 w - {8 c* I! F# u- s+ \$ q
- Sensor_Data[k] |= (0x80 >> i);
8 e Y; b' O, o - }1 z% O% z8 ]3 F
- else if(HIGH_level_read_time >= 100)8 m+ j! j* a- A3 I) k% o! D2 W% H
- {
8 g2 L! k4 `1 }9 K5 U" A7 G3 ]$ A - return 0xff;& [- Z/ r8 _1 C! ?* r
- }
8 A/ p# j* M. B: G6 p2 z/ q1 d - & |/ s, ?/ Z& _. P
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
+ T% O# ]+ B& b" O - {5 I- P8 [9 C; n! n+ a" `
- return 0xff;. u* t$ x& R% ]
- }' v& V. j/ R. @7 _3 _& D
- }; u1 h$ Q# d, j. P: `3 \
- }
. F; T) [% J. R W9 T' A
" b4 v- B7 u* o. }: [% j- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
, D" x2 b \8 @7 W ?: V - HIGH_level_read_time = micros();) t. b+ G, D' v' ~/ l( O, \
- LOW_level_read_time = micros() - LOW_level_read_time;/ S9 _( `4 E% O9 K
- 7 T9 R! S' @1 p
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
+ D5 W. p ~% `2 ^1 c - {
' Y; y: w- A1 j! q B9 L) E/ I - return 0xff;: ]6 n5 ?8 R, S( r5 \% O; Y7 g
- }
* E ^8 s r: L, F/ i& y4 l - + j5 }; C: N: ]- t" [
- pinMode(_DataPin, OUTPUT);
0 f( A* e5 h7 G0 L - digitalWrite(_DataPin, HIGH);
; D, m- M3 x! B! p9 i4 p9 o
5 u9 f8 ]/ b8 F, b* {- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))5 P2 H7 y4 D9 v/ {! D- v; g8 s0 ?( J
- {% S k9 L- } \# l" Q
- old_data = Sensor_Data[0];
. Z2 A& e7 H4 [0 }1 e - return Sensor_Data[0];
# D$ `% u$ k1 t8 K' U; f+ N! W - }: q* `) B- @) d4 b8 m5 R
- else
- O9 H0 m& D, U - {5 s! x4 U% ^. i, a8 _' B
- return old_data;
& e: L5 W! v6 G# L5 } [& d) G2 X - }
9 t( W" B6 L$ V - }
5 y, _! | S+ C# z6 {/ E
複製代碼 1 z! S @9 ^8 V9 J2 ` U, h; j3 n
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
|. Q: J z) T. x
, J+ ?! b6 [- m8 H" {4 L |
|