|
|
z8 n# y9 K% f" `: k6 z9 c- #include <Arduino.h>
- N( y7 ?2 l0 g; `& U - #include <MeAuriga.h>, ~/ V6 R+ F/ G4 t2 \
- #include "MeLineFollowerArray.h"
+ F8 r7 O f( f0 Z3 x2 _2 z - 8 H- c# d6 L6 D( f
- MeEncoderOnBoard Encoder_1(SLOT1);7 Q. k6 W4 S, p- ^9 C) g" O
- MeEncoderOnBoard Encoder_2(SLOT2);
& B* |% z9 N0 z3 T - MeLightSensor lightsensor_1(12);3 H$ ^/ Y2 V5 D. T
- MeLightSensor lightsensor_2(11);
" w9 p |% F5 @6 N1 o2 ?2 [ - MeBuzzer buzzer;& ]+ `! V; c" N. {& v5 [1 B" C
- MeLineFollowerArray linefollower(PORT_6);/ m' Y7 l( X3 M0 S# R* J8 `+ F2 s
6 t4 H- Q$ x/ _+ W& N& `! ?- #define Error1 1) E( ]" R' n7 @ X# }
- #define Error2 2
8 W; l; S& y4 p( T - #define Error3 3- w B7 j0 J$ { P( J& I5 c
) z, X' d5 N1 k+ {- #define Kp 15( V* c s) x, g0 O
- #define Ki 0.15
& `& {! Z Y9 d Y; {' Q - #define Kd 0.03
/ T/ K: g! o# t% d7 J9 `4 ~9 y$ u2 n- J - : k ~+ L" ]! _- y
- uint8_t sData;4 F5 K; h) `& C
- uint8_t D1;1 Z0 y7 e8 v2 H) ^! {. A
- uint8_t D2;
: C) ?& w& Z- M( s3 b) | - uint8_t D3;
7 j: m( k) P4 a - uint8_t D4;0 \# ]" p! C; h
- uint8_t D5;
; y4 X* Z6 U2 m/ d - uint8_t D6;6 X) t1 W- \! b! w! w' s
- # H9 ]! k( u$ C: H
- float previous_error = 0;3 ]: u( c# }) K9 E! F' o
- float integral = 0;0 ?$ `+ [6 J: s6 r; f" S
- float derivative = 0; {6 l+ e5 Y( ~' r6 C* p: _
- int Speed = 160;
% `2 D2 Y8 C) b - float output;9 b3 P7 e" c% E* i
' ? [0 u7 @& v- |, h$ a1 ^- byte Left;( w6 a C; U' x, d' W6 s
4 M2 y- f! h) H- void setup() - e* S! s& b' h" p, F, J
- {' N. x5 E' Z; c
- //Set PWM 8KHz* Q" b2 H1 E. E: R" m
- TCCR1A = _BV(WGM10);
/ C. U( q, b- N+ P; t: {+ p0 M. [- X - TCCR1B = _BV(CS11) | _BV(WGM12);
' U0 l6 o3 H. r: T' j+ W8 G - TCCR2A = _BV(WGM21) | _BV(WGM20);7 X ~# j3 [. T$ Q
- TCCR2B = _BV(CS21);
2 A" }/ K8 k, I3 b+ m. z - Serial.begin(9600);
5 z# y, O" L5 C$ @9 e - buzzer.setpin(45);
! Y" g2 u% n% ~7 ` - }) \! H1 m0 c0 y. U+ b
- 7 N& v: }) e$ F+ V
- void loop()
8 U r; ?! `; [; m& @6 T - {$ T/ q. u" m# |" y" |
- while(!((lightsensor_1.read()) < (10)));( t) F! ], H# z3 ^
- buzzer.tone(1047, 500);4 M& L7 A# Z _! ?9 \
- delay(500);
; u. {' W/ N3 U; ]+ ~6 P - do
6 l5 k0 ~8 _# t3 Q - {. x7 s; v& l/ g& G
- int Err = getErr();
% ~) ^5 W4 N3 R; G2 r9 M - if(D1 == 1)) e3 ], w0 n" s( ?
- {
`$ M: F, P0 O' I; e5 N" u - Left = 1;
/ f7 D( p1 e) ^3 y9 G. g( v6 s1 ] - }% `/ v7 S1 A O, m
- if(D6 == 1)
) B* O J0 C, e* X6 K9 r) x - {6 V: p) H4 m3 t5 d! F3 E
- Left = 0;8 _, Y8 s; x( N- x5 ~- i4 O
- }; U' f, O, d, C
- if(Err == 99)
# |9 ~1 R+ O5 u; P - {. H, b0 Z7 n6 r
- if(Left == 1)9 U$ L% k" c0 D! g
- {
. _1 m5 z! T1 k; d2 m" h% Q - Speed -= 5;
" r" o" J3 C7 Z4 Q4 I0 M* P$ \6 ]; A - moto(0,Speed);
% W* f) ^. V: I$ S - do
7 H. V0 q0 Q4 L - {
4 }+ `% P4 ?8 E - Err = getErr();
7 b2 N2 \0 X! u2 _ - }while((D1+D6) == 0);2 F y U* V. b: J# R: \' v, i
- }. q; u2 _$ Y8 F. d% h( i
- else* H) _. |4 | t/ B( |- R
- {
- U& e5 g5 h9 j6 | - Speed -= 5;* O2 q* \9 X4 s n
- moto(Speed,0); 2 S& E2 N$ c6 k* e) _3 `6 T4 a. t) O
- do* }5 @, m$ M g I2 T3 r
- {
- z \" R% @& ? |9 x$ e - Err = getErr();
7 @* B+ ?; t& T1 G3 v - }while((D1+D6) == 0);
- a4 N8 W Y8 L) R - }1 I2 M# n8 s! H4 z
- }
% Y) }: X7 g3 C - else
4 x& s: \3 n2 |1 L+ V5 c - {
3 l# I) V5 ~. Z6 R, H9 n - if((Speed < 160) & (Err < 2)) Speed+=1;
6 v# }6 a+ d- G0 c8 [ - if((Speed > 100) & (Err > 2)) Speed-=2;
0 b5 `- S6 L* j% Q - integral = integral + Err;1 q9 T0 a5 ~! L6 s! z7 ^5 @
- derivative = Err - previous_error;
+ E. `# p$ f! |; ~- W z - output = Kp*Err + Ki*integral + Kd*derivative;$ G( u8 u3 I' O- n2 ~
- moto(int(Speed-output),int(Speed+output));
8 W2 r1 q( Q- v# C2 T - previous_error = Err;8 Q4 e' U) U0 b ^% }
- }$ U' W" ~% P2 @6 n( T- j7 H9 O5 B
- }while(!((lightsensor_2.read()) < (10)));1 z- E A' @+ s% E
- moto(0,0);
; q1 M4 V* y- Y" s1 i - delay(500);) ?5 |2 Y: Z0 }
- buzzer.tone(262, 500);
# _4 x4 S! A, q - }$ T7 t2 T" l8 V N! B
3 c5 A" @/ B6 b; }' f0 `5 V8 i) L- int getErr()) U) p3 h& _! U+ m. o/ a, G2 s
- { + L4 g# P. @+ {0 c% Y. c
- sData = linefollower.getValue();& }1 O) f v0 { M1 w
- D1 = ~(sData>>0)&1;
6 Z: g6 l$ h( O- e3 e - D2 = ~(sData>>1)&1;' E& [4 Q6 i* Q ^
- D3 = ~(sData>>2)&1;' y, y. n, Z ]2 Y2 c0 ]5 S) y4 f
- D4 = ~(sData>>3)&1;
, f7 F% m1 a0 b - D5 = ~(sData>>4)&1;+ j$ `$ o$ a# l1 w- X
- D6 = ~(sData>>5)&1;
, d9 J2 O$ K# p4 k7 r9 h - int downD = D1+D2+D3+D4+D5+D6;
' r8 O+ V+ ?4 k% c* K - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
% ^" [1 L+ b' X; \# G - if(downD == 0)
+ `6 w& O* i2 h; W' F) D7 i0 ~2 i1 Q% { - {
% A& b- R4 c, v4 O( E! _, S- z - return 99;
3 H: \: N- d6 j - }
9 o0 {; C6 f$ }. f2 }) h, X7 J - else& s% L* I8 g9 l. N7 U1 |* g( X7 ]' ]
- {' C. ^# B8 @2 r+ T
- return upD/downD;
' C# o% ]1 V4 K - }3 B$ r* m7 E- j# d
- } t4 N* B- x0 ?) x' V) }; P* q
7 F p) Y$ F w/ t- void moto(int Speed_L,int Speed_R)! R: l7 J/ w8 ~! ?$ `! X
- {
2 W# L; h! a. C - Encoder_2.setMotorPwm(Speed_L);
7 H+ q6 e, p6 D - Encoder_1.setMotorPwm(-Speed_R);8 n# N& `% Q7 p* P G
- }
複製代碼
' w( k& j# e5 H( tMeLineFollowerArray.cpp
2 M2 K1 S# @. a: z) @: S% `- #include "MeLineFollowerArray.h"" n( ^2 o( ]+ R4 s$ Q! x
1 n1 U" u8 i) B# ]* ?5 O- #ifdef ME_PORT_DEFINED; \0 c [/ o; U$ S' A, R+ L
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)' E" R: F, ^2 s3 ?( W4 m6 f
- {
% O. r6 e6 C- T$ F: }# _8 p% e8 }) l
C; x7 k' w+ e/ f3 z- }
. r# u' K& g& l' `" k - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)* J; a$ l* U2 B* H& ~7 H9 w
- {5 e7 w9 E/ n% ~4 p! O+ j8 M* {
- _DataPin = mePort[port].s2;
. ^/ S! {+ B1 r I; S2 K( e! P8 @ - pinMode(_DataPin, OUTPUT);
( u5 R R8 n# L4 H - digitalWrite(_DataPin, HIGH);
8 ^* L0 [% }, m - }
' O# ?: n. Q6 H8 y4 V9 g8 H - #else // ME_PORT_DEFINED6 g1 v0 X5 @5 A, c- h. N
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)6 o- s0 e- s+ N0 Q# w% M5 I% j3 g. S5 m
- {
1 t$ n9 H" w& [$ s& Q- v# `- _- t1 k$ F - _DataPin = pin;
2 m% F. N3 S/ I - pinMode(_DataPin, OUTPUT);
" r3 n- Q/ C2 J. G% E" x) i - digitalWrite(_DataPin, HIGH);
3 u9 B/ I7 H9 e6 \* e1 i9 U - }
; o3 A+ J9 x5 Q - #endif // ME_PORT_DEFINED8 ]0 p8 n$ }5 Q7 ?% L
- : {! _; |* U( k, v- q" m' N: }0 [
9 e1 p! P+ k$ Z" Y- @! j/ \ i- void MeLineFollowerArray::setpin(uint8_t pin)' Y L' \ V! S6 H! x0 Y: d
- {
1 Z; ^+ S8 w7 ^( n/ ?4 c) E - _DataPin = pin;
& ^3 _! _" D& n' G2 z - pinMode(_DataPin, OUTPUT);
5 S/ o+ m% T+ F! s9 G" P - digitalWrite(_DataPin, HIGH);0 \3 a& Q# k5 E% t
- x5 K- L! u' R+ z( O- #ifdef ME_PORT_DEFINED
; r4 a4 E; h5 H* _. h* T, d - s2 = pin;
) C2 d. p, G# e+ W - #endif7 |. `$ Q& H' p5 O
- }
& T. D) \- O' `7 d - . U, B; b* R! i; Z. w: z2 [
- uint8_t MeLineFollowerArray::getValue()
/ I8 [/ p1 q" }/ p - {
~2 U$ l& N4 X9 I) F - uint32_t LOW_level_read_time;
4 Q* l& @. D$ P, y( G! o& Y - uint32_t HIGH_level_read_time;
; |8 h) |9 ~4 [ - uint32_t time_out_flag;& y" \4 L0 u) n+ Q" f* [/ }
- uint8_t Sensor_Data[3];# H6 b8 p& N9 P% T7 k0 U% R3 c7 k0 a% _- n
- static uint8_t old_data = 0xff;
$ L4 u6 K: H( A0 w& Y+ p# E - $ e. [5 ]6 c' ^2 }* z; M1 P& {
- pinMode(_DataPin, OUTPUT);0 N2 ^; L0 @+ X. s
- digitalWrite(_DataPin, LOW);
o" u N0 d4 `% a - delayMicroseconds(980);8 J5 ?3 [$ p4 U* ]/ y! l* `
- digitalWrite(_DataPin, HIGH);
8 {7 M8 B6 p( ~8 g+ f2 j
& f' y$ d* k2 Q+ h- pinMode(_DataPin, INPUT_PULLUP);3 I) @. ~. w8 E, q7 H
- delayMicroseconds(10);
5 I' U" Q5 U% T+ ~ - " \! n/ t. q: a
- time_out_flag = millis();& b6 Y/ e0 @" Z
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. L# X7 I9 ?- e7 n1 H1 g
- 5 E8 f0 A: `0 [" L/ a7 i1 O% z
- LOW_level_read_time = micros();
& r( o1 s N' K6 M* B - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 c! x4 o# D+ g - {5 u( S, ]8 `3 Y. c. v8 o5 y6 L
- return 0xff;+ H) h7 l' d( {0 E3 ^
- }- J) V+ a) d. d, l: u8 E- r
- % i0 y% c3 k: w* S [
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ i8 c& x$ ]' F* f$ S$ v* T
$ D: {7 {' ?0 \3 }) i7 a6 r- HIGH_level_read_time = micros();
- z' J0 P! O; M/ o: f - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
6 [+ ^% ?8 L' @0 f1 l7 D - - C ~1 N# c; C/ F) i
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
- X+ c, r7 |% c4 \ - {, }% F. M+ V% y) [7 \9 I( ~) ?
- return 0xff;
1 O$ X) G+ m* C6 d- Y! ]" m: i - }
# l3 O8 T' `0 r, r; ~2 Q7 f - $ p: P4 ]( m% W2 O: L0 ^1 J
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
* Z0 e4 B N) S - {
# G- \1 [* M1 b" o( V: }, W - return 0xff;
9 T9 b8 b: F* |8 _4 D3 E7 W! U( r - }; F* N# m- j1 `& T$ f) s. u
" _5 ~' K5 l2 b- `; l( t+ t- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 b: C: o! u: [" L% ~
- LOW_level_read_time = micros();
6 v1 o" Q0 e J - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
1 l% L T1 A G - 8 E! b: V! B- U P4 g9 a
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 X7 _# v6 S! G( `3 Q) f - {8 r K" S- O2 [7 U o
- return 0xff;2 |1 F3 h+ j) T; ~1 c, G4 Y* }2 o
- }
. i+ k( U3 z0 v. @: K: U# }0 X+ p - 8 ~5 C! ~# o2 m/ I1 P
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
) c6 y1 a3 S* F; n; s0 c$ p w - {7 b# V* k/ K7 F/ x
- return 0xff;* h6 _. @/ b+ P! \, K0 z
- }
% g1 q/ N* R4 V3 M+ Y( L - 9 `, H/ x7 q: L3 ?' b/ T M D
- for(uint8_t k=0; k<3; k++)
8 O1 m- [9 f$ N/ m( ?: k7 t6 I - {. e: G. U5 f1 u" _9 @
- Sensor_Data[k] = 0x00;
/ ^% G& e$ V) ?/ I4 T
% Q& d0 Z9 o$ P9 e- for(uint8_t i=0;i<8;i++)
2 S) g! N0 x; ]8 A# ]4 i/ W - {' v1 x# I j: B* f( Z+ ^: v
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
' X1 e x" V, Q) b& ] - HIGH_level_read_time = micros();0 o7 m3 @6 ^ i% q, {. ?' T/ B
- LOW_level_read_time = micros() - LOW_level_read_time;
7 s3 {3 |3 U1 ?3 X2 R+ t - - K1 X' M& C, q, N
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
& l. U% c1 G1 x2 l6 o& e - {
# ^0 R* \# X% Y* m. q' V) A - return 0xff;$ m! x$ Q# j' N+ |. u
- } Y) Y% i9 T/ {, n M' A6 G
- ~5 ^. g7 J3 F: X9 `) H: U
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );# `0 e v. c0 E F, ~
- LOW_level_read_time = micros(); ~ r5 C5 L1 }+ c+ h* }3 i r& }
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
& ?; C, l `8 J! J2 B
7 d: B) N. I& W, X5 l% W+ T6 |- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 11 W: n- P \; g$ D5 l. P
- {& [& a/ G1 b1 w
- Sensor_Data[k] |= (0x80 >> i);7 y6 F' l/ B4 L ^
- }
( o3 Z7 i* X) D/ f - else if(HIGH_level_read_time >= 100)
& |7 g2 M8 U0 V) ? - {% o5 u! b1 K) ~8 d) D; o
- return 0xff;
" s, }8 T1 r1 z& k - }
7 V" u# D" x0 {
8 Z5 V& I( Q3 X, [- M# J- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)6 t" S( }6 r O$ I. R& {
- {# ^* O( A! N, W3 e
- return 0xff;/ A& O7 z2 z e& n9 r3 O
- }' ^9 F& I. X& W: \ m' W0 M) r
- }/ v- O6 g8 A1 n
- }
) b2 M5 i6 q3 p9 K5 n$ J) m
, [. z% E ~, z: @) T' q- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
& \( y; P, Z+ X$ H% \ - HIGH_level_read_time = micros();3 r- r4 ^! O) ]% `" V; ~( G+ |
- LOW_level_read_time = micros() - LOW_level_read_time;/ h5 e% g1 R% y8 b. ^' S
- , N& I" @8 {) J0 Q$ J
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )/ _6 U V# A; S$ r2 u& X2 }
- {
, H7 L# B* S6 ?1 d% Q. G. f: R - return 0xff;/ b4 S6 f6 h* L* f `" d
- }4 u1 S R3 y+ o- S, R1 ^
! ~5 q2 @* O1 C- pinMode(_DataPin, OUTPUT);' h9 L, E7 @! ?1 P+ O
- digitalWrite(_DataPin, HIGH);
: }8 c+ Z' y1 W1 L) |; E - 8 W9 q+ h; q1 \
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
1 ~2 K* {" J! x - {( d7 U( z# N* c: N
- old_data = Sensor_Data[0];$ i ^6 W# s8 e7 j: g6 @
- return Sensor_Data[0];9 @3 I: q w" q9 S/ j1 V
- }
& ~- a8 B1 U* Q3 e4 h2 [, F - else
5 X. Z$ d1 U- \' G0 f. A4 U' E - {
* H% H4 z1 d/ V% o {4 A - return old_data;6 a, r5 m* }. h& ^2 L3 D P2 N7 W" g
- }" D5 Y+ O1 [4 y' V
- }
# r4 a- C8 d g$ p. Q$ r9 `
複製代碼 ! I* F1 A3 o% T) j
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
- w n* J, I. k! V5 u$ A$ u
! U; b- A( P9 N9 X& [3 i |
|