|
|
& |& Z/ k/ y- M. {3 }+ P9 ?. B- #include <Arduino.h># w6 [# @! v9 a: H9 h- p
- #include <MeAuriga.h>1 A, k$ \. x" o# J" q
- #include "MeLineFollowerArray.h"; U4 Z2 `( a* ]) ]! I; _
- ; L/ ]8 J- B+ J! E; Y
- MeEncoderOnBoard Encoder_1(SLOT1);
A4 Z$ ?- b+ R- g6 J6 ]( J - MeEncoderOnBoard Encoder_2(SLOT2); M# C# Z- E8 S' _
- MeLightSensor lightsensor_1(12);
& `8 F' d: H/ A1 C, U - MeLightSensor lightsensor_2(11);
0 O* y. [/ @" @# n - MeBuzzer buzzer;
* V& e- d% w5 V3 X5 I - MeLineFollowerArray linefollower(PORT_6);
7 y4 Z% R4 m+ F0 ?" U0 z# I" b
9 C& u/ M. y) T& a2 s- #define Error1 1- X9 g2 u( M7 `& E- e1 p
- #define Error2 26 Z5 w) O( r) }. g( O4 [
- #define Error3 3 J# Y# A$ B/ p, b Y" g) g- C
- 1 \+ f) H, Q! z5 `( f% S# l+ F
- #define Kp 15# J7 h( }* [ p
- #define Ki 0.15
2 w( P* r9 {7 T Z& E5 E: i V. x+ \, H - #define Kd 0.031 Y5 n# X& P' M8 v6 a! j
* g4 J) V4 y! P" L" H( t% ~- uint8_t sData;
$ A- Q: H6 I8 w - uint8_t D1; N, u3 W/ ]. C+ ^+ m$ J
- uint8_t D2;3 T* ]! B) c: P
- uint8_t D3;
- i* ^: {3 r/ v6 N% G8 S& e - uint8_t D4;1 k, p- O$ N& H+ z/ ?% Q8 M( n
- uint8_t D5;/ G" b/ H$ }7 \$ @9 X: F$ x* e0 {
- uint8_t D6;
7 ?* V) P0 @4 G+ P
; R& F* G f; a W; w- float previous_error = 0;6 V& Y' q+ d" L$ r
- float integral = 0;+ P; Q+ W7 g, n; f! |
- float derivative = 0;6 \0 ~# w8 ]! I# O) w
- int Speed = 160;
+ v) j4 ~# @# a. L% Q( n. C( c - float output;+ V: E# p6 y; G3 t# ?
- & Y, a, } a% a4 H
- byte Left;2 f5 W* ~" G3 W( D; d7 Z
- / h; a1 q( ~( b
- void setup() ; o7 F) ~1 b! @; j- }2 C
- {* P/ G$ Y# I. X5 W) O, V" c: M) Z9 S0 m4 E
- //Set PWM 8KHz
_" @* J L& D5 G( g - TCCR1A = _BV(WGM10);3 t3 H; l! W; s8 j4 _; [/ ?$ V
- TCCR1B = _BV(CS11) | _BV(WGM12);; N: ?. W6 l; K2 ~' g
- TCCR2A = _BV(WGM21) | _BV(WGM20);. [/ G! Q) n# h6 T% D5 i- x
- TCCR2B = _BV(CS21);
+ \9 h$ f" N9 D7 P( H3 U2 Z - Serial.begin(9600);
+ N; g) b3 P \0 s' t: t- o - buzzer.setpin(45);/ @8 x; E( P& B- r1 C
- }" O9 K; W$ O$ T/ E4 o; H, m, |
! B8 } A, s) N0 Q3 ` [4 R- void loop()( E0 ^" o9 E; H/ ~/ Q% t4 v. r
- {
[, `% o! F% H3 W - while(!((lightsensor_1.read()) < (10)));0 _8 M2 p/ U" o7 [, V6 y
- buzzer.tone(1047, 500);; l- G7 e$ j% _# F1 t
- delay(500);' f3 p6 L1 O" D) }
- do, d/ n- P9 M$ T- l
- {4 p8 d2 ~2 C, y3 o7 B4 Y9 J
- int Err = getErr();! o* T) y8 K% N$ p9 i4 n# X7 `
- if(D1 == 1)
% h" z! w4 T" G- c - {
% j) G4 l$ H' X. b4 k6 |; e - Left = 1;/ l, j$ L ^) p1 Z m7 Z
- }
- y% z% z. W, B5 \( M8 q% Q6 t5 d - if(D6 == 1)+ d4 C" c! H% ], p0 P7 c# K
- {$ [# K" s! s; C2 x3 T, V
- Left = 0;# S. K9 S) c6 I8 i; M, t/ A
- }
/ }6 w5 U: i% O. v3 }) t - if(Err == 99)
' Z2 c) I% n1 c - {( Y# {6 f# }; S+ b, x: e+ i
- if(Left == 1)9 {2 Z4 }. w: c* k/ {. ^
- {
% }9 o9 j: Q, T3 Y8 k: I5 ^( S+ B$ M - Speed -= 5;4 o! c+ k5 F; V1 F% T5 j3 U
- moto(0,Speed);
; V- P6 W2 q6 d/ a9 v5 K - do- c) E) g' d. V
- {" h" K, [6 y+ a9 {
- Err = getErr();
1 G; B: B4 f# M7 F - }while((D1+D6) == 0);
/ {4 j' ~5 t' h( [) F1 L - }0 j+ P( g+ K7 W, N. j8 x
- else# g! C @: @' z( a, ~- u4 J
- {: A- i4 P2 o2 Q* f* h. A! h
- Speed -= 5;7 C2 |! _0 L' G- `4 h
- moto(Speed,0);
0 Z* y' ?% g7 Z1 P2 a( X- R - do
7 d/ V5 S( K0 j( a+ P - {
+ v, p& W' j4 k0 \7 f - Err = getErr();0 J! D. F7 g. b. f2 M
- }while((D1+D6) == 0);* E L3 g& g. f; |
- }
' P5 F* W0 B+ h( P9 D - }
7 D% A! P2 z1 W* ]9 E - else7 \3 g& B- x5 T2 {5 w) |* g
- {
$ D9 y* {, P) h# U% z X$ Y( e - if((Speed < 160) & (Err < 2)) Speed+=1; 5 p+ ]4 e% r7 `# n; |$ M
- if((Speed > 100) & (Err > 2)) Speed-=2;
9 m. y+ l% @! U! l: M& }7 f* N - integral = integral + Err;
% n+ W, a- {$ @5 q& J8 m - derivative = Err - previous_error; z- E$ R0 |( y" @
- output = Kp*Err + Ki*integral + Kd*derivative;: U7 F+ q( L0 D" P* \
- moto(int(Speed-output),int(Speed+output));& S/ i) ?& L* R. p1 t# Y' B7 O
- previous_error = Err;
$ |* f, j, h/ J6 e- I6 b - }1 L9 v' l" d) K, u3 A
- }while(!((lightsensor_2.read()) < (10))); S0 H, e2 C# f7 d; R6 T9 o2 j
- moto(0,0);2 j/ M4 G% ~. o e2 T' h
- delay(500);
X: \" L* d; b# L/ Q1 S - buzzer.tone(262, 500);
4 Z: q; }* H' V+ a; k! G - }7 r' r: L" R+ t4 R3 S, @
- . _% t; [0 P& y7 d) f6 }& v& R. @& }2 s
- int getErr(), J9 t$ n% n. M7 B
- {
4 p1 ^* Z7 }* N' s! B - sData = linefollower.getValue();, i% b: _8 z6 d9 X. }
- D1 = ~(sData>>0)&1;8 e. L' g/ ?+ ?7 a9 a, o6 p
- D2 = ~(sData>>1)&1;3 U! u6 _9 ^ e5 j* m- M
- D3 = ~(sData>>2)&1;0 W/ T$ N5 `# P& D1 [1 x
- D4 = ~(sData>>3)&1;
0 x+ u! g" T3 Z+ X- G - D5 = ~(sData>>4)&1;
. N7 b) s: U2 e! o3 e6 F: Z - D6 = ~(sData>>5)&1;" s' C: W5 G, l% _: V& ]- v
- int downD = D1+D2+D3+D4+D5+D6;
/ Z" i! k9 B( v6 F2 j - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);: u6 a, T% g$ T5 R
- if(downD == 0)' V4 |% \- d- [! ~9 _
- {
5 ] J! P. ^, M( j7 \1 c - return 99;8 K3 w, _5 y& Y% n4 I# v
- }
4 q6 e2 R/ M; g( I6 E; c0 t* x - else
# a1 Z+ E$ E) k$ @* @" \5 a - {9 U6 C9 z" s9 h& o* }
- return upD/downD;
( h7 D* d3 e$ o6 O, ]! O - }, S7 q1 h: X6 z1 Q1 [
- }
# v c& y) X) v; E, W
$ `) \" u: b/ d% A% Q; X0 {6 F- |- void moto(int Speed_L,int Speed_R)# r9 R# v7 y0 F* T
- {
8 l. l( q2 s C: g/ n: O1 r/ w - Encoder_2.setMotorPwm(Speed_L);
4 z" b3 u e' X6 [/ h - Encoder_1.setMotorPwm(-Speed_R);! I+ P& n4 K5 V+ y
- }
複製代碼 7 R; v% _* f- h3 Y, L
MeLineFollowerArray.cpp' o8 G+ \# k: _8 N
- #include "MeLineFollowerArray.h"0 @; g2 v7 M _8 |4 X* U7 ?
- 8 C/ U2 ` q a' B, @! K
- #ifdef ME_PORT_DEFINED6 c9 s- D, L/ a! C% V9 `# v
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)! w8 B( i1 G$ z) E1 g; Q
- {
9 @" Y3 h' u( {) M, x0 Z/ o
- E. ?- `( E L6 N/ ]& J0 |, ^4 F3 m- }
& a4 U/ p# t x3 c - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)# U8 {2 h: Y0 I1 r- }
- {' k6 e: E& e# l) @
- _DataPin = mePort[port].s2;
: |: i+ \0 w2 i. T$ G - pinMode(_DataPin, OUTPUT);
7 s5 \9 W6 n- V3 K; t7 E. k' F - digitalWrite(_DataPin, HIGH);
: x6 A$ W- J3 _; w' g* v - }2 L& j1 ^- h$ d* p/ K5 r7 W
- #else // ME_PORT_DEFINED& w0 n& ^2 ^+ [5 Y) U* C4 `
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
1 v$ H+ ]7 A; c- z - {. `5 ?" ^3 D6 s4 p! _
- _DataPin = pin;9 ]7 l0 @# c z+ B* \$ p
- pinMode(_DataPin, OUTPUT);
( V& `8 z% A: x6 x; W: N - digitalWrite(_DataPin, HIGH);( ]. w/ i2 X2 `, N" W
- }
( K7 K9 z- {# Z7 H# v" x& t - #endif // ME_PORT_DEFINED
! X$ q7 X* j7 w8 W+ f - 3 g% J; d* I( u
$ R- F A$ z7 { \& c) o5 S% m) W$ k- void MeLineFollowerArray::setpin(uint8_t pin)8 u! Y% }! D6 t( i) S; A5 w
- {' b1 T2 R$ s, L, S H
- _DataPin = pin;
3 n( W2 i( Q$ u: g/ Q+ { - pinMode(_DataPin, OUTPUT); ' ?9 {; F$ p( c2 d* Q3 E
- digitalWrite(_DataPin, HIGH);
; Z/ K- `8 w2 c% L1 U3 k) B) f
# s" D; Y& o+ h- #ifdef ME_PORT_DEFINED. T. d$ @) N; |. k
- s2 = pin;
/ b; V# z" g$ }/ C* p { - #endif
8 l. t# m, M- o( ?7 m3 c - }
: v; v q' ?4 @- ` - 8 H, j Z0 H. B8 ]1 S
- uint8_t MeLineFollowerArray::getValue()* p/ O& C) s1 m$ U8 S- W
- {. E0 a3 B% S5 C
- uint32_t LOW_level_read_time;8 t/ T; z% o( G: L# w$ V
- uint32_t HIGH_level_read_time;. U0 A( T/ O* r4 I2 p7 |# ?$ y
- uint32_t time_out_flag;/ k9 ]8 o; {4 c- Y, \. }+ c
- uint8_t Sensor_Data[3];: D6 T& j2 o7 N
- static uint8_t old_data = 0xff;9 c* y! d4 o3 f( u7 z' M, _
- 1 x p4 n3 Z% S- z! g
- pinMode(_DataPin, OUTPUT);, k7 y: G& y) |
- digitalWrite(_DataPin, LOW);
9 Y r6 w1 g6 Q6 M3 R' d - delayMicroseconds(980);1 g/ z8 _ r: @7 R% h7 P3 O; k. R
- digitalWrite(_DataPin, HIGH);/ B+ e$ q6 g" v0 P; }( x
. o" ~. J* x; Y& ~* H9 [- pinMode(_DataPin, INPUT_PULLUP);4 D/ C/ H6 k$ X6 g. I" \% [. I
- delayMicroseconds(10);' K4 d) E3 o8 K$ c0 Q5 r9 F
- ! W3 q- L$ T, p2 W- C
- time_out_flag = millis();8 X0 W. f- o& M) W0 u, [0 {. o
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
8 ^9 a4 [# s; A2 p* n. K - ' M$ ^& K4 L4 Z B# K C
- LOW_level_read_time = micros();
* x0 B& q9 S: ^5 u# r. ]4 V9 S5 J - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
: E+ C: |, o2 f) p, G - {
/ N0 ?& E! M2 {1 S7 q" G - return 0xff;
/ g2 ?5 t9 i: b) r - }6 Y7 i+ B4 R; l0 a* v/ y, w& Z- E
- . r! \$ u" a. D" m
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. {: k% N' n- \, G# l5 A- P2 c( v* K0 p
- 4 M5 M9 o# `( J; c2 d/ @8 g
- HIGH_level_read_time = micros();+ o8 j* O! {4 Y/ h6 T
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
6 M* N: ^' ^& U \4 g
% n' L3 _* v0 Q- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
+ g0 S8 T0 k7 s# ^/ Y. p - {" X; W# H" M" O' R5 k9 p
- return 0xff;
: _+ {2 h: R, v; C' I$ a9 m - }
/ ^0 i3 D7 u: n) O$ A" F
& t! p3 Y% K- o7 r- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
I7 M& v5 u: N7 ]) l+ O - {3 h7 e" K% J5 N$ y
- return 0xff;2 z% I+ G3 L; C
- }
4 }6 m6 z/ X: \ L - * ?' r6 U2 }+ B7 F$ f# B
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
) h. j1 e0 I) m1 i1 y5 a - LOW_level_read_time = micros();* k) \0 }; P$ |! f9 {% w+ [( x
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
) @1 z8 P* i3 L+ j - ; `8 R# O! n( F
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out8 w) F! p* N+ b* o9 o0 N
- {
, {* `4 m- _3 F s$ | - return 0xff;. A: [+ B* i8 n0 J* |9 ?# d# D9 Z
- }
- K/ x$ Z C& F6 i6 d; W
6 f" p# D5 V. ~) ~; B1 H' D0 l- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))1 u" d) Z1 a! F) I
- {; l" l1 @% U8 I1 B
- return 0xff;1 d/ A+ G. ^; v& x( D1 X
- }
+ {/ b' d& t+ A# K% N0 y - 1 {. b* G0 {, n
- for(uint8_t k=0; k<3; k++)4 x# z8 j1 H8 y ~7 U0 t' M
- {* Y7 u" P( A+ Y: D- m2 L
- Sensor_Data[k] = 0x00;
( O; ~0 T: N- a- f6 ? y - 4 i9 E, R8 F" B& Z+ t
- for(uint8_t i=0;i<8;i++)- T' U5 b; p# }8 N
- {# g( D. B1 s7 b# m2 N0 f5 E
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level# \3 ]2 [: g" ?
- HIGH_level_read_time = micros();
9 d) G. Z) E1 |9 z - LOW_level_read_time = micros() - LOW_level_read_time; X( I' w4 A+ g2 A5 T% a8 S
9 V9 z. o. l, Q4 @. h1 L2 R! G3 I- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
- y7 i! e3 I6 [- g. T# l1 [ - {
1 Y- q- D, |- }; R8 u; x1 W - return 0xff;
; R. q' J/ L3 e3 U* x! O4 t. M - }$ d0 C* H( S5 j2 d9 m# \. L2 W
/ J" K! N& A" d- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. c% H% o# r1 C! o% }
- LOW_level_read_time = micros();4 T+ s3 B$ ^) Z' N2 O
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level Z$ q/ p( I8 S, N" b
- y+ X7 x5 l+ ~1 \4 w8 y- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
! b5 v+ a5 b- ~. \+ @2 ?& k - {/ T4 a, g) w p' Q; Z D% C4 O
- Sensor_Data[k] |= (0x80 >> i);2 V9 Y; o* w5 {$ ]
- }
5 c8 C" D4 T% x( l4 {7 | - else if(HIGH_level_read_time >= 100)* \8 Q* `1 z. f- a' m! R
- {6 ~/ @) p* f# A! y2 Z# J" p3 k
- return 0xff;
, W4 }8 w' ^) B - }" t4 \; G( o; O4 A, s" A+ v7 L
3 E" j0 i4 q% n6 A0 {( ^$ n/ T- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)+ O8 U3 B1 }0 B7 v$ x
- {3 n1 I) D1 M( `' ~. ~* f
- return 0xff;
: z+ P# z6 \+ Q - }
q5 g# E W# x: g: Y' @, F - }' s# L4 `3 g7 n1 t5 ]/ o
- }
7 O) A2 b1 S, ?2 Y, w$ f
4 ^0 B/ p" @+ g# `! S! V- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
6 W" Q, g8 v! F0 ~- F2 Q; \% G, c - HIGH_level_read_time = micros();/ i4 G9 }% v" Y! V( U" N: G" N
- LOW_level_read_time = micros() - LOW_level_read_time;
& v: o; I) a0 z7 {7 V" z - : {1 M1 O! B7 |: j0 E( [
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
0 U! m. i7 o# k2 o+ J9 c - {
) i# z8 }+ N0 o4 w: l - return 0xff;
# R" v% X# P! u) d9 K. O - }
- w, Y+ {" @3 b( a! ?0 h, P - 3 H K2 Z& H# s. _
- pinMode(_DataPin, OUTPUT);
7 b0 \' @, Q" [% ~9 X7 B - digitalWrite(_DataPin, HIGH);
( I; ?$ @8 i% z3 ^ h3 g - ( h# `" p& M) N% l |$ X( Z b
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
; F% A! o" k2 Y* d6 x( p1 A6 U - {) Z. `' F* a0 O1 U1 a6 Y" C# E* }; g
- old_data = Sensor_Data[0];- H8 h$ _7 |5 O, S5 E& k
- return Sensor_Data[0];" t! ^6 H8 q" `! F8 j) J! a
- }4 B, ^1 E/ u; i9 x5 x- [0 K v; ]
- else8 g2 o9 {+ X) g! i N
- {+ ?/ [* u) @& f5 t+ E- O; T
- return old_data;$ A0 A. u6 s; U' }, ~
- }
; r3 y8 u9 @; u0 o/ A% H - }
1 d* l4 r: g6 N# Q5 l
複製代碼
; Z8 `' \( Z* }! R/ J& vMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
9 @ {, D7 W7 ?& u
3 W& T/ u, U" {& n |
|