|
|
# ?0 t6 Z5 q' c& f3 @$ `- #include <Arduino.h>9 h% P5 d3 ]$ t- D! \" z9 R) q
- #include <MeAuriga.h>& I0 ^2 J+ [2 x/ n C" s4 d: r/ {
- #include "MeLineFollowerArray.h"
9 ^# B: s4 G" i$ Y4 H$ w- z" Y
# m( S/ I3 C' U% O: x8 a- MeEncoderOnBoard Encoder_1(SLOT1); x5 V, E& s' X G# f( K
- MeEncoderOnBoard Encoder_2(SLOT2);
+ ~. R) M& ]- Y3 V5 S - MeLightSensor lightsensor_1(12);; _0 L" z+ {/ H% y5 J4 k" f
- MeLightSensor lightsensor_2(11);
* k3 u6 }4 z7 A& f5 x. G8 @) R( k - MeBuzzer buzzer;6 q R1 P- l) t9 e G' N( H
- MeLineFollowerArray linefollower(PORT_6);$ {0 \8 d) b# E( D/ X$ }2 I- t+ V
- " k. I7 L' s# X) g/ @* B+ O
- #define Error1 16 o {* O0 u+ A( ]
- #define Error2 28 ~! _' l3 }' ]
- #define Error3 3
5 D7 [7 S3 m- G0 Y# b( Y" t
s/ g z7 ]6 {, |+ V6 p- #define Kp 15
: s0 D! F. W3 {: ~ - #define Ki 0.15
$ d4 |0 c, L/ \5 r( o" G - #define Kd 0.03
7 R: J1 M0 l! f3 r; X7 R! J
! @/ S! \5 v& `3 X' z, Y. \& C; ~. H- uint8_t sData;! V) W! r" D1 A) b" z
- uint8_t D1;
[8 k7 f3 T# q - uint8_t D2;
4 T" ~ U) z! K. k8 y- @" k4 R - uint8_t D3;
0 |* K$ `) B$ p3 v7 G: j$ l - uint8_t D4;
* |- ~* V9 Q- i$ @9 i/ X - uint8_t D5;
# |$ D& q3 ]% e+ Y A - uint8_t D6;
5 R f8 z- O7 q" m0 f
6 ]$ ~: c5 l9 C* S- float previous_error = 0;
- ?+ H3 ~' h5 f+ S4 h/ ?* K - float integral = 0;. Y$ h9 K1 Q* O5 n; G4 Y; _5 a
- float derivative = 0;( t, ]+ J! z) W* u
- int Speed = 160;6 M4 ^2 ^0 ]/ ]2 J2 o4 r
- float output;
% m! D& @% D, g8 H
% k6 Z/ I0 P( D2 d, f- byte Left;! d' P8 j8 _! ^9 K$ I$ ?
- ! A" a2 f2 R9 {6 A
- void setup() & b( I( Y! v# |, C' \8 N! i
- {
8 q$ h8 |( l9 `# ^ W: s - //Set PWM 8KHz' o8 l5 f. }& h& f8 s2 S7 U
- TCCR1A = _BV(WGM10);
8 F7 b* q& O% a+ O - TCCR1B = _BV(CS11) | _BV(WGM12);) c" G4 \( _2 O+ Z
- TCCR2A = _BV(WGM21) | _BV(WGM20);
5 [( @- v, e. t3 ?* {, D: M - TCCR2B = _BV(CS21);( E: g j, L; y
- Serial.begin(9600);
/ ^, D4 x3 X' L8 P( P - buzzer.setpin(45);
l) }( f$ O8 S. C* i9 K# X9 B - }1 z% b2 E* h6 ]1 `: L5 S
- 4 q, ^4 _5 ^- v
- void loop()) } X! q5 s1 k9 p3 U
- {
* P. U! H7 f! q - while(!((lightsensor_1.read()) < (10)));4 x; O$ x- r1 {- M
- buzzer.tone(1047, 500);
) ]3 x1 A4 X7 Y2 S - delay(500);
$ D7 L& Q% g6 \& l2 Z6 C6 L+ u - do/ z- T# d9 ?7 c. f( U2 a p
- { m# W, g( r+ R0 R; j9 X
- int Err = getErr();& R& v$ A0 B7 s4 R6 z: R9 {) c
- if(D1 == 1)
1 I/ D8 }7 k+ I+ F& F8 P - {; q% J# ?2 @7 k8 E! Z
- Left = 1;
6 E D% W' a0 e$ S - }
5 V& i# h" l- w9 G1 X - if(D6 == 1)' d4 g2 S2 u- z* i" K: Z: z
- {
9 u4 ?; j% |! ~8 ~4 s$ Y - Left = 0;) w @- N: W7 ^9 S" F* P
- }
4 j4 ^) f5 ~2 M: u: x: U* U - if(Err == 99)
3 v* ^# f0 x r+ F4 a - {
& A' H) x2 D/ \% j4 e - if(Left == 1) X- h. W4 E2 ^! ?, q7 j/ m
- {
- M2 a2 |8 Q1 l( X9 z( m# w - Speed -= 5;' L8 K1 j$ \0 y
- moto(0,Speed);: I2 C; u- c4 R1 Y% [0 A2 j* u+ R
- do
; Z) m& s7 ^1 A0 t' [, a - {% J+ `( U+ z! r% {
- Err = getErr();
& b6 z1 Y' d/ j - }while((D1+D6) == 0);9 E* i% M4 u) G( J$ ~, C7 A
- }
" M8 E% A7 P9 H0 _4 q3 N - else
/ ^& c) o9 B0 y, K9 v7 O$ w - {
. |! q6 ?$ F+ n7 V) ] - Speed -= 5;7 Z a, I) h% L+ K, G+ [
- moto(Speed,0);
) f: I( j5 F- F2 d/ J6 u4 ^+ I - do. Z7 b- k/ q$ h9 [1 o$ ^; H
- {( Y& q9 Q5 U% C8 l. g2 k3 Z/ ^
- Err = getErr();2 o; j5 ^# o h) | F7 F/ P P
- }while((D1+D6) == 0);9 B7 o$ {: r1 J4 B
- }
* u: A# ~. O2 D) i' X3 w" h - }& b2 C6 Y+ S5 T1 Z" u4 E* O
- else. G5 Y; h6 W1 Y
- {8 M4 P; O3 v O+ ?( A+ n: p; l
- if((Speed < 160) & (Err < 2)) Speed+=1;
: J/ g# |; [4 }8 w) m: @4 |1 L - if((Speed > 100) & (Err > 2)) Speed-=2; 9 t- y1 B, ?3 @4 y7 o
- integral = integral + Err;+ Q) t8 c' i3 D3 }
- derivative = Err - previous_error;
" B; N, w& C) L+ Y1 _+ q - output = Kp*Err + Ki*integral + Kd*derivative;
4 [$ B: j: |2 w5 U0 i! R4 z, | - moto(int(Speed-output),int(Speed+output));5 V& e5 {: Z& N+ r* P) |
- previous_error = Err;' h7 e# \5 k8 V0 z5 K4 \
- }; [( Z# B9 R8 ~; E+ f" w$ b# q# O
- }while(!((lightsensor_2.read()) < (10)));2 @ ~/ A' h. h' p& C1 c& o$ C
- moto(0,0);5 D4 H+ C8 U7 c+ L& q. M W% H& i
- delay(500);
9 ~% n6 G1 T! Q0 V4 p - buzzer.tone(262, 500);
2 m) w! q9 q* i; k) W - }
* b9 v, ~- d* G& q - ; ?/ O; ]3 L3 v* ^' h6 Y6 k
- int getErr()2 h4 w$ p/ K8 u3 I/ M7 J
- {
/ T! S4 a3 u# o, l- L - sData = linefollower.getValue();2 q/ m e- m- z! U9 N3 G9 i
- D1 = ~(sData>>0)&1;
8 q1 J8 W4 E$ M" K: T- J - D2 = ~(sData>>1)&1;6 i) C/ Q" c9 ?
- D3 = ~(sData>>2)&1;5 _/ c$ n! s% ` y, A8 j, a
- D4 = ~(sData>>3)&1;
: H: ^& y1 I# z) N - D5 = ~(sData>>4)&1;2 y8 A4 {4 m& J
- D6 = ~(sData>>5)&1;
% w- o4 \3 x! H4 j - int downD = D1+D2+D3+D4+D5+D6;; D6 c2 U* v' J
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
3 p/ J! j4 w) S0 p" f; k - if(downD == 0); j' J9 K2 V) t: A8 I
- {
3 K% f8 u. b" d( Q" ? - return 99;
8 U. M3 j( I; C; r1 E7 _6 N - }: j' @4 U, X# Q% w9 V& [5 P6 j
- else+ S6 E" M* X' B6 y
- {
0 _& H- J1 M& h - return upD/downD;
, r+ g) s/ O7 l - }+ t: q6 u5 c; L h: N! |* Q4 u/ X, E
- }. f7 [0 j& u4 E2 O! @# S3 T
- ' ?6 k# V6 }9 b2 ]! g) X
- void moto(int Speed_L,int Speed_R)
+ t: o% B% ?5 C3 H - {
9 E0 i! a' H9 W0 w& _4 x* X* W) r - Encoder_2.setMotorPwm(Speed_L);
$ k' M' U8 l, T p/ U, A D0 M - Encoder_1.setMotorPwm(-Speed_R);
# U* |* p a' K( a9 v. | - }
複製代碼
3 B" c" y0 U' L9 @( y+ Y, kMeLineFollowerArray.cpp" W$ M6 Y. f$ p! \9 g2 G
- #include "MeLineFollowerArray.h"4 _( s ]; M; a, A7 t+ k
& f, @1 t5 q% P8 z" T' E- #ifdef ME_PORT_DEFINED
* F; ]( j9 }1 \( W6 ]2 [( I - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)1 G$ Z$ O1 q2 Y4 I0 z
- {) }7 N1 N# H- D. b7 `/ g7 h
/ d9 ~# v% S7 w- Q' }* V& @- }" \3 P9 F0 P% d8 l" a
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
( A# t# z; W @% l - {" h$ U/ H, }2 |/ f
- _DataPin = mePort[port].s2;8 T# Y2 O! h+ k* G6 y
- pinMode(_DataPin, OUTPUT); . V2 \4 _# O( w% ~0 ], Z$ d
- digitalWrite(_DataPin, HIGH);
9 J% d# S3 q& C2 d& I - }
/ |. }! z% e; s6 u" q" y - #else // ME_PORT_DEFINED& H! f u$ K0 \$ ^1 ]. }
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
8 Y) S z& Q3 q! b - {
+ Z t2 ~2 L6 P! D/ a7 c& w - _DataPin = pin;2 F C1 Q$ S! K, w
- pinMode(_DataPin, OUTPUT); 8 {& o/ }. p9 a4 | D, g5 E
- digitalWrite(_DataPin, HIGH);4 N& O3 \/ x0 @$ f7 u7 I* _4 O
- }
4 Y e; n6 u6 H2 E/ a4 d - #endif // ME_PORT_DEFINED
3 X( P8 B+ U4 A) {- L1 Q
% f# U m; x5 L* P& }5 p
w' ]0 ?( _3 E" M' g% j: E3 X- void MeLineFollowerArray::setpin(uint8_t pin)) |" R' s5 x9 E% Y: ~- f- J
- {1 O- L1 n( Z2 n/ ^; k3 r& w- N( c
- _DataPin = pin;
% m0 h2 R1 x" t- V: X# X - pinMode(_DataPin, OUTPUT);
' @1 A5 J+ s. f" m# c3 N( I w - digitalWrite(_DataPin, HIGH);
~9 | f. m& |! \% N
, e# r9 E8 G; r0 j- #ifdef ME_PORT_DEFINED$ E" l- u! |. |( W: R
- s2 = pin;
8 Y% I2 X. Q0 \3 w7 }' } - #endif
) l& A9 V1 Y8 f! T. K - }
( s1 Q" F% \9 E: N" [" J3 a - N3 k) \/ k! i+ z! O
- uint8_t MeLineFollowerArray::getValue()( C6 H, t$ D, N! e, h
- {! }6 F, F- }! n9 C) u
- uint32_t LOW_level_read_time;% \9 E: f: h9 t: w6 g
- uint32_t HIGH_level_read_time;
& C& e4 y4 D1 u4 L, | - uint32_t time_out_flag;
9 M, |+ |: O* ]7 X - uint8_t Sensor_Data[3];4 |2 _# W. Z1 s* A% a
- static uint8_t old_data = 0xff;2 c/ U; U# W, D( N1 M4 C
- 4 R+ _/ Y9 e4 d
- pinMode(_DataPin, OUTPUT);( p q! p6 @2 R4 [- [ B
- digitalWrite(_DataPin, LOW);$ y9 p- y- E3 W9 N8 p7 R% s8 ]2 i. ~
- delayMicroseconds(980);9 F& z4 t3 P. D' ^; W( R" |
- digitalWrite(_DataPin, HIGH);6 k2 c6 }. g( }! s- \* u# K
- 9 ]: B M2 p& n+ K' J: a4 L
- pinMode(_DataPin, INPUT_PULLUP);
4 F. I* d% e0 r1 _. E - delayMicroseconds(10);
n, I1 y# y# q
, D* c- ~; O. I' V/ i- time_out_flag = millis();$ e5 Z. f2 y, p& m5 p% x
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 y: ]: K0 p3 L1 |9 }& U' J
8 v& l+ y& M- w) E2 T& I2 ~- LOW_level_read_time = micros();0 y' A6 y+ c) q' B# R% j
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! L+ z+ F5 ?3 a4 X: k
- {
Y8 i6 y/ d% I8 W: l: i; c - return 0xff;
3 p9 k5 ?2 ^7 H' q) L - }
. m0 P) \' P: _9 p P - ! E/ ^, L. P$ d
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 M+ ~5 H* J* N; ~$ I2 K - 6 g7 \! Q2 m1 a6 L
- HIGH_level_read_time = micros();4 Q) W! [! k* Y6 @, x
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
# j! J$ X! r! o- \8 i/ K - ; S1 ~3 ^6 F" x9 q. C" m" _
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* \( ~7 q- m$ T
- {
. C3 g% B+ y T8 v& w$ ?6 @0 ^ - return 0xff;. |) j0 `! t" l- c: E
- }
( c- O: Z+ }' ~
' x+ t3 F# l2 ^) n- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))1 k! z# P. Y/ O8 p( V/ f9 P
- {
( f( \0 r! }; A! F - return 0xff;" m6 g4 `" F9 u
- }" M4 O2 z2 J' V' s5 ?, Z' j* X
- & A ]! c l G. H( F# h
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
1 _2 o" |6 V: I8 x$ }: |( V( e - LOW_level_read_time = micros();
% u: Y {) Y. i3 F* r - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level" a, R! m. \4 t# S2 Y! t
: o/ O; D3 }7 i2 l% f( `- q# m- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
( C7 L, |: Y4 R' I6 ^ - {3 p, W3 M# a3 |! J5 Y; |# m o
- return 0xff; w7 }) I \$ W8 `6 {
- }
& q! B& k$ D7 p! g. \' r - ( d; }9 G9 \" J* [
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
. ?2 U! L, ?0 d! _. }4 f' R: } - {
* H7 s8 j1 ]# F - return 0xff;
& u: K6 Q- [( H6 {* }- U - }! Q I1 v$ J) w& g, n) I! j: O
9 |, g* P9 O0 u8 U: W A' d- for(uint8_t k=0; k<3; k++)
/ g2 j }) M2 ~3 h - {( w" ?- p) O. e/ G" B( T6 x
- Sensor_Data[k] = 0x00;
. Z. ^: i" i. D8 `+ a - ) B# u/ X- N. ~. E9 A2 H+ Z
- for(uint8_t i=0;i<8;i++)
7 m! `3 c' o5 y i, Z3 v% d3 ^ - {: l8 Q9 v: B8 Q# R
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level9 k, Z( Q$ `0 @( m+ k) P* B% g
- HIGH_level_read_time = micros();
5 {! |% b' u, e# G& ]: @ - LOW_level_read_time = micros() - LOW_level_read_time;5 i) O, w B9 _5 m: t
7 f- W/ _9 `6 d3 s; i- H" U9 C# a- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
( a" z; E4 i6 ?" l3 U5 b/ J( M - {
) I4 A* V0 r5 \$ @0 E - return 0xff;
) t/ l5 d+ d4 t1 O9 j - }
( D- k8 c! w3 U* v( F8 _
: s! [' L$ k) ]2 _- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( }9 h# e. K& q! O! w# w: H0 h - LOW_level_read_time = micros();
+ { ~6 ?. |6 s9 e9 m5 |# [ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level0 ^6 e' ?5 b. A* Q5 `& P2 C5 D2 O
! |2 U2 N* \& e4 b2 i, Y- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
* k+ |0 Q: P! | - {
* a+ i) Y7 x& k4 H2 C; [1 o4 i - Sensor_Data[k] |= (0x80 >> i);4 o/ o. b; S# c8 z) @
- }$ s1 q- O9 ?; P6 F6 L! |" i& z6 E& Y
- else if(HIGH_level_read_time >= 100)
9 J2 G6 _7 Z! ?2 }2 u' X - {5 j6 S8 W _+ c1 a) x2 ~5 b0 L
- return 0xff;9 p: I1 y3 m; F O9 \
- }
6 F: S4 y4 a+ U* }- w8 E - % |" a# s8 M/ r6 B% {9 ]
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
1 A5 r. d; S" l P1 M* u - {
* m! o4 `9 L- I - return 0xff;
; p/ D$ D9 w! [ - }; e' G! Y7 t z" Y) E1 D) W& n) ]3 N
- }7 C) j0 i. y9 h
- }+ [; C9 e* O- G1 j! W0 P) x
6 d* R( U) Y. D3 G$ _0 a& @- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level) R' y. _, a( M; ^- X2 i# c
- HIGH_level_read_time = micros();8 ~: B7 \6 o' `7 {, b! s
- LOW_level_read_time = micros() - LOW_level_read_time;" r" S3 Q$ A# Y6 {7 a
- , E) q2 k4 n' n: G9 Z' p
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
" y6 D" h* G! Z/ D1 r& d - {4 t6 T0 D) }" d! o9 C: l9 J
- return 0xff;2 o, c5 h( w+ @
- }$ h+ C5 m, d+ F, k1 N
' I. f6 o; Q2 l% w0 m- pinMode(_DataPin, OUTPUT);1 R. [' h# A+ n4 M0 V( w
- digitalWrite(_DataPin, HIGH);
' ?5 C" Y. S4 _3 b" D( \% I - 2 L: `" c# N# E$ d2 d
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
# ^/ {; |6 N- c* o' p - {
$ e3 O- |$ r1 i t( \/ Q' N$ T - old_data = Sensor_Data[0];
: x( y% [) o: U# J) ? - return Sensor_Data[0];4 R# F" x; h3 b2 W
- }0 `& a+ v& L4 ]# e4 U3 |/ K
- else
. B# E7 H. `9 M- F* ~0 q" m5 P: I - {1 l* I: t3 r, l" z
- return old_data;
! [) _% H0 n+ |! C. o- N$ A! `& n - }/ d7 K% D+ r5 z: O# G5 m
- }) M. ]- t3 k H. o+ `
複製代碼
, Q) c& o8 t$ \7 M4 I1 YMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
: _6 `, P( M& q9 O4 R5 h$ }# c& Y4 ?' L
|
|