|
|
1 o+ J; ]- E$ s6 {$ f8 w8 K- #include <Arduino.h>& ^: h! o, \0 q$ g& ~/ d. q( p
- #include <MeAuriga.h> ~# ^& o/ A, k
- #include "MeLineFollowerArray.h") v8 B& R+ G& [5 x3 {$ N/ U
- # O* L& Z4 E( V3 n& j/ k* k, \; `
- MeEncoderOnBoard Encoder_1(SLOT1);4 V4 _) t/ p+ m& q: x9 [3 m
- MeEncoderOnBoard Encoder_2(SLOT2);
9 t3 o% {6 M1 N" X; w' K. Z - MeLightSensor lightsensor_1(12);
2 ]0 n- F; e- m) h - MeLightSensor lightsensor_2(11);
7 B& z$ a, N( C. o! d( R2 E - MeBuzzer buzzer;; R3 C: V9 m$ Q4 |2 T( G7 V' a6 W. J
- MeLineFollowerArray linefollower(PORT_6);* @ o; ~( f4 H. S. \
- 9 t2 r) N% |4 x% A: _
- #define Error1 1& Q4 S3 D5 c0 l! ~
- #define Error2 2! t% q7 c/ l6 X/ v" W6 _3 }
- #define Error3 3. ]3 R) o" h; v* o$ t) j7 [
- # n. m( U+ R$ R2 ~1 J; _. `1 I
- #define Kp 15+ u6 Q! {8 j* E1 I5 `
- #define Ki 0.15/ P1 O% S+ i$ O7 z0 O0 r. O
- #define Kd 0.03
+ T* _, `. R' |" ~1 t' i, S - ( c$ }( w8 `, v1 d' O0 @0 J
- uint8_t sData;: S& j+ Z& u7 U f: z
- uint8_t D1;4 C! |! `. Q: ?
- uint8_t D2;
" H" P8 `5 L% c: e ~ - uint8_t D3;) x& R3 x' ?; q2 `
- uint8_t D4;
$ V5 L! |0 q# Q, I4 o( v; A - uint8_t D5;
/ E$ B: t' s! f: R6 \7 U - uint8_t D6;
( T% ~8 s$ g4 K4 Z, R - 1 }0 J: ^ q% E3 d+ `1 [% U7 H
- float previous_error = 0;
! `5 d: }- f6 j - float integral = 0;
3 D5 r6 U' r4 |, b, W - float derivative = 0;- Y4 ?3 l2 ^2 ?, f
- int Speed = 160;, h2 s( y; C; }4 k- ?& D
- float output;/ [% k8 e' K Z& O
- ' P! ^# }: i' v. ?0 u2 }
- byte Left;
9 \& U! v0 M) p% U, V# M( f - # \* u) w+ _" N
- void setup()
! a, v- l/ e8 P: X/ h6 h - {
. b; e! Z: ?/ w - //Set PWM 8KHz
5 E: V# u/ D+ }7 e: u# m1 _' x - TCCR1A = _BV(WGM10);
7 p8 q: n( R1 B5 m" W6 W - TCCR1B = _BV(CS11) | _BV(WGM12);
. l! |6 ?, c3 ~. Q( |. R8 S' U - TCCR2A = _BV(WGM21) | _BV(WGM20);5 X' |( Q% [) \- w" V- Y
- TCCR2B = _BV(CS21);' f5 Z! N4 a6 X# ~6 [4 M( L- o i
- Serial.begin(9600);
/ I5 \9 W4 ~& G: e# m* [ - buzzer.setpin(45);
- j4 F! Y# g9 i) S/ z( Q( H - }4 U( z) Z \. ?/ k3 _
- 2 @# H0 v i H
- void loop(): I3 Q9 t# v& C5 S# b \5 W
- {
- t* C# _/ o% B3 y - while(!((lightsensor_1.read()) < (10)));5 R* n# y/ A) k. c1 w
- buzzer.tone(1047, 500);3 V6 Y6 y/ [' `: z( I2 ^7 X# h
- delay(500);
: }3 r. f+ G% F2 r7 ]0 G( Y - do6 Z' M" k) S% b6 D9 ]7 J
- {
5 n' {& F2 H/ g5 g! y0 S - int Err = getErr();9 \2 ?8 [% G" u1 u# x
- if(D1 == 1)' v6 F9 ~! ^8 F( D2 C
- {
, e. Q0 _: |/ U8 E) G - Left = 1;
0 q4 F) n! s+ u5 Y/ @- F2 Z - }
# @8 U+ Y+ T. c. ^1 _- L& G - if(D6 == 1)
5 |- @: W1 P. G9 G% X - {
9 s' H: Z2 y3 c: {: A1 F; a - Left = 0;1 \, x3 h$ i) k, ?) P" Y w" D
- }
D# j X4 G0 n$ M - if(Err == 99); s* W% }) u& w- N8 U+ ? A
- {
) L3 u, ]' Y" M: [8 z; r/ O$ A - if(Left == 1)4 \4 P _1 M% g) [, o
- {# ?0 ]+ y, }+ b3 W: v: u
- Speed -= 5;& p' s" g$ Z) T+ H* e* r( y
- moto(0,Speed);
1 h7 B9 n" S9 @- L6 H! X& G% ? - do6 i- `. r+ q1 D# ]9 ?' v% B
- {: X2 ]6 F- O6 z7 {6 _. E S
- Err = getErr();' ~1 L& q7 r& r5 m* m
- }while((D1+D6) == 0);
8 }: W' x$ `% {9 m) v0 B7 N - }6 o( S5 L' O+ H N/ t
- else
6 X" N3 R5 p0 b0 d! b - {& x; E8 L- H5 I
- Speed -= 5;, C3 n I, G! B, }0 Z
- moto(Speed,0); ' N: o6 ^0 L" ^+ U7 A9 y
- do
7 b7 M0 q E: H$ q& z - {
" |( O3 U* |# E - Err = getErr();/ U3 _/ h* W4 a3 a1 A4 I t
- }while((D1+D6) == 0);2 ]( A0 z5 }+ S/ h" B' T
- }
5 [' k+ O: C/ w" Q8 r0 M - }2 L# B, d; U4 o: R' k4 @# {& ]: b
- else
1 J: q/ S! X* M$ o - {
$ w* |! u6 p6 ~2 z8 C9 {) u - if((Speed < 160) & (Err < 2)) Speed+=1;
; Q7 a+ N, z+ C& ^6 t - if((Speed > 100) & (Err > 2)) Speed-=2;
1 D2 T% L H0 r7 X! s - integral = integral + Err;
# L; c8 R" d* t - derivative = Err - previous_error;. z8 |9 p {" H) a9 x
- output = Kp*Err + Ki*integral + Kd*derivative;
) d4 x& w' [+ M' e$ }+ p! i - moto(int(Speed-output),int(Speed+output));$ ?: n- \ n5 y# O: C) e
- previous_error = Err;3 ?; H7 \; ^3 S# l, p% b$ z
- }
# B- l) M9 u* s) T$ K. B) v( T - }while(!((lightsensor_2.read()) < (10)));
; X/ Z5 m8 P, R5 Z - moto(0,0);
% _& c! }. m1 z# U0 r - delay(500);5 Z( | T, o: W# @9 s9 g7 r# a
- buzzer.tone(262, 500);
. a5 T* z2 c1 A* \ - }
5 @ A9 V1 \: U2 r( \
! H9 B% |5 e$ W4 ?( t- int getErr()
* P; Q6 j& X- u I- n - {
8 n5 P/ d, J7 N% i, L. r - sData = linefollower.getValue();& l! I/ e* }/ b6 Z0 g
- D1 = ~(sData>>0)&1;
8 G0 x" E; T. m- |8 c6 o7 H - D2 = ~(sData>>1)&1;
, P/ r/ \$ G3 Z+ Q( d; C. b8 j - D3 = ~(sData>>2)&1;3 z: p# }& F) |- P
- D4 = ~(sData>>3)&1;
, e% a% g M# R$ @$ ^8 k. ~ - D5 = ~(sData>>4)&1;: ]. J* Q2 C- L& Q* b# c
- D6 = ~(sData>>5)&1;! l' L$ @ P' ]0 V* p
- int downD = D1+D2+D3+D4+D5+D6;, A; q. n( D$ C2 T9 W, s: Q9 F4 Q
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
+ u y J; x( g4 W" @( | - if(downD == 0)
8 e5 b5 e7 [- j& ]6 u$ Q - {
0 r( `: i8 F" |5 V( E" x# M - return 99;
9 m% h1 G. [. X7 |+ K5 V# r6 ]0 V - }9 G: d( r4 t% Z- c1 d, W
- else' r K7 c* I0 r; `
- {& n+ U0 ~4 J( @
- return upD/downD;' x. _# J: ]& E4 P' M7 w
- }: C0 ?4 h% l# Z3 `
- }% s5 M2 T7 P, {/ E2 k
- ; ]7 K# _* ]+ h- C) a
- void moto(int Speed_L,int Speed_R): v x9 q0 h% ?8 {" @& l
- {3 _% N8 I* {3 E& t& A) _
- Encoder_2.setMotorPwm(Speed_L);
6 _! h( L* k+ \5 |& C7 c" { - Encoder_1.setMotorPwm(-Speed_R);
3 K" {$ H+ V6 k8 R) ^% ] - }
複製代碼
5 e) ~4 J) V- u# zMeLineFollowerArray.cpp
& \2 g/ o+ y) Z7 p, {- #include "MeLineFollowerArray.h"
8 {5 I) R& ]: I1 ?3 x6 u; T: }; i- H - G# A( r2 B& X4 H
- #ifdef ME_PORT_DEFINED1 `* n$ m0 [ q9 \! Q. A' h5 G
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)+ Z, r$ K, U; K) J/ Q9 k, Q& B; v
- {
0 _* p4 Z2 t2 ]) J - 7 b. T* H. u7 q: d3 ?% V
- }
3 c3 d* n+ d, y/ ] - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)+ W9 y+ X0 ?. q6 o
- {
`# W; L* h& \" y2 b% z$ r - _DataPin = mePort[port].s2;. I# h/ F! K$ _* y$ h
- pinMode(_DataPin, OUTPUT);
3 h) m; d0 z( y' ?- E' K7 k - digitalWrite(_DataPin, HIGH);
+ p4 \0 k* S5 u - }
) J, ^$ g5 Z9 s$ r0 N - #else // ME_PORT_DEFINED
, u1 t% s& n. X$ l. C9 m - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin) K1 y# u* u g O% F: D
- {
1 F# K1 S+ m. ^6 A! R# D7 I - _DataPin = pin;
. e5 w2 N4 f" S3 n - pinMode(_DataPin, OUTPUT); ! k3 M0 R2 c6 ^ ?$ b0 g2 z
- digitalWrite(_DataPin, HIGH);5 k9 l/ J0 t! D: {0 j- ?
- }
* E( \& I6 T& K; k4 t- ~ - #endif // ME_PORT_DEFINED
, X; j R- W+ i9 s- V - 5 e5 N( s: H5 H( l
7 n) ^; p s+ S# y- void MeLineFollowerArray::setpin(uint8_t pin)* n3 e. a) H1 A6 E. S. E9 d' ]
- {: [+ X- s' I. { z3 L4 K
- _DataPin = pin;
; }4 w9 f: L/ B7 w* \ - pinMode(_DataPin, OUTPUT); 2 P3 |8 n% s: C( X. d: J# k7 x
- digitalWrite(_DataPin, HIGH);! g+ {1 H+ r& Q
. O1 K' h: f7 ? y! W- U- #ifdef ME_PORT_DEFINED' q" M/ d- V9 R. X% C
- s2 = pin;
; x# C" E+ Z p, M" @' b5 \& C - #endif8 N9 N) ^: \5 |/ J6 F
- }
! ~' D1 l: X0 {5 z - 7 R( n6 k4 @/ H( \6 F/ R
- uint8_t MeLineFollowerArray::getValue()* g# O" k1 B) {
- {% [8 {2 ?# f+ [+ J3 `7 A s2 B, [
- uint32_t LOW_level_read_time;
# G% f) P% F, k# u0 p - uint32_t HIGH_level_read_time;
0 T) b. |( n/ a3 r; J: q4 q( J+ c6 o - uint32_t time_out_flag;
$ F m9 s$ e: c& X3 S: P+ y. S; g - uint8_t Sensor_Data[3];. P& \5 x& Q" f5 Z2 g7 S
- static uint8_t old_data = 0xff;4 A) G$ \$ ^, G# ]' a1 F' A
% d Y/ c7 K$ e c- pinMode(_DataPin, OUTPUT);+ U; w9 T- Q- y9 }! ?4 m
- digitalWrite(_DataPin, LOW);0 _+ |9 N/ }: c( u x: d( A
- delayMicroseconds(980);
* o7 o* a- K: O4 n% I3 V) d - digitalWrite(_DataPin, HIGH);+ k/ D" Y9 \' Q* L9 {7 i
- 8 l! w7 @- l6 h1 O) O
- pinMode(_DataPin, INPUT_PULLUP);2 @) q; V/ `: P; ]% Y* {8 Z% m
- delayMicroseconds(10);7 D1 M' O: U% ^/ u9 `5 H
- 9 t- ^7 m: q7 c. g G: h
- time_out_flag = millis();
( i$ P5 v" T! V$ X3 O. E7 k+ T - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- Z% a! f: X1 K- z
- - K$ P& Y# }/ ]2 ~& @; q
- LOW_level_read_time = micros();$ M& S3 h% m: @* [! V
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
4 |7 o8 y& A& R% }4 l! f3 ^1 y - {* |. ^* m1 I) b2 Q" Z2 M& P4 Z. z9 C
- return 0xff;: w! a7 L$ h' M
- }% U9 C4 l t3 A6 n
. ^1 ]5 a7 [& D4 V$ m( o! `. b* d- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ Z, p6 K: w: @1 U) _/ a2 ^
/ N! `( D- y9 G+ B& U6 G4 G% |$ A0 Y- HIGH_level_read_time = micros();
. l) e4 E0 N; N2 Y" V - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level0 z% x Q- r+ q
- 2 l0 c7 Y7 \+ L3 m5 K# z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 Q! \# }$ |' d; V+ _! Q& N - {* i6 C# R {+ u" f/ d
- return 0xff;
6 M1 `$ k+ J$ n! P3 Z# M: @ - }
0 t9 v o' W% d1 W) s+ [
/ v9 P6 b2 N2 u) _- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)) @$ K) y/ M. @: v3 [
- {# q5 G0 O. Y& F( U
- return 0xff;5 ^0 L2 a2 ~0 G# f2 e T4 P
- }5 i+ |# K6 l6 H! D4 M
# w6 U! j- p2 F0 A- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ T4 c5 q- k* j4 E D9 D
- LOW_level_read_time = micros();0 P" I3 V+ ]& m6 }) R, N# Z5 a
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
$ q( s. I: f4 g/ E& m* S
/ Z9 I" A8 P# i. ~' y$ e- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! L2 i, }" ?# B1 s) B' |8 k* k' B
- {6 |1 v+ w% E4 l
- return 0xff;
6 j5 Z; [+ ]! E; @* p! L( H+ o - }0 O' `- t, Q0 \+ m
- 0 y( F) W; P. e* S4 u( O/ d& @% q& O$ C
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
3 H+ {4 \) f& x6 f; e' n - {
! ?& U3 O3 ?( ^% M* T - return 0xff;9 u% g6 V+ j3 j( X
- }. n: |) @" s: F$ |+ Y, w
: h3 K& i9 Y h5 S0 ^1 x- for(uint8_t k=0; k<3; k++)
; Q R5 N. |) G3 d8 } - {
1 g) G3 {7 A& I% P& Z - Sensor_Data[k] = 0x00;
. C) B6 o. B* ]; B5 r ` - 1 I4 V k& p. {, r x; R
- for(uint8_t i=0;i<8;i++)
3 e; _9 f. z& \" C - {3 C2 [' K7 v( S" w( ?2 y: f
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level7 Z+ X- n. p& {
- HIGH_level_read_time = micros();3 n7 A2 F: }+ x" r) F
- LOW_level_read_time = micros() - LOW_level_read_time;
, E* B. n; }' E7 E" O/ M& \2 Y - 7 Q5 r9 i G9 i8 P3 i* Q& r1 F
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
; h$ d' F; c) n; e - {
& G$ J* y1 _* ]1 I$ l% { - return 0xff;' ^- Y, Y2 \- o
- }1 d& M+ |! P: |7 E, {& @/ K+ j
+ A& b: T5 ~4 J2 d% W8 q- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* ]( n1 n/ g0 P
- LOW_level_read_time = micros();' R7 j) E6 `# i) N& k1 M5 _
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
3 Z, @4 B. [; @* L/ n8 g+ @" Q5 D | - $ P$ W' s# ^$ z s5 I
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
% U" F+ `% C. g6 u9 z: p" Z - {* t$ M+ v, `: k7 Y+ f/ I
- Sensor_Data[k] |= (0x80 >> i);; [+ x2 m0 W; b2 l
- }, ?9 ]- K& C* D6 R$ d4 w
- else if(HIGH_level_read_time >= 100)
3 W' C2 t) |" z- F# t3 a - {& k6 Y s- F W5 c6 Z% ^9 v9 u
- return 0xff;9 w5 b, ]$ ?. W8 G
- }
, x8 i$ o+ A8 ]2 U* Y1 f$ \8 m+ b5 c
& ~/ D0 @4 |4 q- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)+ y3 \9 V( k y3 j q
- {) [# |, r% i4 y' t# }4 @3 e
- return 0xff;
$ z& u: J- o" s2 Q! i" }0 R* m - }6 i1 D6 K. ?- u6 x- b! i/ z
- }2 H9 i$ j- e& `& m
- }, T7 `, R7 k6 T1 W" E7 X
+ e& T7 x9 ~- f; R% C3 G- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level, e+ l/ H" r1 v2 ?/ D. ~' t# K
- HIGH_level_read_time = micros();' C' ]& I5 z7 X
- LOW_level_read_time = micros() - LOW_level_read_time;* t( r4 c/ l, E, a) o; X
- + O( N0 m `4 Z; k6 x4 q, N3 c' j4 T
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
7 j6 f7 V/ i% n& V - {
: @# \0 Z1 N# P g - return 0xff;- ~0 c) K# X: ]$ t* X7 G- ~
- }
8 L, I" U3 z4 @& Q: E$ u - & p: x% F& M5 P
- pinMode(_DataPin, OUTPUT);* S6 {1 t- L6 J1 R. ^ Q! P4 c
- digitalWrite(_DataPin, HIGH);: e! \1 D6 A/ K
- ! M" g0 \4 n# K
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))$ g& x# u! t& [: [$ Z( @3 M4 ^
- {& u+ l* C5 V) o
- old_data = Sensor_Data[0];
/ H, ~) c. ]9 A7 x - return Sensor_Data[0];2 G5 Q# H- ^" W' Z H
- }
\) y% i% ~5 t( o7 E9 a - else
# ] X m: u1 |/ ~- G - {; u+ R8 H4 w1 T8 x6 y! d
- return old_data;+ S" f: b( F( c0 x) `$ p
- }+ Y- Y9 j) g) C
- }7 ~4 ]( F1 }0 Z4 o0 j
複製代碼
4 D& C* b2 Q6 v3 b3 PMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
j! F( M9 }9 G( u- m3 Z2 L
- e, y5 o0 O/ ^. p
|
|