|
|
/ D9 j3 Z* v6 i3 v- #include <Arduino.h>
' V- [2 U- [& D7 H9 t! B8 w% w - #include <MeAuriga.h>
" {% h' ?/ h5 \. Q2 i6 ?6 n N - #include "MeLineFollowerArray.h"
2 m4 U: P l$ z
7 }# ]% \+ D* C; b- MeEncoderOnBoard Encoder_1(SLOT1);
4 b$ Z& j6 A! L0 E" z7 K - MeEncoderOnBoard Encoder_2(SLOT2);3 l) I$ v* _6 D( @3 v* ~( M" j8 b
- MeLightSensor lightsensor_1(12);
. a& n' r5 d5 H - MeLightSensor lightsensor_2(11);
' n" x# u6 `+ v# F. {; f - MeBuzzer buzzer;/ M, [! i6 W/ H# g
- MeLineFollowerArray linefollower(PORT_6);
( k2 T2 { q5 h J! z - 2 g }; W& W) h& v
- #define Error1 1
O9 h! h( p: `- P% K$ |+ C - #define Error2 2
. o J( ~' H2 Q b {$ w - #define Error3 3+ C) l- a9 S8 D; q2 z
- 1 Z! A5 B, b. u& [& m3 S3 e
- #define Kp 15
1 c' p# Z" h! C$ n k - #define Ki 0.15
) a x* }( q9 n. ^ - #define Kd 0.03* V- T; U: h) w
- , I; [- G) [8 F) f4 D1 ?
- uint8_t sData;
* U; ]! w. p7 c, | - uint8_t D1;
9 J4 K0 C2 u7 M+ J$ B - uint8_t D2;7 K8 y* u4 X& a! R- R8 B) W/ Y
- uint8_t D3;+ ^& H& A7 ^3 f8 P" b( x0 ]
- uint8_t D4;- k' a0 z3 z5 A2 |
- uint8_t D5;
& _3 V% n1 V0 L* I - uint8_t D6;8 ? i: F7 W! h' {+ @/ l6 M" U
+ _- Y) O5 b$ t1 o5 W$ d. [- float previous_error = 0;
" F% g5 Q/ i3 D) S* _ a5 x3 F - float integral = 0;: T# P+ z* h ]+ S" N. P; B
- float derivative = 0;- `; }0 [% [. z4 I# r' ]
- int Speed = 160;
8 T7 T- u# R8 M! a5 Q - float output;! p1 c* x7 N+ f7 t
- 2 V0 X' @# n( t. y& @5 J. \' R/ Y
- byte Left;; [! k+ I' _$ L- D: A. F' n
: p/ U7 T- Y7 Q- void setup() & g/ y: ^) Q6 c
- {/ O8 B) _& \. S3 _7 `/ e6 G
- //Set PWM 8KHz
. Q/ N8 `1 Q+ q8 i% p - TCCR1A = _BV(WGM10);
1 Y+ y( \; Z1 O7 @ - TCCR1B = _BV(CS11) | _BV(WGM12);
. p" g" J ~! h- s" s+ @0 I' p - TCCR2A = _BV(WGM21) | _BV(WGM20);1 u; G: Y# m5 t: b" l) O
- TCCR2B = _BV(CS21);. t$ a, d3 G2 f- V# ]
- Serial.begin(9600);
$ \% A, V" w% Q9 D& m - buzzer.setpin(45);
* V0 Z0 X7 V5 Z9 h - }6 T5 c; k8 ?3 e" d" [
- ( O4 g; K C6 q$ V7 s% m
- void loop()
$ D$ [; D0 S b H/ @" K - {
+ z1 w$ C5 ?& n - while(!((lightsensor_1.read()) < (10)));
3 d7 O' {, l" Y- r4 ~) U' k S - buzzer.tone(1047, 500);
( x. K0 N6 S* h) v7 g3 x - delay(500);, \; ?/ Z4 ^' S( u* W4 V
- do* _% A1 W2 R, c
- {9 x! h4 t6 T1 ~2 [5 d' X) G
- int Err = getErr();) i6 M* a( s- i9 R8 M2 o: ~. ^# R
- if(D1 == 1)
$ r( p3 a k4 h3 F" e: h' E - {
# {4 h- Z# F$ v" `- P - Left = 1;+ Q$ }9 i8 T+ e/ b& D
- }) s6 b* H# g" N
- if(D6 == 1)$ T" i' V. \1 S0 ?. J
- {4 w% |" J9 Z b' H" l
- Left = 0;
& l. H a9 r( o5 F* Q9 f) v - }2 [9 M& @2 U- R9 `% k* O
- if(Err == 99)
. `+ L0 m: m; q( w$ m c& a, q - {- \6 @8 Q/ [3 X6 Z
- if(Left == 1)' B0 c- d; \' g: r
- {
1 K( e2 ?% \/ h7 t$ X: l - Speed -= 5;" P" Z& d% @& d! Q/ e7 v
- moto(0,Speed);
( X' {1 A! R1 o. p3 m - do; R8 J/ N! k8 A* f( o, o4 R3 \
- {
% w, Q ~% {" O3 p$ C* r2 W - Err = getErr();
+ i" g$ }& Q C) O* Y - }while((D1+D6) == 0);9 t$ ~; ~& C/ K2 [& f# }
- }
9 I& K" Z+ F' V, S - else! G# e3 n0 J! v, t8 v# X u
- {
1 R1 l2 i/ l* B3 ]% ~+ e) c - Speed -= 5;
, W: X3 ^( w+ Y4 W& k! { - moto(Speed,0); 4 h P7 ?0 {9 ^7 s+ T
- do( N# t% |/ c! R" k w0 W
- {! _) w0 E% w/ R6 C4 j' o9 e
- Err = getErr();' g* x2 y8 b' k2 Q/ I
- }while((D1+D6) == 0);! h7 V3 |0 I$ ^* E
- }
) q% t0 o7 M$ D' u# r: ? - }6 S% H0 l; Z) w( z8 U$ _
- else# Z+ m! N: L q" `. p1 _
- {" t6 `; o8 r3 M( T b
- if((Speed < 160) & (Err < 2)) Speed+=1;
/ K5 Q# j* M4 B8 a - if((Speed > 100) & (Err > 2)) Speed-=2;
2 P! [4 A8 K4 \& _" Y2 d" J7 @; M+ J - integral = integral + Err;' ~- ~) Q( a4 S' R5 a `
- derivative = Err - previous_error;
" H0 n5 j3 ?5 m) Z! `' S- Y - output = Kp*Err + Ki*integral + Kd*derivative;5 V1 G) @ @+ k2 O n( j
- moto(int(Speed-output),int(Speed+output));
4 Z* ?2 U) i8 F5 R: u/ G6 r - previous_error = Err;
& N2 m6 h" [& i/ d) \/ X4 v* h T: m - }
6 U) F; V- ]4 a- u - }while(!((lightsensor_2.read()) < (10)));" p0 N5 ^3 w0 [* D1 b& Z+ V
- moto(0,0);
+ I B3 g3 O3 \0 }- n- _- b, E - delay(500);
3 V# q( L8 j' G7 P9 W - buzzer.tone(262, 500);
; q* _ L* a( q - }2 B' ` P. c3 v: A9 L
* E' k) v1 N0 s. t7 g5 ? m( h/ J- int getErr()& Z& q& |0 i! b; {% R% w
- {
& @: j3 \2 D2 Z1 G* ~) t - sData = linefollower.getValue();
# D: k) r# y8 W8 g& E; R; s$ H - D1 = ~(sData>>0)&1;
# Q. m8 K4 e( j% ?4 q. i - D2 = ~(sData>>1)&1;
* f7 u0 u$ V0 ~ - D3 = ~(sData>>2)&1;& X: _0 I. D7 `! I% V
- D4 = ~(sData>>3)&1;5 F0 Y) J! Q! n8 D0 a( q
- D5 = ~(sData>>4)&1;
8 r8 ~$ O; a: m, e+ d; f# A - D6 = ~(sData>>5)&1;# H8 u2 X/ a6 F7 Z1 T1 m3 V% P
- int downD = D1+D2+D3+D4+D5+D6;
! q: x9 C/ R0 ]. _$ r2 S3 G( I! I - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);; A) U; g. |# H5 b1 c3 u
- if(downD == 0)
$ D O/ e) @1 L/ [: U - {; a) ?! o0 g; p0 t7 {
- return 99;
0 Y" z- y) I' m% X" i - }
9 O5 o7 G8 f* |) c' O6 M4 m - else% g" A. u+ [8 v( @3 |* ~7 ?
- {5 o$ r2 [; {( K; O$ x7 t) b
- return upD/downD;
5 F- }: Z3 Y# L. {5 V - }
) x2 H& }+ @! F& [ - }
$ E% W" I8 @3 W: R1 q
5 J* o4 n) l* ~- void moto(int Speed_L,int Speed_R)& o r* ?; {6 O
- {
; ?, Z ^6 E& s - Encoder_2.setMotorPwm(Speed_L);2 B! `! d2 Q. {$ H: y
- Encoder_1.setMotorPwm(-Speed_R);
* d9 Z+ I9 z4 v3 s0 C! d - }
複製代碼
" s* J1 ]/ b% v1 a( _9 @9 `1 [. XMeLineFollowerArray.cpp
5 R" k9 w. I+ x+ i4 A2 W- #include "MeLineFollowerArray.h"' t, I( L7 d A5 B9 b
- 8 v8 t+ C' d+ |2 s2 Y5 c) M. t4 i7 l
- #ifdef ME_PORT_DEFINED% t" M. `. m" Q5 B4 t) s5 {% L. P
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
9 |9 O$ j8 M3 k5 B) \9 Z1 G - {1 { u% |6 ]; C2 G# G( y# ] X
- & M. E; Z; W% r1 N+ v7 S
- }
6 y) Q0 L1 O; i" w- V - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port); W3 h7 h% N' J N1 Z
- {
! k/ o0 C! _% D4 } o8 R0 | - _DataPin = mePort[port].s2;
7 F# @9 `5 O6 {( r7 q$ j - pinMode(_DataPin, OUTPUT); + {8 g% n# R" L
- digitalWrite(_DataPin, HIGH);" A7 `0 U- K( F) l7 l
- }: \8 J4 n. F9 v* [: t! M& c
- #else // ME_PORT_DEFINED
( s$ N! c/ g) Q0 N9 Z, ^* @- c - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
$ C3 Q5 r, x' \1 D - {
5 q7 W2 }6 Y* l# Q" u - _DataPin = pin;$ C. b: i4 p" r& Y! a( R! q
- pinMode(_DataPin, OUTPUT); ) D% D& N- {9 }3 G7 n( T+ T
- digitalWrite(_DataPin, HIGH);+ z, L! g0 C# ]- t
- }
* B/ G3 p5 ? k- q, H6 M. `3 n - #endif // ME_PORT_DEFINED
7 s* O0 C# z7 ~$ F - ; u6 Z ~8 H/ p0 |8 c9 }
4 ?) o2 q1 a0 |+ r3 D- v- void MeLineFollowerArray::setpin(uint8_t pin)
) Q$ l+ p- |: N0 ^5 R - {4 K3 F% }8 d* r8 B0 Y
- _DataPin = pin;8 M+ l+ M1 p& U7 @ b; M2 g6 b
- pinMode(_DataPin, OUTPUT);
" A. s, B$ b0 A* D! N - digitalWrite(_DataPin, HIGH);, ^* ~7 B2 ^* l! u% b
7 Y: ]* x3 c, t9 L3 y1 t- #ifdef ME_PORT_DEFINED3 }/ a/ N1 l6 w# |' U4 m
- s2 = pin;8 z0 Q, K( B) W$ D# j. `7 l
- #endif
+ I' g0 T! N+ |" a1 Q0 k1 o - }
1 V+ \9 {5 v6 ]3 w( x - , ^/ F! G, S8 e( o& a9 n
- uint8_t MeLineFollowerArray::getValue()
4 Y1 T3 J' I8 X* h% m+ a4 K - {! ~$ W# {4 w5 E/ _: N
- uint32_t LOW_level_read_time;6 E& q5 A+ u1 C6 B' ]
- uint32_t HIGH_level_read_time;2 E- s8 D$ R" ]/ b. b8 f1 v* | Z
- uint32_t time_out_flag;
# y9 j; w- M2 O- k. `1 R& V L - uint8_t Sensor_Data[3];! \1 W3 ]7 ^$ b& G
- static uint8_t old_data = 0xff;
: p2 z: H( w6 I
! s- d- v5 H6 |2 [- pinMode(_DataPin, OUTPUT);- K/ s2 R: D5 i7 D, p! I+ `3 m
- digitalWrite(_DataPin, LOW);) Z7 l% F- a6 S; } q1 o
- delayMicroseconds(980);
3 \& a# l0 w, P Y - digitalWrite(_DataPin, HIGH);3 v+ W D. u0 f R; I4 y% j/ B1 I5 V
- / q3 q7 U5 F/ A! e( X" d
- pinMode(_DataPin, INPUT_PULLUP);
' \ s: p' E, Z% z+ D# @ - delayMicroseconds(10);; i0 j% ?+ x/ s! N/ i0 r2 C4 g6 u* X+ Z
. z+ E! U& z1 h" l- time_out_flag = millis();
7 J9 Q ?+ l; r5 N - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ X9 l* a1 \9 |4 ^
- ! G1 S, e0 q. g4 c ?" d; J q1 A; A
- LOW_level_read_time = micros();( Q$ E% o |* E
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 R* S) R$ h/ w6 y& d* x6 N+ J - {4 T5 s. K0 r$ m' n' l! r# o
- return 0xff;- g! s2 J; u _2 t* e! o. p- ?
- }
1 _# H! o6 V+ F$ j2 [& U" [
M) }; w+ m5 A, q- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 L+ \2 s9 k( M* p6 @
- 0 } r0 A1 s4 ?2 V1 |3 _! }
- HIGH_level_read_time = micros();& R' V1 G7 ?+ a5 ?' i+ K
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
% G2 N. t* ^; r) @3 Y7 r/ V - b$ Q9 ?# c- ]
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out" W7 u. w9 s$ b: l3 ~# b
- {9 y. E9 y2 ~) A
- return 0xff;
; L, L; F1 W' q/ V - }
8 e! r1 @& ~: t. H4 E) } - : ]/ z: d. Y- M5 C/ [. [
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
$ s+ d" a- d2 z, e6 n% c - {
" X$ j2 O' h; L6 v' N - return 0xff;
, D B& g+ D+ V, u - }
" W# ?$ L0 w0 e0 R( I
- r2 R" L7 t6 g" g, m- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );6 g1 M, f/ a. K
- LOW_level_read_time = micros();
$ q- q' l8 h+ h7 j - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level/ Y" v! s' r6 }9 o% B. S/ Y
/ W3 k z T" ?3 ?- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 X0 `0 v, W/ X5 E/ L - {0 H6 {: Z2 r8 q, M7 Q% `- o. g
- return 0xff;
; n) r: E$ n0 [6 v% O3 P5 \ - }
* E: ^+ q' K, [! ~ - 4 y" J; ~: S7 O% W" i8 {( V) \
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
q: K. P( c* n% D2 U+ w8 G - {
, ]0 Y- h& l# w7 N - return 0xff;
( \; V) }8 P( ~8 C. {& b. B; w7 L - }
. v8 ?% }$ I7 M6 I, J2 u! z
- l; y% B% F4 d6 a0 ~- for(uint8_t k=0; k<3; k++)
: r( N. \; O, Y W0 K$ Z - {
% a9 [& K% x" ]- c: b! _* F0 Q - Sensor_Data[k] = 0x00;
% y4 e7 k; g* a& k- a - ; r- ~& e N* f: l4 F
- for(uint8_t i=0;i<8;i++). X; e! C7 P: b( B- F$ o/ j
- {+ i2 q# d& z& q9 d' N6 h, @+ m
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
' E$ u4 P) | z, m/ [# q - HIGH_level_read_time = micros();
# \/ D5 s& r( Y1 ?" A9 d" y6 U) O: S - LOW_level_read_time = micros() - LOW_level_read_time;
5 j3 G& |9 y t/ X! n% r. w - % C. J+ M* y$ i2 a
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ): [ J1 D$ t- K" s
- {
: L- |( a$ {, h' w1 O - return 0xff;0 T% z7 t6 I: K H2 {6 {( T- _
- }: t$ m) G; }3 q# b* J
% p9 ^1 f# \# ?1 y( H- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); T' L$ D2 h5 o7 z3 U
- LOW_level_read_time = micros();
3 r* H P6 P4 [# ^: | - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level" s% g% K# m, y; a. B
6 O! `) i8 {5 E b$ j$ x g: f c- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1* O0 @& I D# c7 V! w( d, ~1 B& _
- {3 X: V4 j( U9 H& ]
- Sensor_Data[k] |= (0x80 >> i);- w6 G! O( h2 O
- }/ T- [" \4 K0 _ @* l u
- else if(HIGH_level_read_time >= 100). Y" \' l# H% _3 {8 p# K/ g/ Q
- { J* K' D( C8 b6 R
- return 0xff;; a. t/ _1 W$ w" A, P1 C8 X1 t9 p
- }) r. x6 j, F+ ^: I7 J8 G
7 }2 z0 h2 g( j# T- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)7 J8 a& j4 W; d8 V$ ^: f
- {
Y- o! J' O i - return 0xff;' }5 v7 C1 W) T! s' F! S2 b( t% a
- }3 u% ]: O1 w1 T- R, y/ b9 P
- }
4 [* L3 ^* w c2 |1 t! a - }
9 I/ U5 I+ U6 |, E8 l# Y5 U7 r
' W6 ^! f9 m$ ]! m" X# l7 ~& K6 a- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
* S9 M" U* o% A - HIGH_level_read_time = micros();; x! ?- X2 p! s5 J
- LOW_level_read_time = micros() - LOW_level_read_time;% @4 J0 q7 S n+ V/ k- t
3 p3 a: g4 L, Y. \3 t- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )1 G- n5 Z6 c. i P4 v E) r ~7 F
- {" Q5 p, s* q, z) V5 ]
- return 0xff;
+ G9 ?4 j+ Q, s9 t1 Y/ Z' G: h' w2 { - }! D6 ~8 W. p4 | v% D& ~
- [" h6 G, U/ s7 L/ K* x) ]
- pinMode(_DataPin, OUTPUT);- p+ r$ [& r9 f4 P8 `3 d; u
- digitalWrite(_DataPin, HIGH);- y4 E+ ~$ m$ j4 K
7 Y, X0 R- B' `/ O7 |3 t% [- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
- O U/ e' B5 `. ]2 ~ N; Q - {5 N" P4 |) R" h( f! b" s M6 i
- old_data = Sensor_Data[0];
8 T! E' u& u9 Z! { - return Sensor_Data[0];
0 F- m9 Q# b5 p; d - }$ `. p* k0 k# }/ k% \- l0 N4 A
- else
$ d, L2 J9 S+ r F; n/ g/ q - {$ F/ R+ `3 H7 E H4 c) @
- return old_data;+ H) E4 r9 I8 P( h
- }
. |. R7 P3 h& y% j' W* D - }
7 E% Q$ V( O% b/ a
複製代碼 ! P! S8 y- g# I+ T
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
# C" x# O/ q3 C" k3 q& {; q; `
d4 h: B/ {2 @$ W0 L4 b/ P1 r! J# A |
|