|
|
7 ]5 T( k) }" }: C- #include <Arduino.h># Q3 s- |9 K* H0 ~
- #include <MeAuriga.h>
; l& Q4 t7 U/ p9 r) C - #include "MeLineFollowerArray.h"
. M; W( F' L( |% [# ~3 k: c - ! E$ P! l! u% }
- MeEncoderOnBoard Encoder_1(SLOT1);
8 J2 G: f! D0 x* a; M - MeEncoderOnBoard Encoder_2(SLOT2);$ s) g0 p- y$ r2 ~% G
- MeLightSensor lightsensor_1(12);
* t9 c) V0 U% A7 x8 S2 d - MeLightSensor lightsensor_2(11);9 u2 y% k* n% {% ]9 c
- MeBuzzer buzzer;" m6 Y1 S% ^, F: p! S# u- z
- MeLineFollowerArray linefollower(PORT_6);
" @. Y2 x0 u3 d, \+ D7 r - 1 `3 f8 `9 B% d4 H& T
- #define Error1 1
+ b& H4 V7 e% B# |' G - #define Error2 2
; h: {6 o! V5 ]$ |7 e6 n - #define Error3 3' n R7 G" M: A1 y6 E( p
- 7 P( B* v/ y( l" A3 D. V' _
- #define Kp 15
C0 K/ _3 ?- ^* R( N" S1 R3 R - #define Ki 0.150 T1 n, B* k0 W* X
- #define Kd 0.039 q0 {9 u8 a3 [
- " T- \( g2 E0 L' E
- uint8_t sData;
/ M$ Z8 ?& |+ D/ I z- P5 I) p8 o6 J - uint8_t D1;6 o; L8 t4 Q/ `
- uint8_t D2;. @7 m3 }) B- {! ?1 x ]/ M2 O# Y
- uint8_t D3;7 B( k% B2 l$ X. @
- uint8_t D4;9 g8 C! q& {6 e6 s
- uint8_t D5;4 O( p Y/ Q, Q! q a- F0 a
- uint8_t D6;
. \' y( m, `4 c$ U5 O# V0 ? - - o, }# a; m$ W+ f) J5 D0 b
- float previous_error = 0;
5 c4 n" [" y o9 F& V1 W; K6 N - float integral = 0;- j1 |* g# _! z
- float derivative = 0;7 P" m6 M$ x6 f' e. o" O) z4 q" g
- int Speed = 160;
9 ^3 P i, l2 O1 S - float output;/ p' a3 W/ K' s1 u/ J
- ! v2 _% J( Q/ W# u
- byte Left;
+ \1 p7 \- [: `0 ~9 j
2 _7 |+ m+ e% q- P6 h* ~, f- void setup() 5 O3 B* f* K( Y6 @- K: o4 C- @
- {
# s* W/ k( r. T: v `2 `$ X7 C - //Set PWM 8KHz
5 i+ _# o/ a3 ?. t - TCCR1A = _BV(WGM10);
+ `( R5 A9 g1 R. {# u8 U4 `0 g - TCCR1B = _BV(CS11) | _BV(WGM12);: Z+ N6 W E; u, X0 a( C
- TCCR2A = _BV(WGM21) | _BV(WGM20);1 c0 j& M) c; N: a x# h9 c: E
- TCCR2B = _BV(CS21);4 @8 E1 h% I, O! [# q0 `6 i# m% N2 D
- Serial.begin(9600);0 ]6 _2 H! i. M! ?4 ?- A
- buzzer.setpin(45);
6 R( e$ L* @+ ~/ }! j/ Z - }& R3 I" i% Z& O7 ~- W% H
% K! @/ S& w! J- void loop()1 n3 n0 P1 B" L0 B
- {! y6 l& r; m2 y& k* `0 _
- while(!((lightsensor_1.read()) < (10)));8 G. z2 O: M$ Y) Y+ _1 ]" `; E
- buzzer.tone(1047, 500);
, F0 \( F$ e O" s/ v5 E" A - delay(500);% L2 F. p+ K( D2 t' B2 v
- do
5 v2 w# G/ u. P# V - {
[9 P# }% l4 H- d7 K L: q! D& Q - int Err = getErr();
3 i' Q1 E) A. F/ V1 a! V - if(D1 == 1)0 E5 f) n9 P' @& B } h
- {- T! M. D/ K8 H% F+ v5 V
- Left = 1;
* U- l) v& J5 l+ ~5 @9 t - }
3 \( H H. j- M" H - if(D6 == 1)7 ?0 q$ g$ C) ?2 T9 A1 W
- {
' }5 U0 N4 \2 g) F; p6 s. A Z/ q1 t - Left = 0;
/ \' R$ y! M0 q1 t - }- F- `0 f( h# e: b
- if(Err == 99)+ s( Z4 ]9 n& |) ~2 ^
- {6 B$ o0 t5 _. b! |0 l: }
- if(Left == 1)
) G# W( F" g7 L( E6 d' \ - {4 j/ g$ x$ @- n7 u v
- Speed -= 5;& U U; p. O5 e3 w$ r
- moto(0,Speed);$ P; y/ L3 y6 \9 c6 s
- do
- A- L8 i/ n7 d) u - {
) ]+ U/ d' Z5 J - Err = getErr();$ r/ F+ v& k: L, A
- }while((D1+D6) == 0);# o' ?. i4 n! Q; H
- }1 y: z2 E1 @3 E6 P, m
- else
/ J0 [: C1 X* d/ D6 w - {" p+ ?7 t! m$ V8 F" u( z
- Speed -= 5;
e( \0 M# k4 w X, l; _9 W( J - moto(Speed,0); 3 b( v' o7 y" D6 o3 l3 t& y. t
- do5 Z) d0 X. \5 N- n
- {* p4 ^# Q4 n/ i3 C7 q6 H# u; Z% Q
- Err = getErr();
7 @' i5 Z; B0 j7 v o - }while((D1+D6) == 0);% |: s. ^/ L( ]4 E: w1 N9 M
- }
" E- Z7 V9 J% [# \8 x( y4 G - }
& s% B( H" `" E$ k9 U) O0 v - else1 Y$ p. R0 V V* V
- {1 q2 ^* o; [# G* o7 \, T# s: ]
- if((Speed < 160) & (Err < 2)) Speed+=1;
4 m P5 }0 q3 X2 l6 b - if((Speed > 100) & (Err > 2)) Speed-=2;
s: |$ Q) F( p/ k9 J. @5 T) B6 B! j - integral = integral + Err;! g, z; h! E; o' c( n4 o. F
- derivative = Err - previous_error;1 J! Y2 k* Z2 f: p
- output = Kp*Err + Ki*integral + Kd*derivative;' o# g, H/ \6 i1 I/ b B8 ~$ w6 u
- moto(int(Speed-output),int(Speed+output));* k; |0 C" f: C3 b
- previous_error = Err;
: q$ }& {. d% F( ~5 w8 F - }. l. [" P/ A+ o5 Y
- }while(!((lightsensor_2.read()) < (10)));
2 ?5 Z9 {3 ]( q& S* ] - moto(0,0);
' \( C# g3 B/ O2 \/ j - delay(500);6 Y! X/ h8 N+ o; n+ l3 i) T6 o# h
- buzzer.tone(262, 500);
6 }2 Y K, F" ~) j+ R! b; P/ [. j8 A2 e - }
& `% B8 Y: c, P1 S - + u/ `5 `, E( Z$ o
- int getErr()$ j6 v$ ~( f6 f/ x6 N1 x& h
- { 1 P9 f9 q% q( x" \
- sData = linefollower.getValue();
5 g U: l. [5 Q/ I2 D+ p - D1 = ~(sData>>0)&1;
g1 S2 l! c: { - D2 = ~(sData>>1)&1;
) X, D. s. u V# v8 x - D3 = ~(sData>>2)&1;" Z* \% x. G6 F" e* w
- D4 = ~(sData>>3)&1;% o- m, q" x; Z. D7 U
- D5 = ~(sData>>4)&1;
9 U, z; h/ ]& l3 h - D6 = ~(sData>>5)&1;
) R) r. r! Q0 e W - int downD = D1+D2+D3+D4+D5+D6;
N. p* i1 m1 j3 T - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
2 e2 q4 c& Z" C5 i& T* N, d - if(downD == 0)
( r" h5 _0 _ ]' }- n: ], U - {. P( W/ v1 Z% U6 J' \3 ]
- return 99;
9 c i4 }! n0 d, g - }0 w0 a2 E0 T% ?3 k9 \
- else0 @! a' q6 x2 D4 A b. B Y
- {
" e4 l& L3 z" X - return upD/downD;: R( a/ u! F$ K$ e+ u
- }) W* F! C8 F& v1 H
- }; \- U& [9 r; t
. z# F8 V4 X3 h" U7 }- void moto(int Speed_L,int Speed_R)
2 N( A, i: A7 m+ ? - {
+ \: i4 I% w6 P - Encoder_2.setMotorPwm(Speed_L);
) ~' H$ T$ p g - Encoder_1.setMotorPwm(-Speed_R);! ^; O. {& n U( ]4 d+ r
- }
複製代碼 & B- x: ^3 ~7 j( P5 g* \
MeLineFollowerArray.cpp
b1 H$ o3 o$ D- m" p- #include "MeLineFollowerArray.h"
8 u; A' z" U* K( ?! E" ^ - 7 g4 y( H/ P& @1 ^/ Z
- #ifdef ME_PORT_DEFINED
5 L% k2 ?* v9 B - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
2 L' N3 [2 Y; m" y/ p3 _ - {5 y9 T2 Z: F( Z' l+ P; l1 C
- 9 B& P: s- q# F; d( T: D5 L, w
- }9 ]* F; Q$ K8 \9 y8 H
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)3 s% K$ z. h& @' }! h
- {
5 C; F, @& G8 {1 b0 j' n8 h2 W5 n - _DataPin = mePort[port].s2;2 L& A$ j& n5 j9 F$ U
- pinMode(_DataPin, OUTPUT);
/ P# K' c' i( ]0 ^/ t - digitalWrite(_DataPin, HIGH);. S; o4 B" _. A
- }4 A# j0 }# u3 E4 M
- #else // ME_PORT_DEFINED3 r1 x$ f& q- t( M5 Z: u. r
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)5 Q% c G, U, T4 g3 y
- {
9 t( x2 @( v$ m: Y; g3 H9 q+ f+ v W - _DataPin = pin;- q0 w$ j. `, a+ m9 k# L
- pinMode(_DataPin, OUTPUT); " d7 \0 F% o l
- digitalWrite(_DataPin, HIGH);
9 w8 O% a e0 L - }) N8 T& `8 f9 ?8 a, x% z3 ^
- #endif // ME_PORT_DEFINED
V! k2 p# l; x: J8 X9 v
: \3 J! q' M* _+ k/ b, S
* p+ u; i5 W; k2 P- void MeLineFollowerArray::setpin(uint8_t pin)' w3 _8 _" X! D$ W, J
- {
" i. K! X! \8 _ M) W - _DataPin = pin;- e- y- r' E, e. S$ `9 e
- pinMode(_DataPin, OUTPUT); $ M' K5 l' k" u# [ j* ?7 A$ z
- digitalWrite(_DataPin, HIGH);2 e D$ Q% e$ X5 D6 f& A, ?- L
- 2 r+ H4 ]" h) }' Q3 W8 C/ L: R
- #ifdef ME_PORT_DEFINED
" U7 I8 z$ U& [7 e - s2 = pin;" F& j! \. E: \% {( m
- #endif
a% B6 Z8 t( A7 A4 D6 D: C - }
0 O8 F( ?' C2 q3 ~3 Z" M: j7 k - ( T3 i- Z8 n* j z- H D5 d! R+ i
- uint8_t MeLineFollowerArray::getValue()
* `0 G+ p& w, D7 s8 K/ U5 f - {
, Y) G5 n, i; A* b2 a9 x - uint32_t LOW_level_read_time;' z+ B2 L+ n* K I. f+ F; D; Z" P
- uint32_t HIGH_level_read_time;
3 B3 j" w4 x' V - uint32_t time_out_flag;$ ]+ v, t8 x* ] f" N7 Z. M G
- uint8_t Sensor_Data[3];
* e% x4 U% i1 E: K4 w/ B1 ^. i - static uint8_t old_data = 0xff;% ~2 ?/ J; }$ H+ b& a
- g! ^* A0 ^0 s7 `
- pinMode(_DataPin, OUTPUT);
! H2 }" p0 ?! D, _3 r' v4 w7 G' x4 L - digitalWrite(_DataPin, LOW);
7 B# U! ~ E) g4 ~- _0 Y& n - delayMicroseconds(980);
% F9 V3 `3 I! p6 x8 r0 e/ m& D - digitalWrite(_DataPin, HIGH);
7 F5 i1 T/ _( r0 C - ' e1 e" G# W; f% L+ W. L8 [1 B
- pinMode(_DataPin, INPUT_PULLUP);: c- ?' n G( A6 x% \3 d: }0 Y
- delayMicroseconds(10);5 D% c5 i& T/ n: \4 T- P
- 3 s- \3 r& }7 ?1 a" t
- time_out_flag = millis();
/ a$ {2 \% m& {9 k* X1 s - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ R7 t* Y) q q - # _4 s3 V. }: |/ N
- LOW_level_read_time = micros();7 u6 t, ^8 {" A! [
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out0 Y& {; q/ t; N
- { ~/ {( q) W$ `0 W' E! K3 T$ A; O
- return 0xff;
# C, k' o2 j; u0 I- w) N - }! D S- X+ u, s$ N/ q, }
. F1 v1 p1 F2 V% n" [* c- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 H9 h) X) C/ j
c+ \( `. o4 w% F* V- HIGH_level_read_time = micros();
. U% E( c1 o! k! [1 ^& b - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
1 ^4 e/ i. M) k b( R - : j: \% d2 q, P; ~+ h2 x. `
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out4 r7 V+ J" |9 g: @6 E1 T% z& E
- {6 I& ~3 r8 x" d b. m8 D
- return 0xff;
# _0 J3 `1 @9 n) k - }
, i ]$ f2 f$ M+ u5 r - 0 s' t' `; `. V1 x' Y
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
" ~$ d3 n2 ?1 m# x1 r/ l - {1 ~' l' @ V# Z' G$ Z
- return 0xff;
" d- G/ T3 v4 L* M' d% S5 H8 C - }7 t4 n5 E0 n3 f8 I& |
- 3 H, l% N- }/ N" g: v$ I
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! _4 U- @, S9 X: l - LOW_level_read_time = micros();
0 c0 E6 E5 H( T/ I" `# Q" J1 p - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level* p3 o+ O, Q- ]' I0 U' p2 }
5 F M- X: [9 v% O! e' Y- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out7 w+ i* C( U4 ?5 N- L) W/ E
- {0 ?- x1 u5 I$ I; |7 F/ K
- return 0xff;* w' W8 U$ @3 Y) w, R ]/ o3 \
- }7 j B% {; `" E5 f% J
- * l# M ]/ o, G1 ]2 r
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
- L# C9 P1 l0 a: i% d$ p/ b& j - {5 N; N; R' ]5 ~4 O
- return 0xff;' \0 J. n% j; F; p9 r' B
- }0 U3 e2 z3 ~- l, d$ q/ } Y
& a9 Y! X& g6 L4 p- O- for(uint8_t k=0; k<3; k++)
1 S" j8 A/ x% `2 V; ` - {6 x; x. e7 x% W. U/ |4 H
- Sensor_Data[k] = 0x00;* T! v# j9 e% i, Y0 a# j
# C: y) V' P v% S( _- v- N- for(uint8_t i=0;i<8;i++)
* |6 {$ p( p4 g - {
/ q- D! z* K+ M3 d0 y$ n - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
! G) B8 w; G/ e3 g) m - HIGH_level_read_time = micros();
4 |- v( C, u+ G1 y. e - LOW_level_read_time = micros() - LOW_level_read_time;3 _1 Q5 q' K4 K" s+ H% i/ C: N8 d
9 w. o: Q3 I$ ^% t3 m. t1 V- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )- J0 f' `) G: S$ e
- {
: H1 t' p5 f% P# J - return 0xff;
4 F. M9 s4 e0 b+ W4 k$ Q! j - }% |6 q/ C9 W6 {0 j5 h% r; w6 S
- ! n# l! z7 m0 @9 z
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" N* g# H/ c i7 Q6 y) C6 i
- LOW_level_read_time = micros();
! A! L1 ]& q8 z$ U/ L6 j# f - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level6 a! u3 p1 I3 N$ v
$ \& z9 z% ]- t9 M! v( y8 g% [- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1) y2 M' V- R; m4 N2 |* f( p" }% H" R
- {
& e+ F1 ^$ n% g( O4 x - Sensor_Data[k] |= (0x80 >> i);
, z! C: O D( n3 o7 H1 }% x9 Y - }
* O) [0 m4 @: T0 F" p - else if(HIGH_level_read_time >= 100)
" Z* d) h6 Y0 v: c) C% m& v - {
6 R7 F! p( B1 V" r- \6 I& t0 Q - return 0xff;( k- i4 s+ F7 V& W
- }
# z( L3 V8 L$ y - - g! o( G7 Q& I8 d8 C
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
$ [( Z$ C* |! A7 B - {1 o% H+ T8 W! a7 U' H( |0 P
- return 0xff;
. U4 d; M) h$ u1 p7 K( r - }7 }: D, R9 V1 U8 s* ]
- }
p6 B% M" v3 i9 ? - }! o7 P$ w! m# {2 p" G/ S
+ ?; Y/ O1 X2 e& h- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
2 i8 R9 M/ n; ]+ M' K - HIGH_level_read_time = micros();
1 _+ V5 b" n. r9 e: Z6 R - LOW_level_read_time = micros() - LOW_level_read_time;
) e% f1 A5 O8 y. s) c$ R - 4 p7 H! X# n' ]- D% k) g
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )+ _% Y: q1 F6 ?& v3 n
- {7 I5 l% ]+ |; v+ S2 l+ F
- return 0xff;. k! C0 f& z; U0 u' p5 F; \0 Z7 `
- }
+ M6 K, Q4 V0 @' s+ o' e! P2 w
8 V7 G3 ~1 U- Z- pinMode(_DataPin, OUTPUT);. S& ]( U! L! {2 b9 d" v
- digitalWrite(_DataPin, HIGH);
. l4 y2 ~6 K) S/ \+ p3 L% S) N - 7 g. U. Z3 A5 }) l
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
# ^9 T. p0 M/ A! @/ @ - {
+ F2 d2 v& y8 O; _ - old_data = Sensor_Data[0];5 ]+ h9 ~0 o( |8 p f' \7 L) M
- return Sensor_Data[0];( c( Y2 u5 v5 Y% Z+ P! E# v1 w2 v
- }
6 Q. e9 N7 Y5 ]+ |) N - else- K; e1 T. L* R- l, `5 b0 F
- {4 ]4 w& v B' [
- return old_data;
" t" l! z6 o$ ?* q0 V6 M# y - }3 g V) w0 B7 a2 n- l
- }
$ d6 T$ D/ ~$ _( q" G6 C, Y% H7 [
複製代碼 4 l+ T' U4 S& O) y* P9 [7 f1 n
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
: R& q7 `7 H+ `7 ^2 L1 e' U
4 ]8 G& {" ?/ {' J. I9 A |
|