|
|
; z0 A+ t6 X! }
- #include <Arduino.h>9 {: Y& y/ z5 X# r8 b6 g+ ` S
- #include <MeAuriga.h>
) Z. r0 h' H/ h& s: h+ B; j - #include "MeLineFollowerArray.h"9 J1 R/ r& N: l$ J0 L7 q
( V& C F; |/ U0 Z$ n- MeEncoderOnBoard Encoder_1(SLOT1);1 N$ V1 e/ {; i# C K! h
- MeEncoderOnBoard Encoder_2(SLOT2);
1 J& X' p/ @# C9 g. I7 Q( ?. C8 d - MeLightSensor lightsensor_1(12);4 _1 V% W1 S9 P6 }: r
- MeLightSensor lightsensor_2(11);
& q+ v0 I& q+ S' G$ I - MeBuzzer buzzer;4 p. H, x$ \+ u, }0 }; M4 w8 r
- MeLineFollowerArray linefollower(PORT_6);( z3 }( U# ]7 N# r& `
- - [% o, U) v6 Q0 d
- #define Error1 1
; t5 T' F4 R7 L - #define Error2 2
" x j N8 x# n v" ^ } - #define Error3 36 k6 V2 l3 w( s7 C7 k
- X7 a* Y* r$ Q9 W' y( G0 d$ |- #define Kp 15
9 B: M; Z2 [( C! [) \$ E- N - #define Ki 0.15/ t3 P$ [; h" ^. ?' c& y9 q
- #define Kd 0.03
0 n: n4 W0 F. P0 f( c - 0 ~* K- k9 ?3 c l# s( m/ t6 }
- uint8_t sData;
0 H3 O, ~3 S( b! c - uint8_t D1;% @( H( J! ~5 q( f Z# `0 S! ^
- uint8_t D2;
+ X H, y8 ?& B1 u - uint8_t D3;
! X5 p7 T( b+ B! l8 [, G - uint8_t D4;
& V. ?* F1 e) e7 V7 g" ?1 b - uint8_t D5;
0 b8 m% l" m! {, {6 ~ - uint8_t D6;
- }9 a3 e) ~% ~7 A0 B9 } V
9 ]" p: |. K* w- float previous_error = 0;
% V3 J/ e9 v; |/ W- V4 q - float integral = 0;6 t' H9 v( ^& o) ` b
- float derivative = 0;% h' _ U" }/ h T2 G- s" }' f) L
- int Speed = 160; @( c% Q, h+ J( ~$ L2 J6 ?/ v
- float output;
6 W" I; |: ~" k3 ?8 T5 j0 e
' R- C+ L! u$ u- byte Left;
$ p V; J" b1 X/ Y
: t2 h/ C# T: l* |- void setup()
0 p* a$ q# c/ M$ F. d - {
! f. [! T) J0 }; W' b, n. z - //Set PWM 8KHz
9 X% t: A1 n& T6 j/ l, @+ O6 ~ - TCCR1A = _BV(WGM10);$ c$ Y& H8 z3 L) _& t9 w
- TCCR1B = _BV(CS11) | _BV(WGM12);. {& ?& G3 O" j( q5 |4 q% V5 g
- TCCR2A = _BV(WGM21) | _BV(WGM20);
$ y3 `, r O v0 [4 W - TCCR2B = _BV(CS21);
9 |, f8 @# Q! F: ? - Serial.begin(9600);
6 w/ I& w' d L- n - buzzer.setpin(45);
+ i% l) |# G/ g. A, z. \ - }
, r2 S. ~; R, y: F7 Z - + h% b0 g& Y; z- U/ C
- void loop()
: O- T1 S3 n: }/ H) _+ R( c, V - {
! J, R* Q+ \2 `6 T - while(!((lightsensor_1.read()) < (10)));1 I8 a O8 t: ~. E( u2 z! j3 c
- buzzer.tone(1047, 500);9 ~& L; W9 t- a8 u3 ?
- delay(500);' u- r5 ^8 B1 l# ^
- do8 K |9 P* g! o8 K7 q& }
- {) @9 D% ?! c; l6 C" V
- int Err = getErr();
0 g( f v7 H3 t; ` - if(D1 == 1)3 Q+ r2 L8 E N* B& t9 i3 b) q
- {3 w3 w- y$ D' P% D# ?/ ]! y
- Left = 1;
/ m. F. C3 n( i, G! t - }* d# e, M/ F1 I1 f' q0 S( ]
- if(D6 == 1): `& R, A+ ~8 t2 ~0 ^
- {. F6 I* p$ ?( ~% O$ c
- Left = 0;
5 P8 Y+ D, K8 O, A$ V3 t) K8 o9 Q - }0 p( B) k e" }+ e1 a' b& A( I y
- if(Err == 99)0 k* p' c& r; ^8 n; M/ @
- {* y: r) z9 x. c' d
- if(Left == 1)4 T9 W8 f5 x9 E" {
- {/ S G. W! N/ f" l& o1 w
- Speed -= 5;
+ Y A7 G! h4 [, K1 t" D! x - moto(0,Speed);+ Z4 u% h) b: h# K
- do! J2 p: Z) A# D
- {
( L0 h; G/ x6 @8 _9 j - Err = getErr();0 t1 E2 _: F0 ]2 E% G% `
- }while((D1+D6) == 0); [ Z0 C2 j6 j. D
- }+ x N+ w J- P( f4 z( o- r
- else
; ~9 X! x4 `, R+ a: l- } - {
$ }, P+ z9 o7 Z0 c - Speed -= 5;
# b4 K; |) h5 Y* ~+ H( I* m0 P - moto(Speed,0);
9 {6 V3 v0 U; x, J - do
; d( K. I8 r8 f9 e - {
# r' X) a( E9 X/ C* ? R3 ] - Err = getErr();
: Z6 X6 X$ W ?7 B5 p& F. T8 ` - }while((D1+D6) == 0);9 P9 y- f! X- \' I8 u2 r. s$ t& X
- } [8 b7 V( t5 C
- }# {: n8 W. W. }, W2 I3 u) v4 ?- q& l+ V
- else
0 Q* E: i# f2 u2 Y2 L# E0 U- P - {
! o @- p3 f0 H7 G9 U0 z' ?/ H - if((Speed < 160) & (Err < 2)) Speed+=1; ) V, ^+ l9 }" y: P1 n7 c; W
- if((Speed > 100) & (Err > 2)) Speed-=2; 8 {5 G5 a2 n) I! g& n8 k' k
- integral = integral + Err;
9 y1 u) c4 P, _- V+ V; A7 o+ I - derivative = Err - previous_error;5 O/ |8 k7 n/ @ ?2 M- X8 ?4 x+ S
- output = Kp*Err + Ki*integral + Kd*derivative;+ J$ S6 J- I. ^0 Q5 o
- moto(int(Speed-output),int(Speed+output));1 q9 A4 M# y: r# p! ? N
- previous_error = Err;3 N. m$ j1 ^8 a: E. s: W8 m- o
- }
' n' Q3 I) M' B3 r - }while(!((lightsensor_2.read()) < (10)));
# W1 S3 y8 i8 Y4 I: T - moto(0,0);+ h: w: {/ } Y( i' {1 ^( D; R- \# K# X
- delay(500);
, Q$ l- K; h8 c - buzzer.tone(262, 500);' u4 F7 c1 m5 _6 |! ~3 B
- }
- ^2 x+ Y7 c; E6 m - / x5 \! H! ?/ a6 ?5 c
- int getErr()
$ i. k. V+ I+ L- s V - { 8 }2 N. k0 v% W+ \" K$ s H; B
- sData = linefollower.getValue();
8 U$ a6 I3 {! m$ ]$ l( W- P' ?3 c% D - D1 = ~(sData>>0)&1;
7 w ?' A: n m6 p0 N8 a1 _ - D2 = ~(sData>>1)&1;% Q' c# c9 r+ _: _ g- I
- D3 = ~(sData>>2)&1;8 I) z2 [4 q, i
- D4 = ~(sData>>3)&1;
s5 m# R- V; d' k& O* j - D5 = ~(sData>>4)&1;0 M9 M1 U+ k) q6 Z7 z# ~( s/ q+ M
- D6 = ~(sData>>5)&1;
8 ^) ~ a8 \! X. w8 d4 H$ t% P O - int downD = D1+D2+D3+D4+D5+D6;! m3 C! T( x6 Y2 q o! ~* Y
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
) i' z; y$ H7 `1 ] - if(downD == 0)
r1 i' y% v4 G# B: v - {9 C/ D( a' W# W3 V* W) E
- return 99;
; Z; ]/ g8 R' } - }8 e# G& e! F* C' g7 Z+ R
- else- F6 M8 ?4 v9 S7 j+ [, _
- {9 A7 [* j4 ~7 K1 A
- return upD/downD;
4 o; h" j6 w) y1 E- L - }) `( L& t# b# h8 c+ u) v) R
- }, G4 z3 x3 r) F' `" r* O2 s# J
4 q) ~9 R% l/ E9 X6 t5 r- void moto(int Speed_L,int Speed_R)
) ]. \* _$ M% D. `. s1 p/ r2 R - {
; x/ q( `# ^5 @0 f6 W& J - Encoder_2.setMotorPwm(Speed_L);4 z- e6 C9 @6 {$ d: J
- Encoder_1.setMotorPwm(-Speed_R);
1 _% R0 c. l" ]% ^8 q! x - }
複製代碼 2 t% w8 o$ |2 F X
MeLineFollowerArray.cpp2 A, b' I( O( L$ M! G/ Q
- #include "MeLineFollowerArray.h"7 Z0 \9 s7 b5 O; W* h/ h9 g! R
- c2 M1 X4 U7 R% J) ^
- #ifdef ME_PORT_DEFINED
" k/ o$ T+ N; s* x5 s: R- H* D- \ - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)! X/ _% v5 p6 y, T
- {9 R3 E. w t- M( G! z* h1 Y( j4 M
- - i; D/ l. `% Y( O
- }
% z) L3 g0 s- h& J6 y - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
7 `, z: n$ c! k/ p, H; Q - {
0 S/ @6 s. u6 j6 X' o6 R$ K \3 S - _DataPin = mePort[port].s2;
% [( S9 b8 t/ ~' [1 ? - pinMode(_DataPin, OUTPUT);
: R" D3 w) Y: Z3 H- Y" W* a - digitalWrite(_DataPin, HIGH);7 P+ U* G. G' v! g
- }, w& E1 L' {* o( e3 C% J4 h
- #else // ME_PORT_DEFINED
- }% T) s' @( s; B' S0 P - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)( I. d8 ]/ m* s" F1 |
- {
# m$ z' t- s% X0 |" w& f% p - _DataPin = pin;
5 y$ ^ u7 a$ X - pinMode(_DataPin, OUTPUT); / o. g/ f2 T2 j% V
- digitalWrite(_DataPin, HIGH);0 T" I R) g1 ?7 B" r
- }, n- ?. J! L0 X4 f; V! z' H
- #endif // ME_PORT_DEFINED
Q) C' y \% d$ s: J4 o
( k* L9 n& O; W. p1 d0 r7 m7 `
* C# B" H" e$ U4 d- void MeLineFollowerArray::setpin(uint8_t pin)
6 j; }1 a9 z) i: V7 K) h0 D - {' \. I" W Y& r+ i) o/ A5 Q4 w" U
- _DataPin = pin;
/ N- M# p* Z/ @# P- q! A - pinMode(_DataPin, OUTPUT); 6 `2 }& X- h" j
- digitalWrite(_DataPin, HIGH);/ j) G+ ~1 d, v, f4 B+ D; |
% B" Z. \# t g- #ifdef ME_PORT_DEFINED
$ K2 U) E" e$ w6 ]. c - s2 = pin;
; g4 j- a7 i2 J% D, |7 I' y - #endif8 k u+ P1 H, [
- }0 Q# `2 G( X! ]
- . W: C+ L: H' M/ a" z- p
- uint8_t MeLineFollowerArray::getValue()5 C \& R' |' e& g
- {
: q- X/ B0 Q2 R, |+ H - uint32_t LOW_level_read_time; `. R9 B0 b& v( O: X/ Y3 E. K, @
- uint32_t HIGH_level_read_time;/ _+ p! n k7 j3 U
- uint32_t time_out_flag;' n0 `; w% v& H3 n+ ]' }& u
- uint8_t Sensor_Data[3];9 @7 U) O8 {3 i& F0 K" k$ ~; n
- static uint8_t old_data = 0xff;
% L% ^: `# _$ O" v+ C1 n" p - 6 L) j, e% Z1 g: Z. R7 u' g
- pinMode(_DataPin, OUTPUT);
1 x7 [5 r# j1 B% L6 Q" @ - digitalWrite(_DataPin, LOW);
" t+ Z- E T! W( t" q# _: z - delayMicroseconds(980);1 w4 E3 X, G8 h1 t. u+ p: M, |% g
- digitalWrite(_DataPin, HIGH);
6 d, L8 |4 m0 R: y. _8 E
4 @5 s ?1 P3 b1 G+ Z- pinMode(_DataPin, INPUT_PULLUP);: d2 |9 @) O$ M% @9 I& e% f; O
- delayMicroseconds(10);
, ?' ~. @* ^, a5 J) V, }2 A5 I
( u* p% u4 C& m& N% Y- time_out_flag = millis();$ G& t* m0 Q! F
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! b+ U2 k6 [; |% B
9 \$ N! {& G3 Y( l9 @- LOW_level_read_time = micros();
) ~0 V6 i$ C5 j8 L3 w1 | - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
7 p4 Z$ N# [9 A4 I - {
: o+ h* K/ D, X - return 0xff;4 W$ E4 @0 Q* X9 ~! j( o$ O4 c( @1 P
- }
+ F% B& U# Y+ W, q% K1 l& h - , u9 @7 |, N9 `8 X# G% [1 J6 d7 Z5 l
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ t; X8 y( c# S - 9 s3 N% j e# M% ?+ `
- HIGH_level_read_time = micros();
. f# J7 y3 Q2 c6 }: J7 Q - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level; r. p; Q0 K+ G, n0 E
2 j. S+ ^9 s; Z/ ~4 q6 ?- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 O" c8 U& K5 L1 t" F7 d" G8 ]0 o - {
, U( W: K; }% a' V1 h - return 0xff;
, R p0 q) S- `0 s% R; J: i" E - }, K( H; X5 u( R0 q; g3 e
- * f# g8 p$ |& i0 B6 W- n
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
. y5 S) X" C7 j0 H) a! r# [ - {
; x6 Z4 R" g* |6 X9 Y - return 0xff;
0 x* i# ^( k+ w" z3 I - }8 X0 Z) q: {4 f
% ]2 l; e4 S: p+ |" ~& l; r) ]- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- q% T- @. z8 O6 k - LOW_level_read_time = micros();6 }1 Q6 X6 a8 z' y9 W" v- B
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level$ s: | {! m5 B7 ^& v
7 V7 |$ R+ i# M" o1 s" V4 {; B. `- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 l$ s' ^! Y5 N$ R% d) W/ w
- {$ z& i) u6 z! k2 U
- return 0xff;
+ x( v8 L( \" K* n. [- t( v' C - }& a' P0 L5 M( l& \" T3 i; @2 ]4 x
& l+ q7 k; k0 Y) B0 _- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)) r# S; n0 m+ |0 H. w$ f* f! e% `
- {+ d& r0 s" G( X8 m) f% g6 m$ A0 e
- return 0xff;
$ f- r/ u/ N% h( q( G7 W9 _ - }
6 H& S+ Z, x. u3 r( G
) k/ W: g1 M0 p, F. @. y0 A- for(uint8_t k=0; k<3; k++): p7 e7 n5 O5 e2 E
- {$ C B) E( Y* W
- Sensor_Data[k] = 0x00;
E$ ^" a4 `8 ~6 a9 e - . w( G n. ^9 v" D1 R- }
- for(uint8_t i=0;i<8;i++)
; h( r- K/ n u6 m, P* |/ { - {6 b1 Q0 }1 C. a/ t& ~% x
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level/ T$ K- n# o3 W
- HIGH_level_read_time = micros();
! s4 m y. S* J# [1 k - LOW_level_read_time = micros() - LOW_level_read_time;
5 Y! N e" F9 W/ t4 q9 N7 T - 3 D( S7 X6 B5 U; K& \9 _2 `
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )% ~; K* u ?3 X
- {) j" d3 c8 a X; ^! P7 O$ ]' K: v
- return 0xff;
( b0 x% H7 P+ i; @8 U( d( Y8 \ - }
- C- u# V; n' L3 s - " M0 |4 w: V! J# |6 O/ ^! X
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- G/ v2 j$ l# u$ K- o - LOW_level_read_time = micros();
5 e0 `% t! S5 {; j - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
]( i9 G- w. i" D z* x$ w - 9 ?) R* ?5 x8 h/ I- j7 T) z
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1! K2 ^+ _4 B4 Y' P
- {7 t( \6 ^6 U# l1 Q( v: i
- Sensor_Data[k] |= (0x80 >> i);6 T6 c1 W; ~1 i
- }
2 G$ J2 t9 ~5 b7 h1 e/ n- ` - else if(HIGH_level_read_time >= 100)
( Y6 \5 L' I5 L3 [ - {
7 D. p/ L2 K0 K% Y( T5 k2 I \; f - return 0xff;/ n& h2 A1 B9 q8 K; X( Z2 W
- }
$ z+ p+ N$ J+ M( _
7 f& c! J9 z. T" J- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
% ]* x3 ?: {7 C- r6 n. f+ s/ a - {& I/ b" f# v m& U9 u
- return 0xff;
! e% Z7 k$ q% K9 E) ? - }* B* U( \# E. ^' b
- }
: }6 e1 c/ t& p+ X2 k- y2 w& } - }( m* `' m% g3 k; r# P5 {
- - S7 k( b# I) g8 l0 {; l7 m
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level; B7 } E& y4 i* l& a7 e+ t# L1 h9 Y+ }
- HIGH_level_read_time = micros();1 |0 M# U2 Q! }3 E7 }
- LOW_level_read_time = micros() - LOW_level_read_time;& r4 Y3 w" }. \5 R/ X4 e
: s8 A0 J" W, |6 ~- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )# x f! D3 x: g: G5 g% X* i- N
- {( g& u0 [2 f0 H9 S
- return 0xff;
) Q" x9 ]+ i1 I m - }
, n3 `! Y+ w! q. U3 o" i; V - ; a1 |! E4 B! X# n+ O; P
- pinMode(_DataPin, OUTPUT);" I" i4 i6 Q. j E
- digitalWrite(_DataPin, HIGH);& E4 A2 M! u* U" f# I; Y' J
$ \ @$ j, j; A% o/ @8 N' |- u$ ~- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
9 B& }7 g0 Q' z8 d2 U - {
& q1 N. e; w8 }! w - old_data = Sensor_Data[0];% f9 Q* U9 ^5 ?8 B4 P
- return Sensor_Data[0];' S( Q2 z. S5 K+ q Q
- }
* o4 k) I" \2 a - else$ R+ ?" {* u0 L, z) @7 k Y& v* e' P
- {: z i4 ?0 j" ?+ v$ b4 W) s7 f
- return old_data;
! m$ l6 s" F- Y. f: y - }+ m+ u: j+ {) i# r6 h
- }( b: D! T+ t6 J4 E" D1 `
複製代碼
4 }* K8 G' b) o& v8 Z) |! W) MMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
+ D$ c- F9 @" S) r0 p! D
9 ?0 e- g, Y$ H7 S: ~; q& k |
|