|
% S' u* I* }/ c- #include <Arduino.h>
; x' E0 s" u4 y- Q5 x# U6 G - #include <MeAuriga.h>
. F3 a% c7 @" s6 G - #include "MeLineFollowerArray.h"0 S; |5 A3 c1 f
- 4 `* s1 ^; c' Q3 J
- MeEncoderOnBoard Encoder_1(SLOT1);4 N0 M/ J9 f2 p* \" F
- MeEncoderOnBoard Encoder_2(SLOT2);5 Q6 U& _# H# n1 j' _4 U+ k
- MeLightSensor lightsensor_1(12);3 j4 |( q- B& I
- MeLightSensor lightsensor_2(11);
" _3 G- R4 q& Q1 h5 \2 T1 G0 h% ` - MeBuzzer buzzer;
1 A4 U5 U' |# W( r6 e( m - MeLineFollowerArray linefollower(PORT_6);- Y2 D4 o3 X& A
- 0 B( |: U- \# H% x/ B0 I$ V
- #define Error1 1
7 ?) P6 X8 g8 t% A3 C' C - #define Error2 2
4 D/ U3 H2 f3 ?2 V. h ^. F - #define Error3 3, N0 q( f3 _- @/ M! {' c
4 t) | q7 N9 @4 r1 c# Z- #define Kp 15
9 _. j0 T( q @/ h6 C/ d - #define Ki 0.15! q" f8 w9 U' b1 A. u6 H- r- f4 r
- #define Kd 0.03* k7 J+ @. c8 Y
- ) P3 G3 C, y7 ]/ P+ j7 w0 T& s
- uint8_t sData;( O2 j# z, e# ~0 {4 k/ }2 t
- uint8_t D1;, k4 j4 {2 E9 O" _; [
- uint8_t D2;( q C" R8 q7 [: e6 H, I1 S
- uint8_t D3;
, r, Q6 c& D% k+ N" ~4 f9 X( K/ p - uint8_t D4;' Y: C3 |6 z% K) ~2 r/ B% F' p; j
- uint8_t D5;
0 V! W$ W6 }; J, I$ H5 t - uint8_t D6;
" M% D4 J7 U/ J2 ]1 `
1 @7 x% U" j# ^* s( @: `- float previous_error = 0;
9 ]3 t2 m s/ C. I) {; _ - float integral = 0;
% w5 j; `! `- L! N/ u. L- \! X - float derivative = 0;
/ ]7 b# h& b# u; E: i - int Speed = 160;0 n: v* a6 b# r
- float output;
* }: J9 Z# h6 Y. H) P - 6 O6 i' j2 T1 S z, J1 y
- byte Left;5 c' Z3 \ @* s3 W8 Y% R; L/ K: P G
- * q% {6 Z7 w5 M% l! g
- void setup()
9 S) L! j4 T. d& f9 i# e - {# N: p( B. V. R. G$ G, J
- //Set PWM 8KHz% ~/ t, t$ ^$ r7 n/ A4 n. {7 W- ]: F
- TCCR1A = _BV(WGM10);# z" g: ?+ V+ v% R; i' Q+ @. B
- TCCR1B = _BV(CS11) | _BV(WGM12);
& c! {$ N, L* }/ m - TCCR2A = _BV(WGM21) | _BV(WGM20);: C! m: `" }2 L0 n2 N/ b7 D
- TCCR2B = _BV(CS21);0 L; ~1 i" r9 h( h5 h. H
- Serial.begin(9600);3 q W0 I7 g* ?6 X
- buzzer.setpin(45);) }- I* {1 R9 ?% x! }! {5 G5 B! K
- }3 i7 l) g8 ]5 d: `! h
n* \3 W+ q6 m/ l" K- void loop()+ k0 Z% p7 t) q. s. U
- {
2 w5 i0 f+ B$ u! g/ O - while(!((lightsensor_1.read()) < (10)));
7 u) W. n/ v" e# b; b' J+ ] - buzzer.tone(1047, 500);
* N7 ~, I* z0 w0 ?1 L - delay(500);8 ~- w5 f3 o' i
- do
! X/ p2 W: m Q6 z, G - {
( U8 f* g0 y7 M9 L - int Err = getErr();% i: ?" I" P. C" r; K. V# G
- if(D1 == 1)7 H1 Y/ L& g, x! G( F. w/ Z. a. `! h9 t
- {
7 @. D \$ ^+ z- m6 u- P- p - Left = 1; ~2 K7 H5 a# j
- }" C3 A! }- N4 c$ y. E$ R0 y5 V$ f
- if(D6 == 1)
/ A; B- b2 t/ h& S - {
" i6 M0 ?8 s2 ]* b( m - Left = 0;, J' m8 g4 l, t6 {6 d9 D
- }
* H: H" X* J$ b, f4 t; ~ - if(Err == 99)
; X6 R) c. q, ?& j - {
a$ \) e" c$ T$ H5 ] - if(Left == 1)
8 `& k2 ?) f( G# u- l - { q- `# |) C3 V0 z5 c
- Speed -= 5;
# x; {8 R' @5 n) P5 h% S) N - moto(0,Speed);# D2 H, o' U' {$ n6 c! \# Y
- do% t. f, F& Z }1 N$ f" k2 F
- { E6 @" X9 a7 ^! q" [+ }: a. K- S
- Err = getErr();" N' ~- _1 O( K. D
- }while((D1+D6) == 0);' P; H( s$ l+ j& h
- }+ a; i- b9 z& M% u
- else4 \0 I* g) V- L+ _$ D
- {
, g" A5 v3 w& Y3 H: B - Speed -= 5;
) E3 C$ b/ |7 g1 z( a - moto(Speed,0);
1 r4 e. @5 B1 w9 ` - do
0 [& Q4 |6 a# o* v6 [, G - {
+ C! h& S- J" l7 U/ I$ k - Err = getErr();
0 @& ?* K/ }: L% q - }while((D1+D6) == 0);
0 B+ j1 {' R( S+ M6 [/ W5 b4 W - }
s9 [+ T! W& R, S - }( l. z! C$ T6 H: n
- else; e! v, Q' u( B$ s6 G& F: I$ p/ ~
- {
6 |! s1 R6 @$ n) A - if((Speed < 160) & (Err < 2)) Speed+=1;
2 |- S8 _% O7 _( N$ U* Y - if((Speed > 100) & (Err > 2)) Speed-=2;
" D; d5 H3 M/ Q) k/ i( J: c - integral = integral + Err;
) ?- B) Y+ Z# n1 }4 e# T0 z* g( f5 c - derivative = Err - previous_error;
) V' H# @( }+ k6 g2 t1 c - output = Kp*Err + Ki*integral + Kd*derivative;; }5 k' j% m: G# `8 h, z2 X, `
- moto(int(Speed-output),int(Speed+output));. y7 {& z% F$ @7 X( M* O
- previous_error = Err;
6 w& d* u3 x' m& d; l - }
2 A' b- w* n: ? - }while(!((lightsensor_2.read()) < (10)));7 P+ b4 m$ O8 Y. h6 }
- moto(0,0);. [3 L- I. c0 }7 Z
- delay(500); h2 r# Y* R1 V2 c. Z# l3 M, P! T& ~7 E
- buzzer.tone(262, 500);
: C" U" _' R, |' w1 G+ X - }
- d: n# e: O9 T5 O2 _* ~5 [ - ' Y0 x4 f2 J& I7 s/ {) o, a7 L) s
- int getErr()2 |6 p5 `0 @% S5 u
- {
, _9 V [$ Z x7 y) \$ K* W - sData = linefollower.getValue();
/ E {. S9 h5 n, p/ }. m - D1 = ~(sData>>0)&1;
' k- a" n0 q/ c3 L6 { - D2 = ~(sData>>1)&1;
" K" `. y' H$ S6 g; O" i - D3 = ~(sData>>2)&1;/ d+ O1 d7 Q* i& O9 c# d
- D4 = ~(sData>>3)&1;
' d( ?+ m g a. g, q - D5 = ~(sData>>4)&1;2 O8 E( }4 E, {! M# {
- D6 = ~(sData>>5)&1;* x& q! \% d" u' U
- int downD = D1+D2+D3+D4+D5+D6;
+ _" P" Q" d& A6 ] g2 y - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
' a1 I6 P S( ^1 n - if(downD == 0)' o, U1 y3 b5 |3 \8 w& _( {: [
- {9 ~, K" L1 ~. l# \ H3 D
- return 99;3 P5 i- p- X. {
- }
$ D/ e. e9 Y% ~- z9 J2 K4 Z - else
5 Z6 B5 ?- {! h% [4 x" S2 h7 i5 m - {, i2 P* @3 L. D
- return upD/downD;' O3 L+ Y0 E, {5 J
- }: m# t* S' V9 ]$ n1 v O: i4 s0 B
- }
8 m( t0 @& |' ^" [5 f |7 |
9 f; p3 T& W! K" o- void moto(int Speed_L,int Speed_R). b7 f1 a9 t( _
- {
3 E9 }" _ C: t/ t# ?( V7 E - Encoder_2.setMotorPwm(Speed_L);
$ u" s1 s) i7 b" ]. U8 K8 j - Encoder_1.setMotorPwm(-Speed_R);3 L0 Z& B8 i2 b+ V# L
- }
複製代碼 ) f: x+ K* q* A' ]% H' N* p& Q- E- w
MeLineFollowerArray.cpp
) Y* p& C0 x3 h- #include "MeLineFollowerArray.h"
9 L5 |+ s) @, A& J
8 B5 x {. p+ H' e1 i- #ifdef ME_PORT_DEFINED9 _, ^4 F) E6 m. d
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)( j4 ]; T1 S( d1 X1 T" i) P3 Z
- {. ^! [* Z" T2 K" o
. j; \3 i1 W) v% w- i- }
* g/ y h" q3 o: M9 ] - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
# ]& i7 `. X! E7 J: [, J& [( s - {
$ t$ N( q6 o& t3 e+ c/ n - _DataPin = mePort[port].s2;+ D$ V2 h. T8 j
- pinMode(_DataPin, OUTPUT); % _! ~' V. X9 @6 B6 ]. I
- digitalWrite(_DataPin, HIGH);& f# ~ w2 y6 u* L' H5 B# k
- }
; X, w; Y" ?4 {8 o - #else // ME_PORT_DEFINED) l# Z) |( A% C- `$ e4 k% h* P
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)6 z# z$ b7 K- b6 {+ Q
- {
! Q0 {4 f" B. k) F- W - _DataPin = pin;* q# m# b, D; @0 W1 T% i& S/ T. y, i' v
- pinMode(_DataPin, OUTPUT); ) D5 W" G( P8 Y% C8 _
- digitalWrite(_DataPin, HIGH);9 U8 K3 {+ `0 `3 K% \
- }
. J7 X3 j' R+ M, ? h3 [- p - #endif // ME_PORT_DEFINED
# z1 T$ }: U7 l* [
6 F% t$ F. ]" |2 V, Q* ?+ H3 C- m- ; ?9 A0 s" b( L! `- `; [
- void MeLineFollowerArray::setpin(uint8_t pin); F" R( `4 U5 ~! ` H8 r
- {
* q v; l- ^1 n3 r- h) [0 g - _DataPin = pin;
2 v2 b, o1 K p) J+ S7 q l! Z" _ - pinMode(_DataPin, OUTPUT);
5 G" ]+ b+ g+ J: D* j3 v - digitalWrite(_DataPin, HIGH);
+ |% ]. A& H0 i
6 y8 Y% B5 e M0 H- #ifdef ME_PORT_DEFINED7 C3 f3 P7 X% h' j1 Z0 ~
- s2 = pin;
: N, Y1 f8 C0 J* l6 ^5 B - #endif
0 f `1 S3 K$ A# R8 Z/ x - }6 c9 l2 t N; \# P' V
0 Q" A6 A5 L) V- uint8_t MeLineFollowerArray::getValue()4 |2 v) n1 I( k& Z
- {
: h, A4 Y# u+ I$ N, p0 w - uint32_t LOW_level_read_time;6 i% a3 j& `) ?/ B
- uint32_t HIGH_level_read_time;1 o6 r# O2 a# D0 K, E3 g
- uint32_t time_out_flag;
# } N0 H) q( A - uint8_t Sensor_Data[3];2 M0 x# m& u N! E E8 F
- static uint8_t old_data = 0xff;
, ^. Z" a' p$ M6 k
5 }0 u% w, h$ v+ w- pinMode(_DataPin, OUTPUT);5 n) g0 D8 L" k
- digitalWrite(_DataPin, LOW);
( k9 b: v5 i' c9 o, x. h6 \- a - delayMicroseconds(980);
- R9 v2 K3 O' b - digitalWrite(_DataPin, HIGH);
1 R% O5 X/ t% D" t# ]
( [3 G; \6 ~. a5 r- n! Q$ E- pinMode(_DataPin, INPUT_PULLUP);
2 {( S+ N f& W2 I E" [, P+ L$ { - delayMicroseconds(10);1 J! R* o* z! t, \8 h, u* @
# E; c9 r. m* S l0 W9 w- time_out_flag = millis();
& L$ P& u7 `0 C - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ a7 P/ d$ U& V: V8 u" |
1 i M* I# `, z$ x P/ {) S- LOW_level_read_time = micros();( F; Z9 ]# T6 Y% e% w; ]# f
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 U. U Y6 }& @6 C! X: b1 W z - {* p% o6 R# d$ @% c
- return 0xff;+ z' g6 r4 I! _$ i4 z
- }
% Y0 J( X0 D) ^3 L1 m$ b, s' T& Q$ L - 8 s P v7 B4 _2 E3 P F& ]9 {
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: v0 j- Z1 B/ m/ l; y - / w# X, ?) e1 C' W5 T& D
- HIGH_level_read_time = micros();
* x$ b7 D/ [ o( F/ H0 |, d - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level) z3 V2 c- |2 j* e
: g8 n$ } v7 ]- o. o. L* m- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! l0 u# C% M0 N/ Y
- {
3 |' {& I0 d) |0 u - return 0xff;
/ z' k! X l/ B! S" h - }# U( u. F5 ]6 A* z' y# L
- 2 p" g6 C/ n2 d+ ^4 q5 B4 q
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
8 i5 p6 U- T# ^ - {
9 `: d9 p% N- d5 f! m - return 0xff;' u* `: P3 q1 q8 U9 Y
- }
7 ^9 D7 Y2 p# e/ J3 o - 9 {# ~5 h0 y( q X. h
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
, K0 L3 t+ B d) X - LOW_level_read_time = micros();
1 F$ ^8 j1 V6 m1 C. x6 C8 k - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level/ H" u& l/ l1 D
- & F( N( T9 i, i
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
; L" W. w W. _" I - {
1 D% h8 H+ ]2 I- F& i3 U - return 0xff;7 x) N1 l" [" \: C. I
- }
9 \" k" I, d! q$ _/ j9 V& E1 f( x3 S6 @
6 h7 T" M2 e) `9 O! g' p% E/ l- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))6 |5 }' B( [$ j9 P' ~( }1 q L# @
- {, K( t- h6 I6 x: g7 o0 G0 z
- return 0xff;
) v% Q( k0 Q) L1 J, z - }
: d& z& \' ?- O& C* {$ L) j- x - & ~0 {5 ~3 P! o& s! e
- for(uint8_t k=0; k<3; k++)) x0 o+ |% X6 L4 V0 R' l( }
- {
) @* T' Y d0 P& S8 S8 ]0 A - Sensor_Data[k] = 0x00;
+ F. q" {9 z4 d8 |9 M5 n" K3 A
! y9 {5 e! a+ b- \: C, \- for(uint8_t i=0;i<8;i++)
; t: A7 t4 }5 {1 Z - {
. u2 g* m- e& S) ?6 c8 R: k: P- T - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
, ^/ N1 F; }* d- K# n. q% c - HIGH_level_read_time = micros();
" Q! ^2 v: t4 I8 R6 W - LOW_level_read_time = micros() - LOW_level_read_time;
, g3 x( U$ \1 q/ _1 W4 B G" u. Q3 ^
& h* H. @& Z; Z! o- ?- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
. V/ C# I" B. N' Y4 D' V - {
0 u5 \" M! E/ g& m/ @ - return 0xff; J( F1 ` E4 T
- }0 r# r5 e9 n% ]
- " x" A0 h% i8 {# S
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
& C3 C; |1 V& Z - LOW_level_read_time = micros();0 H9 h! ~; N! a- w. a. ?' i; P
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
. u- C# r5 \1 F2 ^ - . D( G+ }& p; s. r" s
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 13 v6 n( m* j9 q) s$ |
- {: i8 z, Z1 |* V5 T# I9 X
- Sensor_Data[k] |= (0x80 >> i);0 ?! X8 i1 H" j) J* ? \3 P# l( G
- }
7 D3 e; d& ^$ \( B- x - else if(HIGH_level_read_time >= 100)
4 I2 [: A! l5 c$ W7 A - {
/ C( x0 {: W3 G9 E1 i# G - return 0xff;2 Z5 t4 i; j; c. A' g5 ^8 S
- }
9 O2 f& K1 h }0 S
+ s/ O! M& K8 ?: k- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
c9 r/ y& X3 \ R! z( e) _, t - {$ Q, R9 S/ M) v5 i$ f% R9 z
- return 0xff;' J- p4 p6 r4 C8 K
- }- @! Q2 K$ {, H# ]$ P" J
- }
+ V/ }$ N) t; T# U9 S% f7 g5 M - }
7 j. A- u; S9 h$ @$ g6 V! m
8 E2 h# v0 l$ g8 d+ c- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level, _* X2 p6 ]1 X' ~" F" q9 W
- HIGH_level_read_time = micros();4 D& Y3 z2 Z1 `. n* H+ @
- LOW_level_read_time = micros() - LOW_level_read_time;# F: ^1 e6 s9 K. V
( b6 K5 z" n- _; m9 @- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )3 H% ?0 ?1 g) D4 s" C: c' D: ?
- {# d4 Z* C0 B0 S: p5 n
- return 0xff;
H* q0 b! E% [ - }
* W$ U5 A5 @* i! ~4 Y
8 [ r2 M9 @" v: m) y0 E9 K! l- pinMode(_DataPin, OUTPUT);
- Y4 K5 \0 H# o% u% P# I' S, D - digitalWrite(_DataPin, HIGH);/ }9 J7 P: ?2 p1 Q, ]" x
x0 V. I) L. W; q' }+ o- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
! s& B/ E/ o' K! e8 Y7 i7 Q - {
7 p2 k) ~+ s+ r) h; J J - old_data = Sensor_Data[0];
+ ~8 x2 d( F! B9 P - return Sensor_Data[0];: Y; z* S Q0 i5 o+ ]
- }' J* P1 [4 c& g
- else
) ~2 N) x4 X2 D& E8 X! {/ F; } - {/ n6 R" j- f! z. m" Q
- return old_data;
* y: i% l% }) V2 M - }& v, J* L7 z6 F* A0 H$ z
- }& p8 j, J) ~( m3 T( M1 O! E. X+ B
複製代碼
% K7 n; P. u1 ?. Y5 q; e* N7 g% ]* PMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
. W- ~! A6 i, L8 p5 h
J' ?* Q: j" I% C. f, l- i |
|