|
|
: J/ ]" m4 @+ H. n- #include <Arduino.h>
) K0 {) Y) P9 H y7 m! } - #include <MeAuriga.h>+ `1 R" i( ]2 n& q6 ]6 ?
- #include "MeLineFollowerArray.h"
/ H3 Q) I% c8 H5 e4 p. v
( D; F2 T) G/ x* J9 l! }+ {- MeEncoderOnBoard Encoder_1(SLOT1);
' F' P% e" x) \& q: H - MeEncoderOnBoard Encoder_2(SLOT2);, P4 E: N$ u! Q& Q) {( i
- MeLightSensor lightsensor_1(12);
6 M2 Q- h9 q4 h) r0 Z' R - MeLightSensor lightsensor_2(11);
' O. }2 X) l6 a" j- k! ~ - MeBuzzer buzzer;
% m& n, O4 Q; D9 I5 _4 q - MeLineFollowerArray linefollower(PORT_6);; q, |5 Y5 g0 G/ W, [/ `
- ( ^6 B7 [4 P9 d
- #define Error1 10 _7 U4 i" m% r3 h
- #define Error2 2$ R) g* R' Q% W4 `
- #define Error3 3
I7 {$ k1 B, y- F& O% l' O8 t - - @, ]8 W& V, G# d1 b# R
- #define Kp 153 B) @( ~4 Z9 T8 c& |, c+ i( |
- #define Ki 0.15/ }4 } B3 u# R- M& b+ \; p q" R
- #define Kd 0.036 P1 h0 v9 i) i7 {4 U4 i
- + T4 u) h# h- J8 o8 A2 {4 M7 |: u
- uint8_t sData;
/ ^# y: V. e- E! ^1 V( `6 t0 v - uint8_t D1;# d3 p F. x; e" P( n/ K; T2 D2 r
- uint8_t D2;
1 {' d6 l: r* T% U; A - uint8_t D3;( g, U @+ t' \3 P5 R! ^2 ?/ X
- uint8_t D4;
: M* ~! h) {& V/ A9 T7 r - uint8_t D5;
- Q, |3 R! C" F( M% Z - uint8_t D6;7 {, B) S# h3 W, g6 X. c$ ~
' R! g/ x/ W' \6 J' V- float previous_error = 0;
. q! N" m1 w5 R) {( O' { - float integral = 0;, H5 d( o' X; m- n1 c# y# X5 a
- float derivative = 0;* Z s! R: ~% X; ` O; a' ]; a
- int Speed = 160;/ b( u, m' ]- `+ D3 i2 B
- float output;
/ b: c7 V- j- G9 g9 q
5 V+ j8 `7 X* g3 I- byte Left;
3 o+ y3 T t" e) ]% u$ o# N - 7 P6 K6 D2 R5 q% `5 v% G, p7 x
- void setup()
3 p; w. s" R# N8 T+ j/ R9 v8 b - {
# L0 R! F% g2 d5 l( t. i$ G - //Set PWM 8KHz
3 U0 T/ c% ~: @0 Y# r7 p7 W - TCCR1A = _BV(WGM10);6 @/ \* v% ^2 B h- i
- TCCR1B = _BV(CS11) | _BV(WGM12);; t6 D6 t4 a0 A: p
- TCCR2A = _BV(WGM21) | _BV(WGM20);6 a, K2 ~* i& ]# D# [9 m3 }
- TCCR2B = _BV(CS21);2 V; Q. z# [, o. l
- Serial.begin(9600);
' E4 Y& V' P- u! r7 W1 b - buzzer.setpin(45);
/ \2 s: [7 H* E% t - }
' |7 ]1 f0 b+ U+ ^: A% l - & W6 c- b3 S" I7 c" `2 g
- void loop(); H0 q3 r, n+ r; @( O
- {
$ C4 Y* ?2 \+ j - while(!((lightsensor_1.read()) < (10)));" e U8 a8 D6 \' s% H8 K2 R* ]% i
- buzzer.tone(1047, 500);
. T8 T/ G. v8 B2 L, s0 T - delay(500);$ i% v2 M* z- `9 N" S4 j, M
- do9 w, Q5 W9 R; J6 [# x8 J0 e6 C4 A: n! A
- {7 p" U8 e. T0 M S9 E! n3 |
- int Err = getErr();: x! M' S4 C6 ~7 o
- if(D1 == 1)
% [9 I z& B7 P5 g - {; Q! O m4 [6 e8 ]
- Left = 1;
W" L) W7 @: X - }
) B; Z: Z% z! u6 A - if(D6 == 1)
+ O3 O$ `* _4 M' g, l1 n* F - {
: a) D, |7 T3 h7 {' B - Left = 0;
4 m$ E% Q9 J4 ?5 {) _# d: m; S: K - }
' _6 f* a9 f# A6 R9 J/ [ G% Q' } - if(Err == 99)
3 q9 y( F+ o N3 V q - {" `; V* h" ~1 X* u
- if(Left == 1)9 H5 u' u1 }0 q9 H0 v
- {2 w$ s7 i; `5 |+ ]
- Speed -= 5;
- D1 G0 g3 _8 A" y - moto(0,Speed);
: x4 [5 T* V1 U" \7 M - do
4 c8 U0 Q5 n. S. m- j" R& E - {3 M+ h) |' h9 v
- Err = getErr();
+ v! [; U9 |5 f - }while((D1+D6) == 0);
. Y9 A" M/ m# q' j4 A" ]0 G/ V! o - }# F$ Y- a; R# R5 u! T
- else
+ t& C' L- D r) C! q5 I - {, d$ y* n- }( [3 T; u% S) ?
- Speed -= 5;
+ h% a% ]3 g9 a- X - moto(Speed,0); 7 H1 a1 ^9 T* P& Q6 |* o; ^9 a' `
- do
5 w( X0 e6 f3 W9 P! v - {4 ^& Z3 P) Z3 [, I+ o5 {, u
- Err = getErr();
; e1 q! H# V4 n - }while((D1+D6) == 0);1 T6 ^$ B( e2 ]: S: H# k: M2 I) ~
- }
0 T o* |5 h# Y m: g - }
, o3 j7 |4 n! E2 B- e$ ~- |' r - else9 I7 O( p7 Q$ q* m; r2 ]7 Q
- {4 X% d+ L) V1 ?1 `, X0 [
- if((Speed < 160) & (Err < 2)) Speed+=1;
: f6 ?* w' s8 S" O4 J - if((Speed > 100) & (Err > 2)) Speed-=2; ; |+ Y0 [+ L4 R0 f7 V! ?5 j
- integral = integral + Err;
* S8 [; I* z- ` - derivative = Err - previous_error;
# ~+ A" t8 U, F - output = Kp*Err + Ki*integral + Kd*derivative;
& i8 [( `% Q% _: ? - moto(int(Speed-output),int(Speed+output));
- h7 Z O# L. k5 m - previous_error = Err;
/ L; W" G, U+ P2 k2 U0 ?0 R( t- f - }
$ K/ W( u9 g3 Z0 B: V2 G- |: \ - }while(!((lightsensor_2.read()) < (10)));
& ^5 B Z- |. Z' Q - moto(0,0);# E' y+ L% ^* S( [. a) c
- delay(500);1 W, _; r) d% X) V
- buzzer.tone(262, 500);
; K7 P* H; C7 @9 w0 X: c - }
( n! O/ n' C0 F4 B2 {3 C
, x8 w3 [1 J- Z5 t0 G- int getErr()
" ]1 Y4 H& R ]: T% z6 @+ h2 V - { # e. {5 t3 ~1 N
- sData = linefollower.getValue();
' F% [ {7 ^: ]5 T: B- w b - D1 = ~(sData>>0)&1;
" s0 N- f6 K- A. `6 s - D2 = ~(sData>>1)&1;% @! k' q% o% M
- D3 = ~(sData>>2)&1;: j- u# @' z. L; }+ v7 |( S
- D4 = ~(sData>>3)&1;# V7 s& w. l7 n6 l
- D5 = ~(sData>>4)&1;
: T* t t# _0 z4 g0 h. k! l ] - D6 = ~(sData>>5)&1;0 g0 s; k, U: W
- int downD = D1+D2+D3+D4+D5+D6;, Y9 A1 T3 @2 p9 L, E' y
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);: X: c; }3 O' W( `9 ]9 ?
- if(downD == 0)
; c8 M8 J: |8 Y9 H/ j- [8 m - {+ {0 [/ F: h" K% B! G6 D
- return 99;7 a E! e* @* N" K% V+ ?' R! Z
- }- ^/ N5 L. D- M, L! Q% X7 m
- else1 Z$ j* f, D& c6 L! C
- {/ B j* U0 C% U9 y
- return upD/downD;
; n6 i. c6 F/ n( c - }* q/ C5 T5 t% I0 d! N
- }
$ p. L& j% b5 {2 b" O+ i) h5 b- J - : I/ k" a I0 o5 h1 U- s
- void moto(int Speed_L,int Speed_R)6 Z& Z& T! x& {. v& X2 _
- {( ]4 Z: ?( n6 @: j: G3 o1 w# `
- Encoder_2.setMotorPwm(Speed_L);
; h0 ]7 q ~1 ^* B/ ?. e M- z - Encoder_1.setMotorPwm(-Speed_R);; v4 X, H8 c* P6 W
- }
複製代碼
6 l1 G! t8 s v0 S! M0 MMeLineFollowerArray.cpp6 o4 F' t+ F- Y# y1 P% N) z' E
- #include "MeLineFollowerArray.h"& e& {1 O, c/ l7 C1 ^: {7 j
- 5 k. I1 f+ }" i% O# F! r7 G
- #ifdef ME_PORT_DEFINED* v- Q$ [- p9 m
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
2 S6 ]; u$ {7 Q$ E - {
7 x& P* ?& O1 G! ]$ l1 i0 i- t
+ K" Q) @( U. t4 }8 U& X8 O- }
- y0 M) f0 _5 S4 \ T* t8 k& \ - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
* w! o- M# B4 c; B& _ - {& j* y. x. u3 t. a( }) J
- _DataPin = mePort[port].s2;
' e% D8 S1 K0 T. c/ L8 N; Z - pinMode(_DataPin, OUTPUT);
( z; P" j# B, C; e2 q( G - digitalWrite(_DataPin, HIGH);. z. L8 Y w8 ?5 e% a. u/ o: ~
- }
! U" ]3 W6 @9 |( Y2 M - #else // ME_PORT_DEFINED# g; n6 B+ m/ y! m% N8 y" V
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
' h0 w4 ?) ?/ o/ |% f - {
$ D/ ^) f, p% s) C - _DataPin = pin;
, o0 h9 R4 P9 o; W8 E s - pinMode(_DataPin, OUTPUT);
8 \. Q! C+ t/ J- M, R4 ?+ |5 { - digitalWrite(_DataPin, HIGH);0 _/ X2 r8 ^$ b% A' [5 N
- }
: ~8 h+ e7 E' L8 O% l: m8 D - #endif // ME_PORT_DEFINED
( r$ S% X: Y8 E3 U2 @) [ - 8 K& \7 [3 Z4 ^% I% F
- " }6 y) |5 }$ T7 t6 f; o
- void MeLineFollowerArray::setpin(uint8_t pin)8 e( d: Z7 K' Q0 z" N
- { S D/ I3 J9 o2 `' v
- _DataPin = pin;
$ ]8 ]* k5 Q& }# n7 U4 x3 p - pinMode(_DataPin, OUTPUT);
3 _! M0 L# P* [' `7 O- ?! J - digitalWrite(_DataPin, HIGH);
9 P& w, R% t4 \# a$ A# K$ C - 9 L$ N5 i; x# q7 q
- #ifdef ME_PORT_DEFINED5 z( L2 @5 z& a
- s2 = pin;
% ^. S5 V( W- o7 ]$ v! f7 { - #endif
; A* ]* |% U/ E% v - }) m- A" g' k3 ~% w5 \8 o' I
& `" `$ }, g3 }2 {5 g4 X% ?5 B- uint8_t MeLineFollowerArray::getValue()
+ ^2 E [- s, C8 e - {
7 C: I2 n- d, ?2 _* B - uint32_t LOW_level_read_time;
6 Z& p1 o$ i, G, d' ? - uint32_t HIGH_level_read_time;. G# B5 S& h: q$ w
- uint32_t time_out_flag;
7 `+ f/ E! T+ |1 H" g- z - uint8_t Sensor_Data[3];' g- q; W$ P' f# m
- static uint8_t old_data = 0xff;
% o7 I- G3 M, F" B |7 b - . J* N3 U& @, e$ J
- pinMode(_DataPin, OUTPUT);
# A' c6 y8 W5 _4 Y) y9 h& j - digitalWrite(_DataPin, LOW);
9 t9 l. u$ J; n8 B! q5 J# a) U! l - delayMicroseconds(980);
" H4 m( p; M; h- f e - digitalWrite(_DataPin, HIGH);
% |& \5 S$ V/ ^. H - # m& p9 M! X9 `, e1 e! e( R/ ?
- pinMode(_DataPin, INPUT_PULLUP);
/ {/ n# i, d/ g- }* Z. j* l9 W: ^ - delayMicroseconds(10);7 }0 e8 g# e E6 H4 J4 e6 Q
3 L% Y, }8 k G9 }9 _5 w2 L- time_out_flag = millis();
& e6 G, F! K# e: n5 S. b - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 K: a; Y3 I2 E. N! T' t - 7 D7 ^" a3 w Y4 `, ^0 ~: c% i: v
- LOW_level_read_time = micros();
4 q) ~7 [ g K' |* t, G+ w - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 @% ^) c& X4 K) h0 s5 m - {
( D4 [" G. p/ y - return 0xff;( G! A, X( X; [4 V
- }
- v5 s7 }+ ^4 F8 E6 Y1 O/ e3 ?
1 P- x1 e, Y& Z% _; z: X9 j- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% O! m1 \/ D& k0 d - ' G2 T/ i5 [2 e
- HIGH_level_read_time = micros();
% T* N W* C" n; G6 y# a* Q - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
: Y$ `. E3 x2 e9 h0 p8 ? - ( L6 Q2 \- H- g2 d0 x% X; A
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out. ?# [2 [+ D5 N- L
- { p) w4 R! v4 W' S2 i7 Z$ d
- return 0xff;" R6 p5 T9 `( _. s, r) p9 }
- }. b3 b" r+ T6 I' \% K( p* R
- ) S, Q/ |; ` G* k- ]8 T
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))0 M$ I6 N4 ~! [$ t) y+ D6 ~
- {& K) N* ^; h, P4 o4 v5 j
- return 0xff;
1 K; [/ A) O \0 f; d - }( l( ~( h) y0 s1 a
- 1 i0 P1 E! R2 H3 u* N3 a
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 u+ u7 ]" b* R+ u
- LOW_level_read_time = micros();8 m8 s" F5 s9 Q- P, E. j
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level% \& ~' Y6 ~0 M3 ?) m8 p
0 n/ V& _0 ?; C! I+ d) H2 }- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out4 ]2 A: P) f" f. Z3 X( G
- {7 a$ ]. U; m" ?
- return 0xff;' h4 E/ Y5 K. j! [% A( E, Y
- }5 T0 S) A1 ~8 U- z' r3 R' f
- & F7 @: T2 M: t. \3 F8 K/ W
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))6 g1 K+ o+ T7 W' f. V# g: h# g
- {
- }2 d8 I5 Q) q8 a) N/ c5 a - return 0xff;
# l. I4 [1 x& w" Y8 l8 P - }( T8 G8 |2 Y9 K6 w
- * A8 K/ P; @8 l& @$ U& X3 R9 U
- for(uint8_t k=0; k<3; k++)
/ Y5 T i5 K9 o+ N) a" Q1 i - {( D- k- P& A1 Q, B, a3 Y0 P: T
- Sensor_Data[k] = 0x00;6 m4 K M* V& v" }
- 0 K8 o" D: c, N% A `2 H. O
- for(uint8_t i=0;i<8;i++)
) x$ y5 A, S5 d6 F/ w2 i# w - {
) D- n4 Y4 B+ x; R8 u - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level# S! [+ D8 C: K
- HIGH_level_read_time = micros();0 f4 ]# L2 S% h0 L/ s! P; g$ `
- LOW_level_read_time = micros() - LOW_level_read_time;2 M% I' Q1 s ^7 O6 A
- # O7 I1 K0 }# o0 Z
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )/ J. \% m; T w8 ]# b
- {( q3 d, _# v# x- u7 ]
- return 0xff;4 H2 y1 r+ J7 n4 ~
- }
' f# y; b0 H0 p7 ?, K; \ - " Y. C9 n1 o* o) t2 i
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- h" y( F2 E! _: a. m7 v ` - LOW_level_read_time = micros();
+ e+ W; y3 v2 f1 i2 v K - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level) M* @, \" i8 a' T/ S$ m) c
- v7 r A/ f E
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1: a _' I# G3 T- V
- {4 a0 y5 O2 n5 l. K6 W1 U0 V* L+ q
- Sensor_Data[k] |= (0x80 >> i);- U/ V2 |3 w) D V4 [$ o! U% D
- }
; ~' C. w# l% N+ ? - else if(HIGH_level_read_time >= 100)
1 E* ]' r& |/ L; b1 K3 u - {
1 G8 D9 X- F5 [! l/ `9 `5 i - return 0xff;
& J0 o# T. z$ w5 f9 q0 Q - }0 i0 ?. V e. R' D
- + }3 K$ _# g- t% g' ~
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)8 c2 j& P& k/ ^ y b
- {0 g: ^0 P9 U% G k& u( k' s
- return 0xff;
7 R* h. P V( K - }0 q* h" {+ m: @3 f6 C+ E! e
- }
, ~+ c9 k L% @7 ^& S0 u- b: ~- G - }: C4 R2 X; h; }
4 S8 X- {& g' P- Z) T- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
- Z. M( g, V+ J% `4 G4 a5 r - HIGH_level_read_time = micros(); _# L9 | y: v: z; s
- LOW_level_read_time = micros() - LOW_level_read_time;! S! A& C' X8 Z9 S; F+ l: ?9 d6 h
- 0 q x5 w; ~0 `7 c4 f% E, w; K
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
/ {$ T: o) f! H. @ - {# y# y7 m; D/ K
- return 0xff;
' W) d/ q, R1 d5 Z, F7 o, _ - }
u: I/ P5 O* u# `
m1 v+ Q/ Q7 a- pinMode(_DataPin, OUTPUT);
& m" W- D0 U% W, N, M. E5 } - digitalWrite(_DataPin, HIGH);
# X; {# E& S9 h
; B" c$ H) i. K. k* E0 ]- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
" A6 T0 Z/ O7 I) ^' X - {
& W6 n0 B I0 ], A% w - old_data = Sensor_Data[0];2 n8 j# _& L3 o* D( q) x
- return Sensor_Data[0];1 m* ]9 Z. [0 r U- m* Z0 d! A4 O
- }
5 v: g. W: s/ ^& _. \) T7 M4 u& b. ^; P - else
8 o+ \7 e2 g' U# u* o, a! k- { - {% D: L6 N* K" u6 S% M' t/ N
- return old_data;3 M! d* U: M) W& H4 T, e
- }# m: A( E9 S2 ^$ m. o/ A, |# i$ C* z
- }
5 [" f# h6 u% j# n, }: Q
複製代碼
d. ] e0 Y1 N. m# W* b. iMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
4 S7 g7 T7 \* j4 i" q( g X1 }
: ?# T, I! o7 V7 Z+ M w7 c |
|