|
|
8 y4 W- A+ v( B/ E$ o
- #include <Arduino.h>
. W% b+ W% q% x/ G& z - #include <MeAuriga.h>: \/ J9 ~7 @5 d2 W2 l8 i0 s
- #include "MeLineFollowerArray.h"2 m4 O9 c, ^* s5 ~7 U7 O
- 1 f* _& S, k4 K* u& h
- MeEncoderOnBoard Encoder_1(SLOT1);
u# h* V2 C# q/ b - MeEncoderOnBoard Encoder_2(SLOT2);
3 w, d# z9 ]) M9 s1 ?& m0 |7 I - MeLightSensor lightsensor_1(12);
6 g$ y1 n. P9 `4 y; z) q, i9 X1 A - MeLightSensor lightsensor_2(11);; A( \5 ?- Y4 c
- MeBuzzer buzzer;! b3 v/ I0 W# J) E) s! S( r
- MeLineFollowerArray linefollower(PORT_6);# J& ], N' J$ D% x3 K. c
- ; I4 n; o# G9 V. @
- #define Error1 1# v V! Y& A" L+ k) Q2 A5 I
- #define Error2 26 e$ r- L# [. y
- #define Error3 3
( h% n/ F3 R& ?- X - 8 T% A1 `2 ]( y- p% Y& T
- #define Kp 15
8 @, n. A1 ~# {, O! V - #define Ki 0.157 f. `* P V, s& b, r% H
- #define Kd 0.03
6 a6 r9 N- P, n, B7 i$ `
: E$ v8 W6 i( _* w" J \3 i% N- uint8_t sData;
0 D1 K3 c) O1 |# F# k" \ - uint8_t D1;0 _7 C' p( |; @' H+ {
- uint8_t D2;
; r; C+ X8 v# t - uint8_t D3;. @- {7 s/ s+ y' G8 U
- uint8_t D4;
& Z% p+ N6 i% X$ ~8 V8 a1 } - uint8_t D5;: R! P( H4 ]; Z) y* l
- uint8_t D6;$ n# |4 _6 ?" S+ ~1 l) B z: g# R
) K1 H* u% D; k% v$ q- float previous_error = 0;
' f- \' H) e" @4 b# {2 w( P" a - float integral = 0;
6 j! K2 ~: b: z0 I/ N# @3 R - float derivative = 0;
0 N1 c# B' d. T - int Speed = 160;4 e3 u0 M$ Z, B% d& ^8 } k
- float output;
* h* U0 ^0 o& R/ {% o
& T2 X! ^3 T& q' O+ C4 [- byte Left;
' K1 i [7 j; }4 R0 f- N m" E - : x/ ]' q2 d0 J3 u+ E2 A/ J
- void setup() 4 d# e! {$ O; s8 J/ P& N4 o
- {4 V! [8 p, l$ b5 C7 F
- //Set PWM 8KHz3 m" T! m; m6 \% ~5 D8 H
- TCCR1A = _BV(WGM10);
% C+ [- B* C i8 F: u. o - TCCR1B = _BV(CS11) | _BV(WGM12);, E1 h9 J; Q$ M# C+ i
- TCCR2A = _BV(WGM21) | _BV(WGM20);4 s0 u4 d8 X% u6 n% }5 S
- TCCR2B = _BV(CS21);. U8 ^, L0 j( e2 I8 z1 f
- Serial.begin(9600);
7 y$ M l* ]5 v/ w/ Z$ }4 ?1 x' r - buzzer.setpin(45);1 A' }1 H" D( F% Z9 c3 }
- }
9 F0 c1 @* X8 {# e+ p: Y0 \4 g6 y - 3 T! Z- e/ {% c9 m8 O8 ^- w: }$ i" v
- void loop()
+ A+ I# w" y$ p5 d5 \9 `% w( Q - {9 C, V s& k- I
- while(!((lightsensor_1.read()) < (10)));
0 X" Q' d) z& b; v" U5 u - buzzer.tone(1047, 500);) o4 V+ z) {& `9 g9 M& \/ n
- delay(500);7 `/ l% Q3 z7 z# A; |* j
- do
9 q5 n" H: o0 B) \" ?$ f - {" }) c& P! |% e2 b% ~) T
- int Err = getErr();3 ?/ U( q1 J" F& H# z7 c
- if(D1 == 1)
: e0 W0 e3 b# R - {
9 m+ F! F `4 A9 I Y4 O - Left = 1;
' J& e- N2 k3 `! ~+ G! N - }7 \' z* @7 c. i+ X
- if(D6 == 1)# A& s: n4 E) j$ t! N" o& b
- {
+ W. D# I& L% r- A# V7 B' u/ B - Left = 0;
% t# k! z& G5 @' H5 v7 ~ - }
8 u8 F7 g4 r/ _# ^1 j+ N$ | - if(Err == 99); ?2 {" ` x, p! }2 }5 k7 U7 F
- {2 u/ W2 o; Z7 r9 k( c% X, a) g6 i
- if(Left == 1)
: w) S) P( F5 o$ p1 y/ } - {, W4 n( H4 x8 J _4 t, @( P& d
- Speed -= 5;! p( t/ E* i5 B$ l v' m. k. d
- moto(0,Speed);, h' s8 w9 e5 s; S9 [. a
- do
6 g9 f" Q' h+ c& K- Z* e - {: |0 w/ |; i9 ], _ N0 \: z
- Err = getErr();+ w* f9 v1 u7 _( L8 D" i$ U' s
- }while((D1+D6) == 0);' v+ e) p1 G/ D4 g7 L4 A( L' y J
- } C, U! c; ^& k5 v1 O
- else
1 Z% N& y7 G2 H0 ?% Q: [ - { k) u/ w' d n; M& F) _, x
- Speed -= 5;! k6 F; e9 V8 f7 N+ f) X
- moto(Speed,0); ' I, V) z: A7 d% F& d2 V$ C% I
- do2 y H% n t! s
- {1 E# |: N V" g) N/ T
- Err = getErr();& ^7 k8 c& C' q# z! W8 N6 |. Y
- }while((D1+D6) == 0);
& n6 O0 g: ?) ^ D3 s - }8 d% w2 u7 p N7 ?+ B
- }
& ^3 @7 z2 `# u$ D - else
' S) w {* o$ k4 k - {
6 E% Z6 l: U! L, d4 l% ~* C - if((Speed < 160) & (Err < 2)) Speed+=1;
, w" w( N9 }' B" Z9 E& U - if((Speed > 100) & (Err > 2)) Speed-=2;
, y" l. N- i- i2 p7 \: u& f6 x1 U3 ^ - integral = integral + Err;
* {) H5 _/ |9 h i! F1 J! \ - derivative = Err - previous_error;5 J0 U( g o: d
- output = Kp*Err + Ki*integral + Kd*derivative;
0 y+ ?9 _8 i% q0 _: }4 I$ ? - moto(int(Speed-output),int(Speed+output));
9 z5 S3 t2 _& R. X& M/ a - previous_error = Err;. q( ?9 L8 p. q% N7 V2 S( U% p9 q( t
- }( c1 m# ?/ z, y) q+ B( I8 q0 A. V
- }while(!((lightsensor_2.read()) < (10)));
5 ?/ K4 j, q4 K S8 r - moto(0,0);4 f, j \6 K5 E0 A
- delay(500);
' G+ ?8 t: ~/ R/ S - buzzer.tone(262, 500);) \0 x1 _( ^7 c! m d
- }
/ J" v9 W6 ]2 O4 K. |8 I2 t2 N- Y8 L - 5 r8 U8 M/ @5 b: J
- int getErr(), j b. J8 R, c# P. I+ B
- {
8 ? p: {# ?+ f/ _1 {+ m+ B* k - sData = linefollower.getValue();0 ?- u0 _# y1 x
- D1 = ~(sData>>0)&1;7 l0 @1 B% b0 j, F7 ~( p2 a
- D2 = ~(sData>>1)&1;( K! X+ h2 F7 ?) t
- D3 = ~(sData>>2)&1;
* V+ i/ V5 b' n: s1 { - D4 = ~(sData>>3)&1;" D7 t ^) a) o9 @
- D5 = ~(sData>>4)&1;# C! k; u+ c3 T g9 F
- D6 = ~(sData>>5)&1;
2 m4 C/ r& _# A- Q& T; T - int downD = D1+D2+D3+D4+D5+D6;' P/ X$ D' g6 V) \
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);- E0 h$ f8 E$ C: Y6 G
- if(downD == 0)5 Z/ B# t' J1 X
- {7 _, _5 K) `2 T9 e ^
- return 99;
* }6 t' F9 h8 T$ A, J - }
8 b' A* {. P( g& n - else! j6 S+ G J/ K' S
- {4 Z' [; L* b1 L6 A8 b- S- r
- return upD/downD;
7 w9 w( ?5 E& x6 i1 | - }+ [" [9 r9 M1 d
- }
j* ?9 N: j; l) f
, D, v" P4 D9 s1 z- | n) H; H- void moto(int Speed_L,int Speed_R)9 k' N/ c. J8 j/ K& Y- B
- {
0 d5 y. z4 \/ ^ - Encoder_2.setMotorPwm(Speed_L);
* H+ K4 p( s; P6 A9 u* ? - Encoder_1.setMotorPwm(-Speed_R);
, V6 s0 c0 E, B - }
複製代碼 1 V% M6 {! H6 ~$ O. o. x
MeLineFollowerArray.cpp8 t9 j$ @$ I0 L+ J4 E* R
- #include "MeLineFollowerArray.h"+ A l3 O4 }, U' y1 V
' W# V, O+ R# Y8 {- #ifdef ME_PORT_DEFINED
- _- {, b" L2 T* _/ i/ j% f, p - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
h$ V& x5 W9 @7 y1 l8 O - {) } B5 d! [6 H1 }. O3 l+ ?" p3 L
- . H# z" s* X8 C. V6 J
- }- y& q) N; Q, @ `/ R* _
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)( @% e f8 X& `: P7 u k. z. y
- {: @7 y+ I0 r* C( P% A9 L
- _DataPin = mePort[port].s2;
7 L$ ^6 u6 x) A! h% M' s( {# D& `4 \ - pinMode(_DataPin, OUTPUT);
1 `( @: N' V5 Z6 c1 V% |/ x% l - digitalWrite(_DataPin, HIGH);5 g1 [! O* r( K0 [
- }
+ w, u. n- @1 a - #else // ME_PORT_DEFINED3 f5 W; ^8 u) B" g& l
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)+ T3 t" F! I3 m0 I4 T7 o
- {
" D8 M% W. `' {5 g, n - _DataPin = pin;
6 Y; q. { m9 t2 B# S% X: E - pinMode(_DataPin, OUTPUT);
+ |' g( Q1 `5 J( Z4 ]$ f - digitalWrite(_DataPin, HIGH);
# |& H$ G+ c; c5 A; D - }
- v3 ^* @ }2 b& x' r - #endif // ME_PORT_DEFINED z0 I% G3 f, |! W
- 8 M' |) K$ m9 z1 F" Y/ ]2 u' \
0 U8 k, A! I! L2 _$ j- void MeLineFollowerArray::setpin(uint8_t pin)* D! C, @9 h# e3 `' y# ~
- {" T/ _% c4 l7 Y& t `. }
- _DataPin = pin;. E: f( f& Q4 S# y& v5 v, u! X
- pinMode(_DataPin, OUTPUT);
' V3 V8 \: t: c - digitalWrite(_DataPin, HIGH); F4 H$ E7 j7 |1 m1 u
& W- n1 M0 ~1 j' d1 c0 i- #ifdef ME_PORT_DEFINED/ q' f( D) F/ G4 m
- s2 = pin;! g5 y5 `( ~/ Y- W
- #endif+ k* h. L* m. X- t: }* }4 k: s4 Z
- }
7 E0 j/ C3 Y) q* W
) ^" O5 v4 [7 A, y& D- uint8_t MeLineFollowerArray::getValue()
1 A+ Y; c: F" [: K% O7 p - {
) h$ `3 ^ j. R0 t# S# B& M - uint32_t LOW_level_read_time;
4 y( d8 A5 { V+ Y - uint32_t HIGH_level_read_time;
7 M- K6 `5 Z3 W! s$ r - uint32_t time_out_flag;. ^: n% z s! K) p: x! D- i
- uint8_t Sensor_Data[3];3 E) Z& T& y" `. P& _5 c
- static uint8_t old_data = 0xff;- p2 g+ Y5 {: s# f3 Q
, t+ A4 `- `( k- pinMode(_DataPin, OUTPUT);0 x: d& P* q4 b G
- digitalWrite(_DataPin, LOW);6 h/ Y+ i" m; X0 ]( ]* K+ _
- delayMicroseconds(980);
- Z9 l) W" I$ m* h# f% t. z# ^# M - digitalWrite(_DataPin, HIGH);( H( S% F; i5 y z, I& l0 n
& f) x9 t" K$ _- pinMode(_DataPin, INPUT_PULLUP);7 r* |# g: L4 d- o
- delayMicroseconds(10);4 _: m) C( \' ?! i
- ; i1 v6 N' N% q8 C9 |
- time_out_flag = millis();5 Y3 R( ]7 Q$ c0 e2 p" f' _- Q# K4 B
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" I. _9 l; x# V1 g! j$ D
1 h( h* E6 k- l. n# S5 N- LOW_level_read_time = micros();: |' R5 O) G8 _5 k7 D% A
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
% v: E- w* V8 d - {% f8 R- Y1 r3 k) {
- return 0xff;$ F5 j4 b' w8 M0 c: h# W
- }
: m/ `: j/ D6 V) D6 z: e0 d; e" ~
% J% S4 x; a' y# F7 s) s( t% Y- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 A7 A9 f3 W w" ?' }
9 e7 z0 x/ r' v& W" q+ f- HIGH_level_read_time = micros();
9 P2 y6 W4 T ~+ {& ? - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
, l. p1 C5 _( A) p
" }: C8 a+ P6 I5 c- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
' N# g; w# z, }8 F2 Y4 D3 C i s - {
, L# x6 b' Z1 ^. c/ o - return 0xff;, ~% ~; y1 J8 [# D3 W* s" ~
- }
" t' E0 ^* H& y% ? - ]$ Z* U- x% b2 l8 Z
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))$ c* D5 {; t0 D) B% q
- {
! c3 p; _3 n1 O1 M1 } - return 0xff;! U b: l9 t9 i M
- }
: ~% H9 j+ |- W - 7 E$ `) I* U* C, F! _# i
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; X# L) O' x+ H$ [ - LOW_level_read_time = micros();( a7 I. S; N( K& L. S# U! @& L+ S
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level" m/ p( n9 C0 x* f \; o! M: ?- H. \
- 5 v2 Z' w6 J! X$ Z: |
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
$ Q; z6 w2 a( x/ Z+ n - {
# c% l; u$ m- Y8 F - return 0xff;
- K) P9 ?9 m ~$ _8 u: R9 K - }
6 N# D6 ?& r6 u
- A2 `+ x4 z% n- B& x0 n- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
: d! @4 Z) M1 Z+ u& R1 f$ r - {
: y* ~. x: C' R - return 0xff;0 r; _" @% R" C
- }0 U/ B5 r9 _) K
- : q4 U( [1 s6 N2 Y2 G8 I+ Z
- for(uint8_t k=0; k<3; k++)
. z/ o4 f' g, P - {# G( v6 \8 ^* G6 ^' m: u7 i! `6 B
- Sensor_Data[k] = 0x00;8 k) }& l8 M: ~! G7 u1 s; x
- , G: l M4 M4 W T' o$ Z$ b8 n
- for(uint8_t i=0;i<8;i++)5 @! V5 m" A8 C
- {
) L% T" }' q7 d6 h# s - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level- h; D; m& C' c8 C
- HIGH_level_read_time = micros();
- {! n+ k$ _$ M- s - LOW_level_read_time = micros() - LOW_level_read_time;' J! d# N. ]) X( Z, R$ q! G1 s
' |2 ~( Q" b& R' P- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )2 s2 V$ N& i1 Z/ h; N: \! g
- { H8 Y, i( i. ]. f- R, v
- return 0xff;9 T5 g& a% u2 Y5 A
- }
7 H7 L& r. u2 O" j - ' ^4 e- G! w- ^; S+ k3 n9 V
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 K' P I" S% P+ |8 Z" Q% E" H. T - LOW_level_read_time = micros();! T, v/ r; d$ o
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
" o8 r5 D1 F: N2 a4 r3 P
6 u( x# I! g) k' R) ~9 A8 m- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1$ @) ~$ Z; ~" L: J. Y8 a. W
- {9 t" m( ^( z+ u% E; u6 k" z& S/ T
- Sensor_Data[k] |= (0x80 >> i);
' n+ v' d; {( M - } x" V1 q" W% c/ e) ]% k
- else if(HIGH_level_read_time >= 100)) g$ Z) m' K+ C ~$ c8 G8 {$ e
- {) {0 D+ @, u& J2 [. @ ~
- return 0xff;
* \- X( N# b1 r/ \7 @. R9 ]% d - }
0 b2 i2 ]+ ^# A - & S; c' ^: k' C& T @* M" \7 v0 s
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)4 n6 v5 F& g2 X4 O
- {! G: v2 p- E' J! Z0 n4 k8 K
- return 0xff;
/ X1 G6 j; }8 w3 x2 ^; c2 c - }: t" ~' D7 `. O, B$ @
- }4 e/ `' X: J) N6 q
- }
# K! ~% B+ N8 W! i - 6 Z* G5 [' ~( i* L6 F8 i
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level8 {- C8 y3 f4 f5 N- R4 m9 a
- HIGH_level_read_time = micros();6 s6 G4 I/ {' A- S- i. |6 e; A( T$ e
- LOW_level_read_time = micros() - LOW_level_read_time;
/ s, @0 [ ?4 M# R
3 l* g: V# i) w& r" a- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )# H$ b% A8 i0 r8 Z3 G$ J
- {8 d7 P4 M" H) w; X/ G8 H1 Q0 ]- y
- return 0xff;
" O* D+ X ^8 c8 ] - }
* b4 x. `! C: n% j: _) v2 ^+ i - n% {2 W) O. v# s) N1 Y
- pinMode(_DataPin, OUTPUT);
. E; ]; D8 a# l- H+ k, y - digitalWrite(_DataPin, HIGH);
+ P5 M* e6 L2 d( d: W9 U+ g) b7 ~
* ]( \7 @5 C2 t Z- ~; A3 _- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))5 ^. n/ }$ _' r# T. c4 u
- {6 _* v4 g/ s S' Q
- old_data = Sensor_Data[0];* }' j' w6 [( T0 [; N$ A* Z# c
- return Sensor_Data[0];
* c& V a+ H! w% N - }
' p" S/ Q# o3 _ - else
' A [+ _! S" ~. k% z - {! d5 b; q3 [" g
- return old_data;3 ~7 k( m1 ^1 U% {# a
- }: S/ g: k) h$ ?0 j! i$ J; ~
- }7 X2 X) I) O) @! V, V& B" G ~% ~
複製代碼
: h0 {4 @( w2 y5 {- H- GMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
, k/ B }; `) @0 k& Y" e( h3 o/ `/ d
|
|