|
|
% c+ u7 J, V4 X4 c1 W: K- #include <Arduino.h>4 b6 [ g6 o8 @ o5 B: n
- #include <MeAuriga.h>
# c, G- c) N4 G0 V - #include "MeLineFollowerArray.h" j- Q. V, q" T9 x
2 H M2 e* k0 u' }- MeEncoderOnBoard Encoder_1(SLOT1);2 A8 S S* [( G: E2 \
- MeEncoderOnBoard Encoder_2(SLOT2);# f* t% N1 a. s G, [
- MeLightSensor lightsensor_1(12);
9 R9 m7 t- V( t9 J - MeLightSensor lightsensor_2(11);
' T" e4 r% B% V; X, } - MeBuzzer buzzer;* e ^( Z% j v0 L" G& F
- MeLineFollowerArray linefollower(PORT_6);
4 S9 E2 m) u( v }6 x1 ^
1 @1 M1 F# i( Q6 m. N- #define Error1 1
$ N9 ]* n/ U& X; L) B+ ~ - #define Error2 26 _# H# h% z# }$ `
- #define Error3 34 \! g# J( b( i6 _# z
7 R# J0 `$ B+ c8 ]: X- #define Kp 15& g. N v& ^4 h5 y3 U
- #define Ki 0.15* H, g* u6 \. x9 t/ i
- #define Kd 0.03 B+ h' ?' [7 D r% O/ ^; T
, y9 ?; F- e, N6 W0 e- uint8_t sData;( y% a8 W3 ~, \$ O! c( L
- uint8_t D1;
$ V/ x' ~6 F! w- ^ - uint8_t D2;; o) M* L& V: W
- uint8_t D3;
4 R [; D2 D; N& Y - uint8_t D4;( \# P3 [8 g- T/ W) X8 C
- uint8_t D5;# B. z) \& F7 \' `
- uint8_t D6;
0 H% q" ~( ~* [- v0 B5 O/ b
% f2 \+ k) J8 h6 W- float previous_error = 0;
, [7 ?- A7 U4 J+ N+ n - float integral = 0;
( m1 x5 b& h/ Q9 D6 @ f - float derivative = 0;8 l. O0 j6 x) g5 a7 V
- int Speed = 160;
* u9 h! Q: Q& g! J - float output;
9 Y; r$ w v- \+ Z* S
% B6 f( F |) ~5 {, e- byte Left;
, N+ M6 {+ o8 ~: |
; S. |1 e* K5 N+ A- void setup() 6 Y; T* w6 D2 |5 P& o3 d* ?8 m! m
- {0 d4 z/ m( ]+ b9 K1 e* ~: V5 J* S: I
- //Set PWM 8KHz
( m8 a% Z1 K, P# R1 y - TCCR1A = _BV(WGM10);
% G0 N1 \6 X4 A3 `' F - TCCR1B = _BV(CS11) | _BV(WGM12);& C0 g- K6 R% v3 x
- TCCR2A = _BV(WGM21) | _BV(WGM20);/ @% C1 ^# g( o0 g* \
- TCCR2B = _BV(CS21);) n' Z0 Q0 F9 q' C+ k
- Serial.begin(9600);
3 P: ?7 X+ d% w* ~; R - buzzer.setpin(45);& C* e/ B& z! z1 H' u$ R
- }! k4 {0 r! E& S: [# b& \8 ~/ G
- 3 q! S$ h' o! L5 O! H" C0 D
- void loop()9 c, |! o. k& U- }+ ^$ i
- {
7 H( Q' v- d2 X# b, N, t, z - while(!((lightsensor_1.read()) < (10)));
, P$ N& @& Q% o( j - buzzer.tone(1047, 500);
9 l1 ^/ ?/ b* @0 `* t b - delay(500);
- ]1 {4 G1 m, e u - do: ~ l$ Y5 M! t: z: J
- {/ {- s$ J% \& O3 S
- int Err = getErr();
- O0 g7 x$ c( O, \) s, c% L - if(D1 == 1)1 u/ T8 n/ n6 o- m4 V: y! V
- {
+ o/ J: i/ l r4 n& o" \0 \4 ] - Left = 1; p. S7 R1 C3 F1 l y) O( ~
- }3 ~2 |8 b: s+ e) F/ r
- if(D6 == 1)
/ {5 u* h8 q$ y- }6 n - {- B# I0 L! K1 v4 M1 h- j* C2 M+ ~
- Left = 0;7 |" r( S' \! w
- }/ F3 d1 ~' c" ?! Y7 _+ x
- if(Err == 99)8 f" e) J2 U1 @3 s- p# k
- {! X4 Q" `8 T2 h7 _/ o
- if(Left == 1)
h3 [0 s( d/ ?# o - {9 J8 ]& T3 E. Y/ b* k/ e# ]
- Speed -= 5;5 n! ^$ O) ]5 V% S- _
- moto(0,Speed);
- L* M; e) Z" l" U0 G - do/ z* @" p" H. T7 n" x0 |; k
- {6 R' {, @- T3 Q! F+ R! ]
- Err = getErr();# j* M2 [8 ?6 }6 U5 O1 E0 a
- }while((D1+D6) == 0);# \" E) H; I) b+ u: W3 s R N9 u
- }- S% z, ~% D' G0 u
- else6 f+ d9 |7 B: c7 W
- {
& }) b+ q G2 f - Speed -= 5;3 B0 v; x% Y- E2 ]4 W( F( u" c9 `, _) ?- @
- moto(Speed,0);
( ]! V: H. U: E2 |# M - do
# |1 P( e2 B! C1 j - {
x3 h# ^4 n& h' @- H& H - Err = getErr();6 e9 ?; G# {+ n) G/ @$ J: p' ~
- }while((D1+D6) == 0);
# P i- u/ x1 R# w# N' P- E. q - }
# q. B4 j% F3 M7 Q# P - }& ]& P" F& _* {8 x2 p7 `
- else
- U# F' C$ ^6 [' t/ A - {$ p/ l0 {4 X2 V& ~
- if((Speed < 160) & (Err < 2)) Speed+=1; 0 q: _& r4 n2 |% D2 l
- if((Speed > 100) & (Err > 2)) Speed-=2;
0 I: X, d6 c" ~2 } - integral = integral + Err;
' |# y0 t* h. b0 X - derivative = Err - previous_error;
" _4 o$ ^# B6 t0 D/ ?/ ] - output = Kp*Err + Ki*integral + Kd*derivative;8 G; E6 A- q' q7 r. e
- moto(int(Speed-output),int(Speed+output));% A6 w" p2 U) [6 ~2 V1 S
- previous_error = Err;& R3 ^7 _" O# Q1 c- c5 ^. |
- }. e( i( C: C: k4 Q0 Q$ W4 r7 q: S6 M
- }while(!((lightsensor_2.read()) < (10)));
- |5 m* [+ D1 T: }) v& o, K - moto(0,0);/ M) `* U: ^0 w& \4 f4 Y# ~" r
- delay(500);; L" z- V* \1 _: I5 E- C2 [: A
- buzzer.tone(262, 500);: M9 v' O+ x3 W6 [3 X" Q
- }' J/ {+ p A/ O$ P# P, p1 M
- 5 y: G1 P! M j1 y# U
- int getErr()
8 a# B6 y9 ~/ m; s) m- Z - {
, E0 R, Y/ R5 W9 U. z - sData = linefollower.getValue();
) }' H8 S2 e9 |6 ?; a% r- E4 M/ l - D1 = ~(sData>>0)&1;& l, o$ T/ b( V. m. C
- D2 = ~(sData>>1)&1;0 i& N& H3 A: j; t+ ]- L& T$ a2 r) ]
- D3 = ~(sData>>2)&1;
' R" R9 L/ q4 v" }* E' c - D4 = ~(sData>>3)&1;
+ o* q3 ^: k) N0 I4 i - D5 = ~(sData>>4)&1;1 w! r. L% W# P2 e/ c
- D6 = ~(sData>>5)&1;: E3 S7 u: N' y! y& {
- int downD = D1+D2+D3+D4+D5+D6;
3 y$ w; }6 n* M' i2 a - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);! _* v# K5 Z4 b( K) @/ S s
- if(downD == 0)* i. g% C- F, ^" n( [# f2 B
- {$ k8 B. p% J. X+ U+ l
- return 99;
, _0 W. A" @6 m# I, l! m - }
/ e6 ~ v2 B- X0 i6 Z4 Z9 F9 j - else. C1 w. Y1 |: J5 G- `; H' |
- {% U! J3 ?( ^! v7 A8 k% l- B
- return upD/downD;5 x$ F$ O1 ~5 }3 }6 ]0 o% Q2 c! N
- }
' F) X* p+ z- [, A4 p; \' I+ M7 Q - }
8 E; G0 a6 x6 B" o2 n4 \1 D - 0 Q5 S7 e) p+ E* h: m t) ]( w% G
- void moto(int Speed_L,int Speed_R)
/ s. A' G/ x. ]/ u& V& U6 ?% v7 L - {
. o, S7 y" [' B# o" j; P - Encoder_2.setMotorPwm(Speed_L);
' B( `) C8 I' {0 l8 c - Encoder_1.setMotorPwm(-Speed_R);
+ C: k: f0 X8 M1 `: j1 n - }
複製代碼 * O* u3 |3 h! T7 Y
MeLineFollowerArray.cpp/ E+ `/ q" \7 A8 E9 Q# W/ ?
- #include "MeLineFollowerArray.h"8 `4 J6 q) `6 o! \. b5 l
- & G" N/ G9 ]+ }" c. `& |
- #ifdef ME_PORT_DEFINED5 w; h; `2 U4 | i% b
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
) L* ^3 z5 x- c$ J( D: U - {
1 l8 ~) d. Y7 ~: M. p
( c6 y+ H: k% F( {- }6 K& N* ~7 _3 k! ~ F
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)( v; L" v3 ^: C4 a) L# p5 w- G
- {- O( {3 T, ?6 _* p) N/ K4 p2 S6 ~
- _DataPin = mePort[port].s2;# r+ ~! J, l `% h
- pinMode(_DataPin, OUTPUT); 9 m- F1 X+ [. K% ^; m- q4 B! D
- digitalWrite(_DataPin, HIGH);. C: C l1 e7 c7 {
- }- }) t3 k& p" A! F% }* }" N; p& V
- #else // ME_PORT_DEFINED1 k, ]; K; N! H' M* [! W- C
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
/ f. @# v1 ^ Y' Y; I- u$ e. e& Y - {3 Z% v3 e* c6 l- A, j: C) L
- _DataPin = pin;
: F# r: f) s9 s9 [' _6 j - pinMode(_DataPin, OUTPUT);
$ B4 L9 ^. ]& J( \6 e - digitalWrite(_DataPin, HIGH);0 _" A! E# I7 G1 d) w7 ^
- }
, f! r. A! G8 A! d- [$ Y - #endif // ME_PORT_DEFINED( ~" a7 N; M3 m9 L. ^' R
- * a. o. m, E8 s& j
5 I# H( O9 _# N2 B: W- void MeLineFollowerArray::setpin(uint8_t pin)+ o, r8 m9 d( \& C3 y, o- }1 I
- {6 B c4 P2 |0 |7 Z8 l _
- _DataPin = pin;
6 r) i3 \7 q+ I. Z9 [ - pinMode(_DataPin, OUTPUT);
! z. H" y* k0 U+ N3 p - digitalWrite(_DataPin, HIGH);$ l3 W* C2 f/ I' y
- - B& \% ^7 D- b
- #ifdef ME_PORT_DEFINED0 g; g& d; t+ D. |) _; }
- s2 = pin;/ o& l7 Y6 G9 y& N& M
- #endif
, g4 F( u. h" ^7 D1 m. J - }/ [" G2 U. s6 c r' Y ]+ Z
- : J* d+ u4 c0 M. A# L
- uint8_t MeLineFollowerArray::getValue()- j! ^5 y( M, ] \
- {" O v7 q6 Z# i S# j6 j
- uint32_t LOW_level_read_time;
% r" s' O6 P" U - uint32_t HIGH_level_read_time;3 h& w K p) G! e5 M5 P i
- uint32_t time_out_flag;
/ \9 M8 a+ t& U6 q3 q - uint8_t Sensor_Data[3];
) E1 E/ f f' r7 d/ d - static uint8_t old_data = 0xff;, Y% }- ?3 s% x; V0 S; X( v
- " C$ |7 {" I" @+ A3 T: I2 f1 _
- pinMode(_DataPin, OUTPUT);( ?, D3 R( Y/ Z+ s3 h. }
- digitalWrite(_DataPin, LOW);
: ]" U: \. a, |8 P/ J% z/ K5 f - delayMicroseconds(980);
. d8 @' }2 U2 c4 B( Z- z. v - digitalWrite(_DataPin, HIGH);
]0 v" }* F' i& ^. C - ) r( C" |& J9 g8 A
- pinMode(_DataPin, INPUT_PULLUP);1 [' @ e& @2 l1 n$ X X+ j
- delayMicroseconds(10);, ^4 m' B- Q) }4 i# r7 v' E) z0 B
) O5 o& o0 Z/ _/ M& Q& L- time_out_flag = millis();8 \% P7 e$ M8 X& g9 U8 A6 G. e
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );# q* R; D& N/ Z9 U; b0 G
/ c- e6 L# C$ U, X0 {- LOW_level_read_time = micros();
* g/ e& m' k7 A - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out# u$ t( a/ I3 q3 G# U- F8 x, O, |* b
- {( u( P2 m( N# Y+ l
- return 0xff;
' h! [% s0 j; [5 U: C# ^ - }
; ?$ Z9 @, R, I! k; F7 u5 j& p
- c9 G& k) G, `: E1 ?! O2 e: E- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );; L0 Q; f" k- ~7 H2 r+ I2 P
8 w+ u5 r/ d+ b- HIGH_level_read_time = micros();
. N W% v+ N; j8 T& P C& {! u( {, S - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
; Y; H$ r9 F$ L# F, Z5 D - + H6 c' x) f* C7 D
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
9 D& m8 W" R. B P4 p. T - {9 I5 n8 C0 y/ [! l9 N
- return 0xff;
8 j; b0 }" f! H6 q - } R1 M+ ]/ \% E5 O
- 0 i5 b$ w* P0 I! d- w8 `
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
; u/ r7 {& Y! L - {4 A: h! q1 Z1 \4 e# k% \
- return 0xff;1 x( Y2 M% _$ Y2 G
- }* f, B6 j. g, T7 H
1 H5 z7 d8 k& T6 N. P. { z' n- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
8 A2 i+ ? b9 D9 p7 J - LOW_level_read_time = micros();
R' E, {$ m% ]. v0 W3 @ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
$ m: w8 @5 P1 [# N \/ R; b/ z - / C! s# P' Y! h+ R
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
- o: ^: _5 w2 V% \! ^4 r$ l$ S - {
4 f+ G7 U* ~1 E# R4 n4 w. ~3 u - return 0xff;
% F* i2 _3 t' e4 m) _ - }
# e: ]8 H) F1 ^& n/ D- V' u1 C - - p; p* }$ \' u- e' f- Y
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
4 S9 S* a! x& s$ A - {4 ^8 d4 W' v6 ]3 j& \+ c2 ^- D
- return 0xff;0 ?3 i, t. b8 }# I) L8 ` c
- }
) w2 d' O4 c& A - % Q* N, W! S! }1 n
- for(uint8_t k=0; k<3; k++)
- F- g/ F3 M7 B- ]; E8 J - {
& G1 h3 ]5 { _3 B }: n - Sensor_Data[k] = 0x00;
! i( o# m' {7 Q: d+ K. T
$ n0 A' K( U2 }. ], Q- for(uint8_t i=0;i<8;i++)
; x7 r+ _3 [% \3 ? - {6 B P1 @% L# Z# t, c# u
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level |7 `) [8 P0 N, }1 q
- HIGH_level_read_time = micros();# V" { a) k, @& E& ?
- LOW_level_read_time = micros() - LOW_level_read_time;& d, e8 b6 ]; g, m7 d. X) O7 }0 U
- 4 L# P* N Y3 I5 Q, f& t! k
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )9 d; _% }9 M- r+ G$ T
- {
. `1 T& ^3 ?: y! D: o7 } - return 0xff;
- l6 ^) q7 ~6 [# \- Y; [ - }3 O J$ v( E3 M) H, S/ \
- 1 R/ U6 {2 ^9 ^" n) @5 V" r8 n
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# |' S. T `0 U# j! y; h- Z - LOW_level_read_time = micros();
1 T9 A* k1 d8 ^) i7 M5 T- l7 v - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level- j q: u {5 p" R+ I* q9 b
% ?/ ]# B' b) N% R- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
" i7 h6 P- x% E& ]7 N' U* k$ ]. R - {0 |2 b' }9 S9 |% k9 s( X0 X, L3 N
- Sensor_Data[k] |= (0x80 >> i);/ l% t: w( b) q' F4 `1 n) ?
- }5 W5 J- i+ U8 u. q+ u1 i* ~% Z
- else if(HIGH_level_read_time >= 100)
; I' }4 W, t8 s, k Z" c- d - {
0 I1 F2 b- i/ m+ w - return 0xff;$ u% c V5 ?, u. S' B' @8 S
- }/ M4 u% C) n5 X# [
( E. U- ~' o1 s. d( C4 v- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)7 r9 I+ Y i; B& D! R. a
- {
* c1 h+ S* L; V5 p/ `4 g - return 0xff;
( W1 I/ R) V% n; A. a - }
H( S6 A4 k1 s8 t0 d8 M - }3 I9 L: H% e# H4 s u1 h+ U5 h2 G
- }
' J% c' n2 K# {$ p$ n2 u" I' t - 7 g4 e8 e: ]! u) C" P
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
% F- J) T4 L2 q9 _! t1 L2 G( l - HIGH_level_read_time = micros();. H; H9 Q' D+ D( i( U3 R
- LOW_level_read_time = micros() - LOW_level_read_time;
" J( S7 K/ ]+ R, G: y& C
) V( k. t& ?' _- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ): [( d F* h1 t2 r
- {$ _2 Z; N) L- R+ X+ ~& j
- return 0xff;' l6 S/ A, _$ r# X' L6 H m0 T: H- ]
- }
7 W) F' }: h/ \* ~7 H
* w9 C6 A1 \+ K- pinMode(_DataPin, OUTPUT);
7 a- |+ g! O* I - digitalWrite(_DataPin, HIGH);
7 n# |# o0 e& ^/ [' t# Q - 1 _& w! z' {8 D9 s B- b
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))" a4 A' l; ?& L" S
- {9 `5 G" [( H' m' s4 s! ~
- old_data = Sensor_Data[0];
4 k \& A! B) {1 M- U: l5 |! C - return Sensor_Data[0];
% j& ^1 a* R6 P2 i - }
( W: u3 I6 t% @9 s" T U. y - else/ z' u. c# D) N1 U3 n8 J! M! z
- {' q# H1 D C3 G, Q
- return old_data;1 ]" K/ B `: n; {
- }
3 i4 J' [% S9 c8 J6 d, Z% q! @& Q - }
+ F+ d( v: g! h) w& C" K1 I
複製代碼
: ?$ c: O: V) W& d5 wMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
; A, b5 v6 c6 \+ H) u; r9 n
& }! v5 c: o6 j" t
|
|