|
|
/ F4 u; E \8 D9 M
- #include <Arduino.h>
5 @7 s5 f0 i% b P6 K* n0 g - #include <MeAuriga.h>$ X5 z. C, M5 _& ]( {
- #include "MeLineFollowerArray.h"
6 F0 k5 x/ r( N. ^4 T - 1 S, ^: A' k0 K
- MeEncoderOnBoard Encoder_1(SLOT1);5 ^/ g$ k x6 c; ?: I* H
- MeEncoderOnBoard Encoder_2(SLOT2);6 a& k1 U _5 N9 w3 u
- MeLightSensor lightsensor_1(12);
( O( A3 e) H3 m6 M& p - MeLightSensor lightsensor_2(11);
0 }' s+ i. l5 d; p% ~. D - MeBuzzer buzzer;9 @" z$ _3 }2 p3 a4 N# s9 |
- MeLineFollowerArray linefollower(PORT_6);
1 n; W) Y+ K$ Y% ]6 N% K, o0 w" N - ( j Z) n/ T( r! X* x8 z4 {
- #define Error1 1, c2 Y: C) @& Q/ c; {
- #define Error2 2
- g. Q, s" U$ `" t7 M3 |7 h, j, }1 i - #define Error3 3# ]. ~4 S7 k" s0 k" I
+ ^, A5 d. N! B( b- w/ H8 V4 t% t- #define Kp 15
+ {! P4 Q- \7 L; `0 o" i8 D) @0 { - #define Ki 0.15 H8 H* ^. u" |4 |- ^
- #define Kd 0.03
! _0 N* I' L, [. k+ p - ) Y4 |. f9 c1 l' [
- uint8_t sData;$ j& T! ~' N& Q: L3 A5 i0 v
- uint8_t D1;
* w4 g3 q0 p! e% M: j0 \ - uint8_t D2;
4 u- i9 [% x! l% r - uint8_t D3;
) S. ?8 r- @* B) A - uint8_t D4;
7 `3 ? C9 Y2 g0 p8 _! p. s2 | - uint8_t D5;
! g+ Q" j I; Z6 y* |% r) a - uint8_t D6;
. v v( ^4 q8 s/ [3 f - # t' {) M% {4 ^. \' w
- float previous_error = 0;4 N$ d) h# s- z2 B
- float integral = 0;
3 y0 o9 e! U4 Z- b) s+ Z- J - float derivative = 0;
* A+ C0 T1 t$ Z4 {7 { E - int Speed = 160;3 j; { F8 }, d( }
- float output;- o8 u" k9 q4 U) {# `, ^+ G
- ?; _3 O2 C/ D$ h6 c! `8 n- e' W
- byte Left;8 K1 b5 r! Y1 ^
- 8 E6 m, Z0 t! l7 ?% O: I. f# t
- void setup() / G& \3 e9 c$ @
- {( o' {% D9 N% o" w0 ]" }- w2 J) y/ W; r
- //Set PWM 8KHz( z- \6 p, ^% e, {4 e$ H
- TCCR1A = _BV(WGM10);* v' e; ?6 R; ]; z3 Z
- TCCR1B = _BV(CS11) | _BV(WGM12);
: m; V2 g# K/ y8 r9 K6 d3 [ - TCCR2A = _BV(WGM21) | _BV(WGM20);4 o D# C; ]* D& V, A7 C6 w- J: |
- TCCR2B = _BV(CS21);
' h( z) l5 K& k: V0 a+ D - Serial.begin(9600);
% [% K6 t$ A, y3 Q- M0 v; J" T$ |5 i2 { - buzzer.setpin(45);
* U6 ~( p- f' l1 p6 P - }6 L3 p4 V. b5 D% W
& J& N4 J. s; u- void loop()1 c# \* m: V. C3 U
- {, P4 q& v9 d% S& p/ D1 k
- while(!((lightsensor_1.read()) < (10)));7 [$ L; Y5 X! u6 b! n
- buzzer.tone(1047, 500);2 m1 Z, k" Q$ {+ c% @* o8 l# F
- delay(500);
. o; Z5 Y2 Z/ L' i3 a4 { - do
d; y( ~. g$ U9 o. v5 c+ p - { }- u9 J) n$ c! \: d
- int Err = getErr();
3 O9 G$ a# K5 a4 d* |+ D - if(D1 == 1)
" X" D1 n8 l* y x7 a - {
; _( V2 n+ ]+ @: M/ W& m8 h - Left = 1;4 O0 D4 n* ]' ~+ g% U- y* C$ C
- }* m' C) P8 T9 N4 M; f
- if(D6 == 1)# \7 N7 a/ b( i4 s( H- l
- {/ I4 E& j* z" M3 R( c p, _
- Left = 0;* r& e; N9 o- B6 g" C# U
- }" a5 @# z' O1 N; d/ {
- if(Err == 99)
( ?- \7 [/ B# O - {
" {: G0 E0 b5 y) _ - if(Left == 1)
+ A- }, n# E# X( Z' N: u8 {7 j - {
( c+ R) p! c" C9 _ - Speed -= 5;
* O7 j7 P b6 i l6 P - moto(0,Speed);. C7 L0 o3 R: G+ t
- do
6 j% u0 T: _- e3 z+ ^, C - {- ` e- H. q& L% m4 F; }! i; \
- Err = getErr();' k* \0 H2 s2 ]* A; i
- }while((D1+D6) == 0);
1 _4 v2 y' ]2 L J d - }9 L. a, ?% H7 o) o( X7 U/ N
- else
2 h$ X: E' T6 v6 B# o6 ~) m5 Z; z - {
# @! J1 n% A0 v5 N3 v v$ ^ - Speed -= 5;# L# G! `% D b u4 e
- moto(Speed,0);
! _% W. O3 X# t, f6 T - do
8 a2 ]) K% e6 ]% C' J2 [ - {$ G/ T A$ \& @
- Err = getErr();
! u; ~! \7 }% a7 L/ f/ T - }while((D1+D6) == 0);
* m8 _0 A! I# k1 s$ \$ j, p - }
6 Q/ z2 b; \' Y5 z - }
4 q/ C: X: Q! h" c! |' v y - else8 C# O: h; f2 g3 K0 @7 F
- {
M0 R/ W9 h+ b1 Y - if((Speed < 160) & (Err < 2)) Speed+=1;
w0 M' p; p; h0 y - if((Speed > 100) & (Err > 2)) Speed-=2;
! D" W, H8 u) y - integral = integral + Err;/ |' ^. w) a: T' b" J W5 i
- derivative = Err - previous_error;* h1 ~! {: d w' Z" n$ K5 ], K! A
- output = Kp*Err + Ki*integral + Kd*derivative;& V$ o! { L8 B* |7 {7 L: Z" g
- moto(int(Speed-output),int(Speed+output));
4 K' |; k' c# i! x - previous_error = Err;" b6 O0 @% X# [2 K: V$ h
- }
$ H5 O. t9 r8 j3 d1 A( h: L7 c - }while(!((lightsensor_2.read()) < (10)));0 M& f1 @/ {) e* S5 p! i: a
- moto(0,0);
g/ T0 O* }9 h- e8 } - delay(500);5 j9 X ]6 C# d* W
- buzzer.tone(262, 500);2 V! t! F# O& i, }5 e
- }
' }% a$ ~5 x/ D0 E: ^ - : w) p) o# Q. k6 f
- int getErr()
; W: d, n9 T: j - {
0 N2 I/ q/ l p% } - sData = linefollower.getValue();
1 u( T/ j6 E ~ - D1 = ~(sData>>0)&1;6 M: J! b" d& O3 O& {, B/ ?& e+ Z# k
- D2 = ~(sData>>1)&1;
% y( b( W- p, n - D3 = ~(sData>>2)&1;) m" A6 T' t5 G+ r: ?3 f
- D4 = ~(sData>>3)&1;
, k7 a9 l' x# C - D5 = ~(sData>>4)&1;$ F1 y* L0 S7 K2 H7 Z7 C& l
- D6 = ~(sData>>5)&1;9 v3 _$ I1 C, ^; N; W- p) y+ F
- int downD = D1+D2+D3+D4+D5+D6;
9 z+ |, d( d" c! ^; Y1 p - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
% u3 C# _: `4 F3 b - if(downD == 0)
$ [! V: s G; _/ \; p2 I8 h0 p) S - {* B) [# ^/ c# g d* D
- return 99;
3 c3 |& K9 |0 L - }
/ {6 h% J- Q; ?; ]3 i s6 G% } - else8 y' X7 W' u6 N. h0 h' X
- {
% Y' |1 ~; k. E' `+ E! ~" k* w - return upD/downD;* o0 P+ i1 z. G4 g
- }/ a- o, `/ y* G0 j- ~6 Q
- }
1 H) B7 A$ _" }: `+ {" b - 3 H. a8 r0 y$ s' c- D& ?6 Z1 A
- void moto(int Speed_L,int Speed_R)
: y; n4 g9 x( B& N$ O, E - {4 q! A. Q3 I. Y. t/ h- a
- Encoder_2.setMotorPwm(Speed_L);
' l* w+ e( y! k$ ]& A+ a% K - Encoder_1.setMotorPwm(-Speed_R);9 @6 B$ \ o0 ~& ^! E- z
- }
複製代碼 . P3 l3 u4 j; {. S) a; T3 A8 b; K8 R
MeLineFollowerArray.cpp
9 t/ o9 V! E" ~- #include "MeLineFollowerArray.h": Q4 \ M1 t: A. g
8 |' @$ B) [0 Y+ X) d7 m5 \4 R- #ifdef ME_PORT_DEFINED: U6 G. ?0 J; L |: \: F; l4 t: h
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)' z2 j9 Y, T$ z
- {
2 D" o% `4 o1 _
3 D" k8 g$ a# p* y- }& K# D+ K! U# V k$ D1 k' A5 c
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
$ ^8 z, P" l! f# z, V, h2 n, w3 z - {: B8 W& u' m! w j" s
- _DataPin = mePort[port].s2;
0 E* L. x3 c# D1 A - pinMode(_DataPin, OUTPUT);
# ]: y! O! E1 o3 P2 X7 k - digitalWrite(_DataPin, HIGH);% S/ b9 `. P2 Q- x
- }
1 }+ c1 m/ ~+ V& V" W - #else // ME_PORT_DEFINED, P0 g( p- |# W8 X
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
5 a: T& T( z/ l5 W' l: F - {
/ [: ?! r: \7 f" F5 [ - _DataPin = pin;
8 e t7 b: [: ` - pinMode(_DataPin, OUTPUT); 2 L+ [1 g6 [5 o/ S" f
- digitalWrite(_DataPin, HIGH);- m) d+ d! {$ P$ r- H% w3 B# ^
- }$ I+ i" U+ c% S; p
- #endif // ME_PORT_DEFINED1 ^: ^3 a: s+ v5 D2 r9 k
- # m" {4 @4 A: B* q. b5 ?, A
- % X$ ~( P1 a$ p
- void MeLineFollowerArray::setpin(uint8_t pin)
2 {1 U7 b1 G0 Z8 L" O }% K: `! q - {
. n! y) G5 C. Q2 j - _DataPin = pin;
9 j+ } R" G4 A: B - pinMode(_DataPin, OUTPUT); ! x$ P5 j3 ^+ y& z
- digitalWrite(_DataPin, HIGH);
! L4 a, k: |( F$ t8 D |& O
: G- Z9 `2 B2 n- #ifdef ME_PORT_DEFINED; C4 x7 f) X& M# B
- s2 = pin;
, u o) i$ G& E: V1 c2 {( Z - #endif
; |0 A# c0 [& ?' y' ]: n - }) \: \- ~% S2 l* W: U! i7 ?7 }" P
) p& Q- r+ {9 \' I- uint8_t MeLineFollowerArray::getValue()
N4 F7 o' X3 U, M3 t - {
+ W1 L3 ^& p# K6 W3 o" W, U4 y - uint32_t LOW_level_read_time; X% E" d, D2 J5 @2 a) Z
- uint32_t HIGH_level_read_time;
1 D- }1 G3 n3 ^" M - uint32_t time_out_flag;% I) j6 `, x4 ]# Q
- uint8_t Sensor_Data[3];
5 r2 z, I8 N1 i$ K, L& F - static uint8_t old_data = 0xff;7 |$ d) O1 T& U4 U; X* X
- : E/ }; ^" u: ^9 F
- pinMode(_DataPin, OUTPUT);! b- k: R; p9 p3 `8 P4 m4 l
- digitalWrite(_DataPin, LOW);( z6 |( y; f3 X V- y7 r w Y( E
- delayMicroseconds(980);/ i* z" x6 r8 `" R7 O
- digitalWrite(_DataPin, HIGH);
5 x. U0 I$ m# s" [
% E& ~7 Y0 _' d# E. P, k5 C+ }- pinMode(_DataPin, INPUT_PULLUP);
2 z7 L( m7 D3 j - delayMicroseconds(10);0 h+ J% ~6 h: ?: [# ~
- . V0 Z7 K. |" u- a, w6 a, o- \( K8 j6 W
- time_out_flag = millis();% N6 K1 n) q) V/ |2 r& c
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
8 U' s# W _/ D3 v# M - 5 s# L J8 x" V: t) p0 \' D: _. i+ {
- LOW_level_read_time = micros();
" J$ r* ?' o! b |# ~6 V - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
5 [0 {4 s$ E9 o2 n3 E( | - {( `7 }0 t6 b! {( d7 d
- return 0xff;
- ~& m5 c, }: ^+ h( X0 ^# | - }5 H7 L+ z5 ?7 t$ m) V9 R
^ w& _ a' S- a- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 E! \1 ^5 K. K" w9 C( a* E- t, t
1 K1 B' M, z% V- HIGH_level_read_time = micros();, H- z$ |3 m# r. p9 R
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level- L" _, n# p) w, p1 S/ |
- & X% O. o* I& D$ L; E& o/ J
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
+ H. x5 S& a' ^2 k# C - {6 w2 {" g7 v1 T% J; @3 q
- return 0xff;
0 [$ M7 E1 N5 J$ S2 |& K - }
1 q# Q- B. V: O" k& C# i/ q - * A6 S: J0 ]% T9 |+ v
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
9 }! T/ f) a, O& A4 k - {1 n8 s5 y5 k0 n9 O
- return 0xff;* [/ j6 E, c/ F. D
- }* y: K5 O, V8 y
- % e" v }$ y; B( @: s4 f- B
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );& Y5 l# l; G& Q A( V. v+ R$ X
- LOW_level_read_time = micros();
6 o U9 L8 [. B+ o6 f2 I, d: p; u - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
# j& o: c5 {, d$ m- S4 x q - 0 k' b, v9 G# i! H2 P& x
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
( Y1 P/ Y3 x( n9 a/ G! j& j1 { - {
1 O& `; U( m; z" q \4 t$ G3 i - return 0xff;* I& ?) O0 D; Z
- }, d3 r' G. Y: R
! M M6 S/ b/ \& G& ~. R- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))) s0 R- L0 Z5 s- h- H
- {
: g* W" F. s( W5 M - return 0xff;$ S5 w9 x A; y- b
- }
, B: _2 H( A7 i$ ^/ c - / {1 q) a/ _/ h" M0 F
- for(uint8_t k=0; k<3; k++)# x' s% e! Y) L2 T' Y6 U; J
- {4 F6 D# A7 ] W; _ g5 B
- Sensor_Data[k] = 0x00;7 l' h. y+ R1 r1 s3 a+ N# n# ~0 Z
7 O# w8 c: N) Y" ~/ w8 w- for(uint8_t i=0;i<8;i++)& n/ k! i5 ?" l6 O4 t2 p
- { W1 ?% d: S7 E6 x7 O0 @5 H! _
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
" E2 ^, P k1 H& C7 A, P - HIGH_level_read_time = micros();
/ M/ g( G6 z. C6 n# n/ } - LOW_level_read_time = micros() - LOW_level_read_time;5 |8 |- |' V4 |6 I9 X
- - }+ h& ~7 ]+ K, e% Q3 q
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
3 k& ~" l2 A; j6 D/ C) u - {
' [; v& i( [; B7 B, i - return 0xff;
5 ~' m C3 z* U* u2 s: @ - } ?/ _" ^$ K q8 ^" _ Q! T
- . p. q( T& O2 m h
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 R0 B+ D% N. w6 B - LOW_level_read_time = micros();' ?( t: B/ E; I5 D6 |% L
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level# V4 O( Q/ a t( i7 i4 U
5 l# E6 }$ U: I- P- U3 |- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
: k+ }& y# ^! W& W* A& E1 R1 `+ f( g - {
4 B0 H7 g4 [5 w) e% N# {4 m( n - Sensor_Data[k] |= (0x80 >> i);
5 d2 I3 U' p. \ c% ]6 c8 n - }% e/ j; m. C% P
- else if(HIGH_level_read_time >= 100); f* _! ?) Q, y6 j
- {2 ]5 ]- e. u+ L( p% L* K0 u& K, A( d1 r
- return 0xff;
1 G" I U L7 F0 z6 i. D: F - }
4 r: }) N3 X8 ~+ {' O1 I* D( ^# ?
: o: u1 p1 Y" H/ S W3 R0 y& \- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
" p: _* ~9 _* @0 F( Y+ @8 C - {" n9 ^9 C& W7 |7 [: l2 M
- return 0xff;/ f" ^+ h, ^, d! s
- }! ~8 v( \7 s' |) C
- }( A4 Z1 Z3 C5 C
- }
$ Y1 x/ z" z3 @' Z. M- L4 C
9 H7 B; a2 _& A/ V# J+ \- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
0 {; \) K' K: f* M( g2 N - HIGH_level_read_time = micros();. e9 p$ S2 V0 ]3 O
- LOW_level_read_time = micros() - LOW_level_read_time;# K5 j# s0 F0 B& L- g6 H
& N& u, A% T% o. d9 }- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )! c/ \' d1 r) S
- {
% v6 D2 q) W9 e+ h8 [1 x - return 0xff;1 }. h+ X6 u) ~
- }
* |$ k6 ~1 B) h1 K% b
/ O# _* a, j" {/ m" Y- |3 ~- pinMode(_DataPin, OUTPUT);3 w2 m1 Q! L ^. M* e
- digitalWrite(_DataPin, HIGH);
8 a2 y9 e- h5 F/ a; | f5 w+ i9 I
7 d) I6 ^) \) r; j7 H4 c% X2 O4 P: _4 `* O- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))1 L( h' W {, l
- {) I9 [7 v2 U/ d. s
- old_data = Sensor_Data[0];; P* r, p, ?) h; d, A( _& S
- return Sensor_Data[0];7 h' q, D8 c" F0 J! [; L7 _
- }& [- \1 @! P8 Y9 P1 s1 h
- else5 F% U8 m$ B3 `" x
- {6 @% n, @1 [7 F- S4 @$ b
- return old_data;' [$ l: O/ N* B. H7 t, R3 [) @6 G5 T5 a
- }
( Q4 m2 g3 B' D( c5 g - }8 c7 `7 q9 Z# F0 I8 y) ] z1 a
複製代碼 : `- N+ M* V5 N$ Z. u
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
) Z/ _5 D% p1 @, [# ~) k1 e/ ]/ ?
% P* {: u. |# D: M% a4 N |
|