|
|
& ^4 L! X6 Q4 j
- #include <Arduino.h>
0 L6 ]3 G, a! Q' C/ H" ~" w" n - #include <MeAuriga.h>
' J7 ~5 L# K- A" |9 q1 Z3 U& y+ ]3 G - #include "MeLineFollowerArray.h"* I* o) p4 d% T$ E6 n/ M
. }, w. J6 {9 n J: t4 g9 B* i- MeEncoderOnBoard Encoder_1(SLOT1);
' _: F9 b& e; T4 K - MeEncoderOnBoard Encoder_2(SLOT2);8 k" N8 E, H+ E; r6 M7 `+ j8 v$ z
- MeLightSensor lightsensor_1(12);: ~' Q- ^: i1 R. a( D. E
- MeLightSensor lightsensor_2(11);( X" E W4 F! W ^1 o6 C; W) b
- MeBuzzer buzzer; s( k' {* S4 p! s3 V
- MeLineFollowerArray linefollower(PORT_6);
- D0 H% C) }9 s7 x' F f
7 `- V. J+ E! r1 ] o- #define Error1 1& q x- m1 V# Z# o
- #define Error2 2' U, B N3 M( u' P2 o# I9 v
- #define Error3 3
4 ^* h: T& z W: D. J) q3 z7 b
9 l0 ]+ G- @# ~- #define Kp 15/ |- H& M) [5 r, r9 w
- #define Ki 0.15
/ J* c* m2 `+ u1 a( w, M0 a, t$ u - #define Kd 0.03) w1 R: _& K, K
7 U: ?( L( F+ t7 E5 b( @3 o. c- uint8_t sData;3 g& \1 k" g8 [9 a; b( X
- uint8_t D1;
& W0 g5 l5 ~/ F; L) i3 ?. G( K - uint8_t D2;; u; w) E$ F% ^' W- F. o+ {
- uint8_t D3;; q5 F3 e3 |7 n! `9 i& } T8 b! B
- uint8_t D4;
" `+ m9 z7 c, A$ |# o: H - uint8_t D5;
0 I* R; D* g# X. U, Z - uint8_t D6;
' }9 J/ A: X0 m+ X - - d; o3 R s& k- p- q7 ~ g
- float previous_error = 0;: ^9 o% \& O) X+ P1 m& E
- float integral = 0;) a& u* y. |$ V8 Y9 g
- float derivative = 0;
# I* u! r1 c# ]2 A/ V2 i - int Speed = 160;* M. }& V) [+ B/ M. `* Y3 ^
- float output;0 l1 f4 e' O2 H/ S
5 u" o3 r$ f" S( }" Q2 t/ K. l- byte Left;6 G. \# L# `( w( F+ J, }
$ [! [1 Q. k# ^! `- void setup() : A1 e# u2 L* L1 J L2 g/ U
- {
G1 }' M" R2 Z5 v7 c# d& S - //Set PWM 8KHz
% X0 U) J- L, v5 w3 K - TCCR1A = _BV(WGM10);! F' ~) H+ j* ^" w( _
- TCCR1B = _BV(CS11) | _BV(WGM12);
) C' O- A% f% O$ {; v - TCCR2A = _BV(WGM21) | _BV(WGM20);
1 K; `3 e+ v1 e: k: H' W! k3 n - TCCR2B = _BV(CS21);3 Z) f7 G' x1 N1 r
- Serial.begin(9600);+ x4 J9 m8 N& o1 ^, Q
- buzzer.setpin(45);/ P$ v, F9 k( m9 v
- }
( h$ B* H1 D/ v, j+ n( ` - 9 R) r* K }/ Q/ J6 s2 d$ t
- void loop()
5 C% X6 R4 N( i1 @+ \8 X7 H - {& P! b/ `( q. F4 N/ i- w
- while(!((lightsensor_1.read()) < (10)));
& X% L7 L5 i5 v! _' @: O" ? - buzzer.tone(1047, 500);) a! ^9 \. `- k; o
- delay(500);9 i& E4 S# t% w$ ?" \: k9 }1 {
- do1 F5 o# r! ^" }" d9 y
- {
7 ?* t9 ^; r( n2 B9 Z+ _( v" k - int Err = getErr();1 L% S) h+ w; a6 ?+ E' g
- if(D1 == 1)5 M+ v# R! ~* {
- {
; D% X; c2 v. b, ? - Left = 1;
/ D' D1 L! N1 ]& ?: o5 f- O R8 t - }' b u: ?& n3 l+ o! X5 c( i
- if(D6 == 1)! Y8 z) l( G8 Y! @7 ~% j
- {& H d: g7 N& @2 U" N
- Left = 0;1 m1 e- x. h2 b: c6 c7 R0 G
- }
6 |( D! d8 G8 X T5 W7 @" F - if(Err == 99)
0 r9 A4 I* z' R. I5 ^ - {2 n; O7 N* R( F( z2 ^2 Q
- if(Left == 1)
4 s& F7 P# ^4 S7 E' e3 \5 p - {8 M; `0 M8 B. h
- Speed -= 5;- Y# y3 O& W( Q& R- v+ X
- moto(0,Speed);# Q4 P3 Z- @- s" {, o) ~& @5 J
- do
8 L5 X2 s& y& W# i( y# B; {; n) o; l - {1 r& O; _$ @) {9 @& Q. I, O+ G
- Err = getErr();/ R" Y8 h5 ~0 t
- }while((D1+D6) == 0);
( G8 d7 k& ?6 y- g3 r - }
/ N: G; a$ z, ]( b9 n# ] - else8 I1 ^$ X# _ @1 j3 a$ J9 o
- {( e" E( J3 h- Z
- Speed -= 5;
5 K R: u" D' {6 v$ e - moto(Speed,0); , ?, |% m; I& I6 A
- do! _! G v) t# k6 i4 v3 t, {; M9 s
- {
: x" f! a$ W, f7 M- } - Err = getErr();& e- Z2 _8 X8 l
- }while((D1+D6) == 0);
/ }6 p" O4 u) V7 h8 ^ - }. E. y7 ^* Z* \1 m0 O
- }
9 t( @+ N) h; D* d T - else6 h, M7 A3 Q7 w+ A N9 p
- {$ u% E# D% u n( b
- if((Speed < 160) & (Err < 2)) Speed+=1; ! Y7 F5 T- n) C" Q( S" t( T
- if((Speed > 100) & (Err > 2)) Speed-=2;
+ I- x2 M s" ^( |8 |& o - integral = integral + Err;
$ ?$ @" b5 `, O - derivative = Err - previous_error;
- Y) C5 f: s8 d; ~ - output = Kp*Err + Ki*integral + Kd*derivative;3 F! @& w) s- k2 g/ ~* I0 b) B0 {
- moto(int(Speed-output),int(Speed+output)); C# I' }! P' u: h! Q ?' [
- previous_error = Err;- T3 _0 F) P" @3 N
- }1 a/ P" y& \3 B# V- m$ Q7 Y# E
- }while(!((lightsensor_2.read()) < (10)));! W8 [7 O& j% j1 Z
- moto(0,0);
. [4 O8 l" G! g {' ?3 T - delay(500);
) i8 `, H7 K) e6 c3 S4 p u - buzzer.tone(262, 500);( H" Y5 f/ d2 ^5 P7 J8 c
- }5 ]) c( P1 ?) \3 |+ }: y% T
$ q! S! C2 m8 @. `9 E; ]- int getErr()6 d6 Y2 R3 s$ u8 Z3 o* T% L
- {
* C- C5 E8 ?' @: y2 g9 w; n4 U - sData = linefollower.getValue();( Z2 Q1 H7 s5 G+ J$ S$ W* B
- D1 = ~(sData>>0)&1;$ ]. t: A3 d5 g! w; \4 r9 H7 m/ [
- D2 = ~(sData>>1)&1;8 G: g j2 u* h4 c: q: o; M; I' H, h' ~
- D3 = ~(sData>>2)&1;, w+ B2 M7 W8 s$ p* ]) z
- D4 = ~(sData>>3)&1;
8 m. Z* g) l/ s3 ~# }1 t - D5 = ~(sData>>4)&1;
T: L# O6 u! y, a+ X5 ^* M - D6 = ~(sData>>5)&1;8 A/ W" N2 O; l9 c! i7 y7 H
- int downD = D1+D2+D3+D4+D5+D6;( p" o, q0 @ [( H- x& B3 F
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
* q1 K/ A8 S! w( ~2 F - if(downD == 0)9 S+ _9 Q1 C8 D. X# U# V9 L
- {; y# |9 J% _5 n
- return 99;
1 ^6 y" N7 I8 u+ [0 \" Y! d; K - }
) |1 Z8 C/ G4 t5 y* I' X3 s - else
4 H* ^# Q9 V$ \! t4 L - {
& n0 |$ c: G& Q) L - return upD/downD;) V+ T& }$ }; L) H8 \ L4 |
- }4 g" n: k/ w9 f2 n6 h8 R/ h" Q
- }
0 V6 g' _9 I, V5 w' W
T x. X5 {* N% s# [" i" E) C- void moto(int Speed_L,int Speed_R). a7 q0 H# q# q9 I( N& M9 u
- {' U& z5 _) q7 C4 b$ |
- Encoder_2.setMotorPwm(Speed_L);1 ~9 Z( V5 A8 F1 T6 U+ m f
- Encoder_1.setMotorPwm(-Speed_R);
) Z4 ?- y% @/ ~' f0 } - }
複製代碼
g+ U9 k9 ^) ]- G3 T+ p5 U# JMeLineFollowerArray.cpp
8 v& U! d0 F9 g' d- #include "MeLineFollowerArray.h"
# V& {$ P! y0 o! Y - - ]% T" y; v9 q, K+ W o+ t. _0 g
- #ifdef ME_PORT_DEFINED2 F6 g( s: T1 p
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
& ^1 n' V e4 ~7 m. O3 J% C - {0 ^+ `% u2 j) E! r4 W
j- P& |7 L$ w8 e9 J& Z1 w- }
j. ~+ j: z2 T2 j1 O* D3 N - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)% S3 |, d: u( s! n* h
- {% E9 r1 |; u5 Q! E. s9 ~, V S8 T
- _DataPin = mePort[port].s2;
' t4 b- m$ ~" Y$ m3 ^3 X - pinMode(_DataPin, OUTPUT);
& @! g8 q+ I- g! a) `0 e1 l - digitalWrite(_DataPin, HIGH);+ m2 d+ F: {5 w. f# q: ^$ |) W4 _
- }/ |8 x2 E& `; U7 A
- #else // ME_PORT_DEFINED' G# I$ ?; ~9 m; ]( `
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin); {: ^7 B6 ]7 P' w: I
- {# j/ k, D8 W/ `9 ]$ ]; r# b+ c
- _DataPin = pin;
% A" Z2 b) d' E& L; O, C; }4 ?+ f - pinMode(_DataPin, OUTPUT);
: c6 X5 F# Y/ i, w - digitalWrite(_DataPin, HIGH);
" Y+ _9 ]5 A3 _; R2 e! f - }
. A! K+ e: Z% e - #endif // ME_PORT_DEFINED: y9 |/ d! l; z. t4 X+ K! w
5 M7 A* G0 e8 T
& ^! ]0 ~' M; b; M; f- void MeLineFollowerArray::setpin(uint8_t pin)/ g/ d* T1 j5 f
- {
$ I+ | V- {% x- [9 k; [' A7 |9 w - _DataPin = pin;3 v; _7 F7 l+ |
- pinMode(_DataPin, OUTPUT);
5 L! S" A/ d; h* N - digitalWrite(_DataPin, HIGH); {% R( n4 J# P1 B, \
+ W% z+ S# @& `% E- #ifdef ME_PORT_DEFINED, u) o# K2 I+ n( V4 z3 n
- s2 = pin;6 |: Y5 m; K# @
- #endif
+ U5 b8 G9 I- l) v5 r; O- m - }/ \+ S+ ?9 N( B) e
- 2 L$ ?' b: _ W: \
- uint8_t MeLineFollowerArray::getValue()
8 o6 W5 I, {( Q# l: D/ M9 e5 ^; d - {
" x- u0 u C( x) O - uint32_t LOW_level_read_time;( }6 k, \" J) F! A9 S
- uint32_t HIGH_level_read_time;& _+ x% k8 V, F! h. i) n
- uint32_t time_out_flag;
. R1 p8 v: B* r2 }& G: m* i4 i - uint8_t Sensor_Data[3];
/ @+ f' v) q4 x2 | - static uint8_t old_data = 0xff;
8 @6 i7 |. U* H/ W/ x$ s s9 M
5 ?9 I( Z# d0 q0 D. ?$ E. q- pinMode(_DataPin, OUTPUT);
$ G& Q; S- M6 q3 [* @ - digitalWrite(_DataPin, LOW);
2 |$ Q2 `& ~. { - delayMicroseconds(980);
) A, V9 |4 L* x - digitalWrite(_DataPin, HIGH);; H* ?; ~6 r; E8 E/ E: L* }( M T7 D
- O& X* e! ~+ n+ d! b6 s$ ?
- pinMode(_DataPin, INPUT_PULLUP);
# d, U- S& u, x* Y3 e& W: G - delayMicroseconds(10);( K( a, b2 @6 ^
: Q2 f! b& @( T7 q- time_out_flag = millis();
4 |& t3 J+ F: T7 L# u- h: Z! t, Z - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: E0 _- ^$ X; b8 I( o, P( ]4 t - $ L) |" [5 `3 ]4 j/ k- t9 D
- LOW_level_read_time = micros();; L) S( [; I# H4 V* c5 n
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out0 k9 t1 M+ z% L# T
- {
3 }4 E5 E6 q: N - return 0xff;
4 f( Q- U4 X" Q+ B; M! S - }1 x6 W1 l: K. t% v
9 v2 q; R" N2 N7 w- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% ?8 @4 J3 @4 f7 a
x7 a' h. ?7 k/ Y- HIGH_level_read_time = micros();
* X1 T. O/ U; M - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
8 ?. _ r+ Y( L* k0 T$ p
- G1 F, U. g" _+ g, f: V- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out, x! a% [/ o' S' f9 V8 Q
- {5 n) ?. j O) @3 p. K0 G/ B
- return 0xff;; y. \- e8 B& _/ W: ^: M
- }! }# ^7 h6 h* _+ X- h% Y. C) U+ j
- 4 j, J. j& K L8 R. P( u4 z
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))! }/ r" Z+ a0 G
- {
9 N5 \$ t* M0 i% r - return 0xff;
C4 r, O% d& f6 A6 X' f4 t8 ? - }) i, A8 k ^. y* b
- , M: w' B6 _! d2 _) Z
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 l3 L. a' e& T# ]. r* e
- LOW_level_read_time = micros();
3 L3 @9 Z, a" q* s - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level0 t( t3 m( I _2 j
7 y& D& d% |, w2 ?- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out6 E7 |1 ~1 z- z6 |. J0 c% n. l% I" T
- {) G( L2 [5 x% ~
- return 0xff;/ o9 i" D- \$ n6 O5 c
- }% G; C' j' R& Y2 ~# h
- 7 R5 [8 R1 l( r ?$ G5 ` F
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
0 X8 e8 ~% P" `0 ], L - {
' q% Z6 ^( K! W1 E - return 0xff;
+ v% N& R( V5 C* J$ R" ?, n. N - }
, `* A& l% G1 w3 m" z - ' t+ \/ x: Z2 I8 i7 r0 ~
- for(uint8_t k=0; k<3; k++)
" o/ m2 i: ? {8 Y3 a - {
/ ~0 p S. _9 ~0 P - Sensor_Data[k] = 0x00;2 V7 p. ~3 ]2 o! s( {
- 2 H s9 i7 B3 Z6 R0 A
- for(uint8_t i=0;i<8;i++)
5 {) A& y* U) z9 } - {. m0 k9 E2 ~# O0 c, }! |# ?
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
) x) |' C9 o5 b' H3 ]' I - HIGH_level_read_time = micros();
) l E @# M. b, s: A& i k - LOW_level_read_time = micros() - LOW_level_read_time;
7 t* m- M$ H, [' ` - - Q+ h: a F) ~3 `
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ), b/ p0 Q+ C$ L0 i8 x g5 O; |* x
- {2 m4 O0 h' f! c: Z; n
- return 0xff;
0 {. Y' R* g7 ] L' H" q! d - }: p& S0 _( t3 ?. i# W9 a/ ?
1 z3 r* F" m$ u2 b- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* u5 a# G& d% ~' D" ^4 y
- LOW_level_read_time = micros();
" s1 c1 Z' Z$ F+ X0 X - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level' a. B( M7 ] |; h* W- [
- " r+ k6 `4 q% Z) Y9 T
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1( I+ ~/ p0 a! s3 ^) J
- {
. w& k! v4 ~& H/ R1 { - Sensor_Data[k] |= (0x80 >> i);; i3 n3 T( b, d' y. [7 z5 d) Q6 }0 N
- }
& `: h; V* T. `- u9 _# H4 a+ N - else if(HIGH_level_read_time >= 100)8 @& z, U2 F" Z
- {: F2 O' O" C9 F
- return 0xff;( x( `) B( c L! K4 h6 J+ h
- }
0 v# y$ M0 S0 {: E# q/ M; n) L - . Q+ e. Q5 m+ s
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)! \: o! M& _3 ?* k+ _* P; w
- {" ]/ l" k! N; j+ b) Z4 {4 ~4 Q
- return 0xff;' ]8 Y0 M# @" Q+ k+ V
- }6 g4 K: Y1 G, t; z5 X- b+ k
- }0 g5 \& D3 u) r
- }: K% t) e& D5 d9 b$ e& T3 x) c& k. x
- ; V: E0 C" [& l6 z0 H- Y
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level, U' D) e. D- M) [' P# ?, \3 b
- HIGH_level_read_time = micros();
8 ^1 A8 S' k# [3 x - LOW_level_read_time = micros() - LOW_level_read_time;
4 l- Z! ^& {+ S, L. l; } - ; O# e: b+ K$ o* X+ ~& H( K4 d
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )9 P6 u o1 c. r/ h" h' l/ |
- {
4 g9 v0 @( c2 x3 T% ]! _4 d - return 0xff;
, S# O) m# d/ r) d- F, m - }
- M3 O! V) t8 ` - ! q: _% V2 t8 k; C+ w
- pinMode(_DataPin, OUTPUT);
, e- u+ a' v! ?, e' C - digitalWrite(_DataPin, HIGH);
& `; T r. J, P- W2 {: K1 Z/ ]# M - f3 o" R# z+ T
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))+ q: j7 S& }& h1 S+ s: h
- {5 n4 B- L) c" ~2 d
- old_data = Sensor_Data[0];
6 a" ~7 C6 o9 _# q2 H* t! i& ^ - return Sensor_Data[0];+ P! O( V* q, ?1 i
- }% c( B9 s: w- v9 j K
- else- }2 V, `% Y# d8 n
- {
4 B: K: A0 N7 X% h( @% D - return old_data;
6 z: p8 ] N; n+ ?6 w, W& p - }
1 m8 _8 D% J' Z+ M2 p6 K& L- b" I - }3 Y5 ?2 Z6 s1 n
複製代碼
& t3 s5 C8 `- v7 ^# ~MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
& B7 d1 M7 |/ \7 D; K) w; x
; _9 z5 X1 I( @
|
|