|
) B7 @9 j6 Y* u2 r2 ]# \$ W- #include <Arduino.h>+ M5 i1 @6 U! s3 D) |; q7 j
- #include <MeAuriga.h>
% K2 M) S# j& d - #include "MeLineFollowerArray.h"7 v! R. G+ Q- D. a, u# d* M. A
- + C D, \. y, r2 _; ^5 j4 j0 o
- MeEncoderOnBoard Encoder_1(SLOT1);
- j) X$ d$ o8 X - MeEncoderOnBoard Encoder_2(SLOT2);
- W( ?$ [, G; \# M9 ^ - MeLightSensor lightsensor_1(12);& u$ T' d1 O k5 e. R
- MeLightSensor lightsensor_2(11);
+ A+ p# V; z; t8 |: g - MeBuzzer buzzer;
" i4 k4 ^4 s- A y- N5 G - MeLineFollowerArray linefollower(PORT_6);7 W+ ~: y0 h, f. l* o
7 l7 L2 {( n" d& p. P9 C( Q0 j- #define Error1 1" y4 T- x' P% V; z' p1 y) f
- #define Error2 2
- Q& _0 y+ n) Q - #define Error3 3. v$ W4 @9 s2 y) q- z B
- 4 h! B" i$ h5 G$ k' G
- #define Kp 15' b' B- y2 @+ w" T
- #define Ki 0.152 Q1 b- T7 P3 N
- #define Kd 0.03
2 {/ g+ a. Q1 V* O - ! \* Z, a+ L* h& N3 F$ P! B
- uint8_t sData;) e2 A& X3 h1 |3 d
- uint8_t D1;
% j5 x5 c, A! |+ H$ N0 z - uint8_t D2;
* c `( k- \5 M% P% x1 A - uint8_t D3;
& A2 [+ c3 P" c" r! i. _! M - uint8_t D4;
. ]. `) d% z& o* N$ k8 \& B4 L8 M - uint8_t D5;
6 L% T6 h2 n& |7 ~ - uint8_t D6;
) q6 U5 ]3 N# k& P
. r7 a4 ^6 b/ f- z( P; g- float previous_error = 0;: r" m3 V9 I" j7 _% [5 W. q
- float integral = 0;
G1 Q2 w' i! x) D - float derivative = 0;+ l0 q' v" I! |4 u. U% u' [3 W
- int Speed = 160;
9 `, a$ T: P: }& N6 l - float output;
( }7 D+ R1 E. o/ t$ |2 q$ r4 b8 d9 a/ h8 a - / @$ z' O. f0 G' i
- byte Left;
8 b' d" G% m# p7 e) i - + }% k3 ^& G0 z; v% @
- void setup()
3 Z" F- F. |; j. p+ p' j - {
, v# C! h$ a0 u0 K# N+ E - //Set PWM 8KHz
! q; u+ X- X! I- ` - TCCR1A = _BV(WGM10);% s4 P. x% \- P2 q3 t
- TCCR1B = _BV(CS11) | _BV(WGM12);
+ F5 u+ L9 @8 ~" h - TCCR2A = _BV(WGM21) | _BV(WGM20);
+ U* X) Z$ q* F! G7 T6 V5 _/ W - TCCR2B = _BV(CS21);
$ d. f7 C0 |0 s' \( C: u2 U3 S. x - Serial.begin(9600);; g7 G( j" Q, K
- buzzer.setpin(45);
4 b* m2 ~: S: l. ~" ~ - }' b% a6 u8 k: B" r$ w
" d" g5 S" p# a" U! Z- void loop()
, i$ n( N% M) V9 ] I B' W - {
0 C5 z0 A/ a1 [" g& S/ M o. {0 ^: _6 z - while(!((lightsensor_1.read()) < (10)));! Q4 _, b3 O2 M$ D0 X1 ~8 G) u
- buzzer.tone(1047, 500);$ F) n( P, U# D
- delay(500);
" G* R" W% y" {9 X - do
- j2 G0 |4 x1 x0 X - {
7 u$ \/ Y) L- d. C# _ v* P+ E - int Err = getErr();4 Z" N, s+ |0 d G) U% u3 @$ z9 X+ i
- if(D1 == 1)
( p& Z# y2 w8 Z0 _: T, v& Y - {
: j0 G U7 A5 s+ T* e n/ u - Left = 1;8 o' B! k4 B J& ^) G
- }9 W# x; Z& J* S# y4 C6 Z
- if(D6 == 1)
; |9 |; I3 y2 i - {6 c/ z+ h- w$ f3 m- o* v% _
- Left = 0;( q0 S% w/ K" |. y1 p5 b+ f
- }
/ C b! l/ }: ~/ T: c: Q - if(Err == 99)
. I: H1 c1 ?1 ~; K - {
' J4 C) h2 O8 v3 u' F9 k3 ^ - if(Left == 1), U( V7 Y" P+ Y5 r q( \/ |( }
- {
+ d7 n9 k, p& b, l& m8 h1 K, w - Speed -= 5;
! L% |% w3 Q& }. m4 x - moto(0,Speed);3 X( L7 h6 e) ?! c- H
- do* y7 T, c/ r+ e: ~4 V0 x
- {
/ N R8 q7 P/ @1 `' K$ Z! |* M2 c - Err = getErr();
7 B5 O6 s: u1 g9 Z6 I - }while((D1+D6) == 0);" e: b/ t0 T4 E3 p3 c
- }
) T# \) i% w- L; ?% |1 v0 Q" D - else: d' L, l2 p/ [4 D$ p
- {) D! y/ {$ E$ e. u! T; H
- Speed -= 5;. q: Z0 W' v! z9 z
- moto(Speed,0); 9 _% D3 b' U' E" X" N8 v; H
- do* K( g7 Z5 V5 X( Q
- {* }! Z+ k2 K: D1 X
- Err = getErr();4 T* v% i3 ?9 M2 {1 K
- }while((D1+D6) == 0);- l, ^$ f/ C, D( v0 H/ @
- }+ m) J- o9 t( H* f) s7 g; m
- }. t6 f2 S" z% V' P6 ^# x
- else; F) K$ }6 ~3 s6 e) g5 n$ O
- {* L9 _* h3 m1 S% [3 a
- if((Speed < 160) & (Err < 2)) Speed+=1; 8 V) d3 C6 V9 h$ c( m( f+ J/ F5 d) P/ {
- if((Speed > 100) & (Err > 2)) Speed-=2;
5 n0 U s+ }6 k: U - integral = integral + Err;: s z! E. i" J& v9 l% _/ P s# @
- derivative = Err - previous_error;
5 Y2 S! z" R9 f: w+ c - output = Kp*Err + Ki*integral + Kd*derivative;
; ^( y6 G! x7 i1 `) X - moto(int(Speed-output),int(Speed+output));9 _/ Z6 U- \" y! }# ] x) A3 x
- previous_error = Err;
8 a* L T& q3 \ - }
/ C' H6 `5 c" z! v, ~, H - }while(!((lightsensor_2.read()) < (10)));% m* ^9 b8 c$ u# B
- moto(0,0);" _0 Q) w9 U! j! Y% Z
- delay(500);
# E9 U% L. G. s* A0 ~, E - buzzer.tone(262, 500);+ v( m) X1 Z, C! J' t3 _) m% K
- }
& N0 a# d4 D, {: h: ]' v* x4 o - 9 @( x- U+ k$ Y& h* u# @
- int getErr()
# _( L3 b% n8 w' m1 C+ F - {
F) m' L2 F% P( X" o - sData = linefollower.getValue();8 d6 M0 X! X& `4 |" u* C! Q$ z
- D1 = ~(sData>>0)&1;
$ R+ w( s" k" k9 [! J9 r - D2 = ~(sData>>1)&1;
/ m3 W4 H+ n% B2 |+ Q$ ` - D3 = ~(sData>>2)&1;/ z0 \( a: T& E) p6 @! S5 d, r
- D4 = ~(sData>>3)&1;1 R0 S- j, c0 M2 o3 o7 x
- D5 = ~(sData>>4)&1;- e% t1 I5 [/ u
- D6 = ~(sData>>5)&1;' @. Z: ]( B& G N2 H+ Y# z
- int downD = D1+D2+D3+D4+D5+D6;( d( K7 L+ Y# e6 v; e& Q) ~
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);& c/ c1 [/ Y) o8 |" f$ d( N6 y* R8 K
- if(downD == 0)& k1 H! V& {- v* ]) O4 U% z% \
- {) Z" _4 C$ [. J4 U/ G* C
- return 99;" c2 X/ j$ G- Q, Q4 ]
- }
& d, K5 n: k: A v9 B- G - else
& e# U+ @, z# _+ F) s, Q - {# V! W/ g1 N# V3 N, f4 e% r! ]; F
- return upD/downD;5 i0 Q3 i. n% J$ k$ ]4 o8 |
- }0 P* b/ c0 X' j9 M. h
- }
" v9 u7 X. o8 ~; h
. N3 D4 s( J# |& U" i4 m1 b- void moto(int Speed_L,int Speed_R)
; E, D6 w7 u7 }, z% {# w - {
* p$ `/ i% ^- b0 T* c - Encoder_2.setMotorPwm(Speed_L);' ]1 @9 L# [$ `# G' R
- Encoder_1.setMotorPwm(-Speed_R);
, c5 V! A( ?9 K, z4 {, P - }
複製代碼
2 X; t8 W b5 o# C) a8 m8 zMeLineFollowerArray.cpp8 f6 p& M A6 d* X1 y# H
- #include "MeLineFollowerArray.h"
* n8 D$ X) i k/ t* c' F: a% C" K - 5 c# ]2 F9 L- K) @
- #ifdef ME_PORT_DEFINED/ T( o7 @6 I; ?; ?0 D# n! E' p
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)9 F. l: m" x3 d/ C" u1 }0 |3 E
- {! _, n" m, {) @
- / m+ _+ C" m: i* A
- }
" u; N( M, R$ }7 |- o$ O - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
5 A9 z: W; @0 Y: h' b4 I0 i Q* W( } - {- A) t% ^; F( R& Q
- _DataPin = mePort[port].s2;
1 D& A3 U+ m( N3 r9 F0 |; s - pinMode(_DataPin, OUTPUT);
' D" C0 ^2 K% w: P! j$ h# f - digitalWrite(_DataPin, HIGH);
6 I Y5 R2 k5 P' Y - }" H; i; U( Q/ r" e6 d" s2 c8 p" k) J. Z
- #else // ME_PORT_DEFINED$ n! a5 ^. X% F0 L6 J" z" j( {
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
5 R& ~, C3 f7 E& H2 m* C* Z - {0 f6 b% x- ?2 F$ s/ k8 d
- _DataPin = pin;* f; t8 g8 i% F7 v' {4 u; }$ a
- pinMode(_DataPin, OUTPUT);
6 P. W$ T+ k. j- Y+ k$ M - digitalWrite(_DataPin, HIGH);' d/ q( ~; L/ `* G' Q3 C* A: H
- }
& |- U! d0 S( e9 d) s - #endif // ME_PORT_DEFINED1 O( d' h+ c8 S: e: e( ~. _
- 9 @! m) z1 S/ `& ?8 r$ S
1 m+ f) x! r- {5 s& t T* f- void MeLineFollowerArray::setpin(uint8_t pin)
2 y% f5 F3 v% e" h5 H - {8 ^9 i3 `6 w' O4 n
- _DataPin = pin;- u+ Y2 H( o. \7 c$ M! _: z
- pinMode(_DataPin, OUTPUT);
0 i+ g: K; j: u( k8 ]! A9 m - digitalWrite(_DataPin, HIGH);
. ^/ Y( A0 k& d" J$ H. x - ' _& ~4 M: N- g0 V T9 u
- #ifdef ME_PORT_DEFINED
; @' }$ ^$ f1 L2 t0 l D$ h: ~ - s2 = pin;* k1 N) ^( G! B0 j" Z h z
- #endif: I& a# G' n3 E/ A' w
- }# {3 O0 b g% T% M" K+ `( a+ f7 l6 t
+ a, a. y1 a1 N, L4 S; F9 l8 b- uint8_t MeLineFollowerArray::getValue()
, g! t4 L' h* t: o9 B - {1 s" F$ ]$ C5 j. D/ O9 G" F) g5 N
- uint32_t LOW_level_read_time;
% P6 L+ S, B r/ }! j - uint32_t HIGH_level_read_time;
; A/ W) ]5 \+ p, G7 z - uint32_t time_out_flag;
2 A) B9 f" ~- w" F2 o) f# e6 C/ ] - uint8_t Sensor_Data[3];4 a, J3 n9 T3 n$ \6 d5 G# p
- static uint8_t old_data = 0xff;
* h; _' X9 ~& ], L v
9 B7 y, k6 `. v( }9 G- pinMode(_DataPin, OUTPUT);6 M% w& Z$ R. }! |" h) ]( }
- digitalWrite(_DataPin, LOW);
$ r' K4 U& h# o0 l4 b# d - delayMicroseconds(980);
- Z/ r; Q. |9 W B4 Y - digitalWrite(_DataPin, HIGH);
" x \. Q: O+ O! f& q
7 P2 U6 x# t! i9 ~4 @- pinMode(_DataPin, INPUT_PULLUP);& g: d. _8 n- Y- r( [$ F
- delayMicroseconds(10);
0 P- ~) W4 o6 s# W - + {2 p4 _& G( b) y
- time_out_flag = millis();
4 S8 Z# N! }4 }, `6 z - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ t9 m8 q7 V" j! I5 I; r
$ N2 @& s7 l& p. v* y; P- LOW_level_read_time = micros();
/ Q& d- M3 W X" m - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 e$ Q4 Y4 q* r' h6 ^% A - {
9 k4 l0 m6 e0 k. y' J - return 0xff;
' n. T$ }* b, d" d* s K0 M - }. d9 ^+ w- u9 ^
$ ?( y' h+ B0 D8 x; T9 [- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
2 P- N& O1 x$ y# i - 5 y9 M5 q9 b; \- f6 @
- HIGH_level_read_time = micros();" b% a' a3 e# ~6 q% B
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level8 l9 x2 _2 o+ D' u% C+ \
- 1 y. W- ~8 \9 n& z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 h. O& O5 y9 `4 P& [) o
- {+ k: ?- |4 W( l. ~! f
- return 0xff;. d6 Z* B* c. n- g: ~4 ^) A
- }
7 P3 @3 t, @% D% ` n( { d4 O - " D: e+ j b. |/ G9 ?5 U
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
" ^3 ?, O# S5 n9 I; J3 ~0 x - {2 p3 F8 `$ |9 q+ V7 T
- return 0xff;3 b$ R6 [& h- d; L6 W
- }
3 x, f5 j7 w, p! v J+ h* \
/ y2 [* v! V8 w H, R- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
8 ^% S, U+ g* N - LOW_level_read_time = micros();; R1 A# @. u: g+ w- M7 w5 ~# N
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
9 B5 S7 D5 i& l9 }0 w: ~; K - ( [5 C7 j: g- c" z( G& u1 d
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out( x. p t9 B4 L# k1 ?, I4 _
- {: J2 g) Y. O& `& Q8 H' u
- return 0xff;
! y- w! v9 P) {8 X5 h - }
6 `4 _7 j) Z6 N9 c
" l8 \' y% t5 ]) D1 k, p1 x; ]- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
* P$ g! U" J. U2 C! F) ]5 ~ - {, |" N# \9 M6 k) B. Y" o
- return 0xff;3 j" D* A! Z, J$ x9 g. l
- }& L7 ?/ S) R$ }$ Z! b0 p
/ ?: d6 U& }) L$ I! p+ r5 [- for(uint8_t k=0; k<3; k++)
6 w$ K# e- p. Z - {+ W' d) [+ t) B1 J
- Sensor_Data[k] = 0x00;2 A! i* h }8 O* }$ R0 h0 Y/ m+ k) P
- 7 O' h2 m$ O4 @% ~
- for(uint8_t i=0;i<8;i++)" t C4 I- c4 I* M! O' w- d, B {7 m
- {& s" b6 f& M6 W( a4 F+ U+ r9 T
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level. s( ]4 A1 q( F) d
- HIGH_level_read_time = micros();! z: R$ h, e( [/ z2 Q: A6 D$ O2 k
- LOW_level_read_time = micros() - LOW_level_read_time;
0 E% q' @) Z4 A& `8 U5 p1 @
7 ]9 w$ z7 f y9 ^) F: L- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )# X& z! W$ `2 l' q0 w( p* [
- {
9 |8 R( }8 |/ p8 K8 o2 v/ V) N1 A8 Q u - return 0xff;# n* ^) B5 g6 ~8 T
- }% f) t( Q7 f. }6 a: Z
6 u8 C! v% ?3 }# X; C+ z" `/ G8 A- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 G' s8 x# j; N j - LOW_level_read_time = micros();
" _! R3 ~ b) W8 M) e. ~ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
: w+ M0 k, x# T* C. Q$ A
; X6 A$ G% t! t6 Y- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1* M5 n" N& Q; I
- { R: q+ j- z; e# t, Y) [1 ^1 k
- Sensor_Data[k] |= (0x80 >> i);$ t# t9 b, V& T4 c- r0 L
- }% m; L. [3 B$ e3 W
- else if(HIGH_level_read_time >= 100), {' c/ r( u7 @" \: P5 e$ }
- {
0 E! m1 U0 O# a- p1 s# `) E g, B - return 0xff;
) n' n B* W$ C: G$ v - }
/ G8 X Y6 a" A2 x$ B3 v
) P$ B/ c( k( {; R M8 v; t- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)# f: F" K) i) E$ e, [) G
- {
" n; A& V+ ?2 `/ q: b, [ - return 0xff;9 h1 a8 h2 P- V1 [, E, Y- u
- }. m+ F( z* E |/ N4 ~( p0 L
- }* W% D" Z! \& W! P% b. o
- }
8 O- N2 D. O, n- W+ {* G
' u) e6 }: |- o+ D" F0 Q- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
0 ]. p; a5 r$ V V4 _ - HIGH_level_read_time = micros();
4 v* @) J+ D) _. R& y - LOW_level_read_time = micros() - LOW_level_read_time;' M0 Q) n# |& B% S: A$ x. A' m. c
- ! z. J3 ?0 B1 ]2 v- q
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
$ _- k. t& y& F) j$ i3 K - {
. O7 m( I. @3 e, L% q+ h' |$ W - return 0xff;' |7 P% D- q& X5 [2 W6 c$ f
- }( Y! ^ \1 u9 }; {9 m
' D1 P- v( X% ?! g# e/ C3 a- pinMode(_DataPin, OUTPUT);
3 v0 H/ O; M8 c8 t' p0 P- S - digitalWrite(_DataPin, HIGH);
" i" ?8 `3 |% x3 I7 @9 `( j
# V1 J6 B" G8 Y- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))& F: I; _3 c' T1 ]8 p
- {0 m8 M7 p4 ^0 P+ ~& H6 T, b8 r
- old_data = Sensor_Data[0];# W' L4 n3 o5 C/ n) D# e/ d( l
- return Sensor_Data[0];# B4 F3 V" g4 @- K0 f2 q
- }
) R0 D9 l a! {" ^4 T5 J5 d - else$ q; n: x1 Y" V! ^/ ~
- {
; I, B$ p6 s$ b. j/ ]* X - return old_data;
7 u0 Z% ?9 {! s# B- E - }
, }" X& v7 P9 K" i Y; b( O t - }
4 v7 p5 T4 I4 f( P7 G
複製代碼
' ?3 ^* g! \" J6 L9 [MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
: B! A- g* ^ @4 J
! g, j" t0 \9 Y7 P |
|