|
|
: R. ^2 Q! A& ~1 b( c2 ]2 f z
- #include <Arduino.h>
5 y& E/ O! K# c0 i# A, z6 z - #include <MeAuriga.h>
: [! U; G* x, D* w - #include "MeLineFollowerArray.h"
4 y: R/ a: I0 \6 ?& B8 u, `4 V, i - / D; P8 i0 `7 m. A8 K4 N
- MeEncoderOnBoard Encoder_1(SLOT1);$ h _# |; @) i8 ]* I+ H* ?
- MeEncoderOnBoard Encoder_2(SLOT2);
7 g' q( ~3 U7 [( W8 c6 l - MeLightSensor lightsensor_1(12);; b9 M9 m+ Z, b8 b; ?2 G- L" O
- MeLightSensor lightsensor_2(11);
a {' o8 m$ r+ a, w& \ - MeBuzzer buzzer;3 J/ C1 Q# B1 r0 o$ I" D' k
- MeLineFollowerArray linefollower(PORT_6);
2 P/ V4 K( ^+ I( c8 b
0 E! c4 h: V( p/ I0 S2 L- #define Error1 1
3 i6 d1 q m! s- u - #define Error2 2
+ r( Z4 [2 u( s6 i0 q" @ - #define Error3 3
( B) b c3 A1 {; w" @
: \/ C6 I0 q; q- u* I9 D- #define Kp 151 h- X4 m9 }3 r% @0 a' N
- #define Ki 0.15
7 m; k# P$ H: s, e/ b) s - #define Kd 0.03' j! }3 f" \" M1 i
- - `3 E$ i& j' `: D' Q
- uint8_t sData;
* [2 L. {4 Q5 H, H - uint8_t D1;
# @2 q! v Y1 T2 A( t: I - uint8_t D2;
% W3 X" f- m- `* M# Q - uint8_t D3;( Q5 |# d \0 S0 A1 w/ J
- uint8_t D4;; C) M# x( j7 u
- uint8_t D5;
- d: A: \* J) l - uint8_t D6;5 U6 g& u. F4 D2 B! {
* R" n( a$ ?/ e( u% c2 F. h; ?- float previous_error = 0;# q- ^, W# X9 g$ @1 i
- float integral = 0;
- `8 S1 |. D- c/ j( `+ h0 w( q - float derivative = 0;
7 T+ F9 v! O' [- T+ [ - int Speed = 160;
% e- s {' v# ]' c - float output;
5 x* r3 G T8 M X
2 s Y0 W* |! p/ v1 C* p9 `- byte Left;" P# V2 V6 A; I, W# T/ C
- * s ?$ a$ O0 |4 s/ Y
- void setup() 2 F( f: `: {" h% ~& e
- {1 u! t7 f2 I6 p) v4 z3 _7 M
- //Set PWM 8KHz
4 f/ l& e" i' t2 L4 k( }2 U% \* R - TCCR1A = _BV(WGM10);# y0 J$ X$ t8 ?5 }- q$ O2 Z6 O. H6 ?
- TCCR1B = _BV(CS11) | _BV(WGM12);% S" A8 @1 B( B, h, [6 n
- TCCR2A = _BV(WGM21) | _BV(WGM20);4 K+ u: t O! ]
- TCCR2B = _BV(CS21);/ [2 a8 a- r2 K
- Serial.begin(9600);
3 y% b' @3 V& [ - buzzer.setpin(45);* Q; b! d3 n O& k
- }
5 o$ W" E( I: z( D - 1 X9 `4 |2 o ]
- void loop()6 q, O& o8 b+ c& b$ ?
- {% W# L9 s& }9 T- B& {7 d9 V7 m' x5 E
- while(!((lightsensor_1.read()) < (10)));
, L9 O: s; X' X, U* K - buzzer.tone(1047, 500);) e8 A: _4 V6 Z( R- P+ t
- delay(500);" @" W9 S( m& p' |; ~9 H7 y5 z
- do
* [: N3 b2 d4 O, Y; H - {; h0 ?6 Z: d9 |: W, M2 y3 Z$ b
- int Err = getErr();
5 F9 i0 }/ c& N7 [: V9 r5 }1 s" p% d; G - if(D1 == 1)- Z) U* z/ {# p
- {
; a2 L' {+ d# ^- X% B - Left = 1;* h. V9 _* d& n4 f# m* s F
- }6 C! O* B9 f! j+ r: ~7 g
- if(D6 == 1)
2 |* m9 }/ `- G. ^4 Z+ N - {
@* ^8 J8 h+ U$ j - Left = 0;1 j) i! ?8 q; {8 B
- }& K4 N! r! b5 u, E W, E
- if(Err == 99)
8 d. q! C5 f! n, T$ b+ ~ - {
8 f1 Z/ h8 Q) C* X. f* s1 }1 O9 ` - if(Left == 1)
7 A& F7 `/ @5 r6 B4 Y8 p' s2 R0 K - {
3 G2 R% Y+ G* T7 r% e6 [ - Speed -= 5;
8 ~5 F2 A; B7 A+ H. y& K1 @ - moto(0,Speed);. x. R n0 y4 r5 b* V1 H0 u
- do' T9 J# U# b& Q8 a$ S
- {+ b0 X2 n/ j- ^8 i8 i
- Err = getErr();
9 ?' [4 z' {* Y5 B+ E! F$ L$ L! b - }while((D1+D6) == 0);
* K r5 I0 t3 B& `# t - }2 h0 ]( K1 E& S
- else
3 B0 k R: W0 Z% B/ a - {+ D! w2 l6 b5 M3 e
- Speed -= 5;. P* F: D% v$ _- h2 X( E4 X, r3 l
- moto(Speed,0); 7 i+ f8 c" ]2 p7 q0 l
- do/ @* A/ L% R5 m; T$ T3 U& {) g7 f' \
- {
Z3 D4 n. W( ^. C. V# ?: t8 Y2 h - Err = getErr();
2 {" K" i0 a9 b# d( j - }while((D1+D6) == 0);
: D- i9 z& W9 B1 i' x8 r( L' j - }
. n4 u% @- t1 r - }
+ |, t8 j* A: `& Z: k5 \/ J - else
$ L. N) A7 @- ] - {3 V) |) x x+ b+ M) W
- if((Speed < 160) & (Err < 2)) Speed+=1;
# q1 P! K# d" [, u2 u - if((Speed > 100) & (Err > 2)) Speed-=2; ( f* C" T$ }: V/ D! u4 @1 a
- integral = integral + Err;
* N' |) T( @1 z, ~ - derivative = Err - previous_error;! y: T& X" z" u3 s) V% @" j/ R
- output = Kp*Err + Ki*integral + Kd*derivative;5 e0 t# q3 `& M$ r3 `
- moto(int(Speed-output),int(Speed+output));
5 e3 G$ Z9 L4 z( v/ S - previous_error = Err;# W, z+ c/ z# v9 v2 X# r6 `
- }
% H) K5 k" B, D0 E; j; D* ~ - }while(!((lightsensor_2.read()) < (10)));+ U& L8 j5 V$ @3 X5 f! `6 j
- moto(0,0);( H) N2 s6 c( r) N
- delay(500);8 n z* q' X$ C
- buzzer.tone(262, 500);
6 s% I: R* S% U/ a. }; L S - }
: w" I8 F2 ~& w, |! P7 e - . T( q5 `& {) F7 ^
- int getErr()) W# ]! D: A! z; N$ B7 P2 Q8 \
- {
( y: H: e/ q( @! i2 x% W: P* M - sData = linefollower.getValue();8 J C. Z; o( Y, i1 i- ~
- D1 = ~(sData>>0)&1;
0 I& B; c/ G7 ^6 S/ G% ~- @ - D2 = ~(sData>>1)&1;6 P5 ]# L2 h4 ^2 h+ G
- D3 = ~(sData>>2)&1;
7 V4 V% w# k9 P$ u+ j - D4 = ~(sData>>3)&1;# C0 Y# g l O, ]; |6 {1 j3 d
- D5 = ~(sData>>4)&1;" J# x6 x, I9 |5 u& C5 m
- D6 = ~(sData>>5)&1;
8 Y1 }. v `6 @" g. M; @ - int downD = D1+D2+D3+D4+D5+D6;
" ^* c* D$ L% M$ E - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);' s1 ~# O3 W, @) Q7 v6 A4 r% \
- if(downD == 0)
" K& v% B3 T0 @/ p) Z - {( V/ ]: b! L w& M, T5 X
- return 99;. U. X4 I/ \* c! \9 E- Q) w1 [
- }* u, m5 O2 N4 w0 r
- else
4 Z$ d, }( m1 v, ?& b8 r - {
2 O7 u) f' `/ V! R: l, H - return upD/downD;
4 y2 f7 x1 P' \( K! c) f - }
- e$ w: Y1 v, t! u/ \ - }
) t+ j8 w7 n+ c$ X3 ~' [% |9 ? - 2 m- _+ I/ W) {- \5 f$ [
- void moto(int Speed_L,int Speed_R)/ x" T4 F1 n$ g0 c' x
- {1 E0 u" f w; Z( ?
- Encoder_2.setMotorPwm(Speed_L);$ j' i0 h# m6 J6 @
- Encoder_1.setMotorPwm(-Speed_R); E4 H. H0 x% u
- }
複製代碼 ' b) X5 v2 ^' X: }
MeLineFollowerArray.cpp
5 J+ w7 N7 N$ P+ Q3 k/ B- #include "MeLineFollowerArray.h"
# v1 F$ F. d @# g7 o - % d' O; w( h) G" K
- #ifdef ME_PORT_DEFINED, W: x. S+ v& ^: Q( h" A, k* v
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
G- @. r$ h# Q+ N- D - {
7 m4 }' i9 h, g7 K" p - 2 O- n1 ~5 v! b; R
- }
6 {0 E. y d/ `6 e) | - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)8 Q- r, |2 z; D
- {
& U' c. n/ r" Y0 E - _DataPin = mePort[port].s2;: m' X" }8 f* B% C% C; |7 S. d
- pinMode(_DataPin, OUTPUT);
, J# }' t1 J5 L5 T* ^4 [% z - digitalWrite(_DataPin, HIGH);
/ ~- G6 \, \$ x# P4 ^ - }' U) A) X. O& R, _ A3 l" K
- #else // ME_PORT_DEFINED ^: U I& ^3 C1 n4 Z
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)% [+ j& I6 \( x! }2 c
- {
+ j) e z2 K/ E9 G6 c2 a2 p" F" R$ b - _DataPin = pin;% E6 m7 p( V' f7 [
- pinMode(_DataPin, OUTPUT); 1 Z* m) @% q; N4 B8 @1 ]; ~5 |) T
- digitalWrite(_DataPin, HIGH);
3 x/ l7 f6 O9 J/ _6 C- x$ a/ E3 m$ t( k - }
0 |( D! f, }0 Q; m3 g+ J0 c: F% v& E - #endif // ME_PORT_DEFINED
( L: K3 D3 V1 f9 d - 3 L m- E4 x T# K# W
- 2 P: {" A' d$ o/ v$ A- M
- void MeLineFollowerArray::setpin(uint8_t pin)
6 C7 ], m; S- z% d# O: p3 Y0 D - {* B$ J0 r3 W$ D2 G! x6 }8 L! f1 {
- _DataPin = pin;
: n" E7 y! k4 Y3 {5 V9 H3 R0 | - pinMode(_DataPin, OUTPUT); - w9 V9 G' D5 d5 V4 F
- digitalWrite(_DataPin, HIGH);
: }8 q& {& G* d+ I
/ P- e' F# ?# a u1 b/ C N9 E" _- #ifdef ME_PORT_DEFINED
7 L$ m5 x, K$ F2 f% E - s2 = pin;- Q5 h1 _1 ~8 B5 F
- #endif
0 T6 x' M$ q( x" t1 w/ d/ W) | - }
, J' }' o* O" E: o
. A+ h- e# P; M) A; s# p- uint8_t MeLineFollowerArray::getValue()
9 y8 G( l; v% P - {
3 w2 F. ?5 e9 I6 B( N, R7 K* H - uint32_t LOW_level_read_time;
' k$ }8 L4 b* ]$ F# v3 h - uint32_t HIGH_level_read_time;. j5 z. D% b, ^9 `
- uint32_t time_out_flag;
0 p- z1 O" R6 V" R2 k# C - uint8_t Sensor_Data[3];3 A; D* X1 C q, A& I9 O1 b
- static uint8_t old_data = 0xff;* w; B+ D0 ?6 s6 l" [# e
4 p7 }4 {+ K4 y- pinMode(_DataPin, OUTPUT);
6 v. X \ A. U! V& J1 B& X- @" R6 b - digitalWrite(_DataPin, LOW);" |+ c% ~: c. J6 L
- delayMicroseconds(980);
3 |" n+ s5 m2 o: H - digitalWrite(_DataPin, HIGH);
6 O& H8 t: O f' b- i& Z - 0 q$ d7 Z0 r2 @% m' D7 t
- pinMode(_DataPin, INPUT_PULLUP);
# r" e7 Q. V4 C' Z' \' V! B8 ? - delayMicroseconds(10);
7 M6 q) A5 _1 P, j0 B4 U0 B - 6 g8 S1 }# [/ u5 n
- time_out_flag = millis();
" K+ B" Q. q1 w! g" y - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" T1 a9 A+ x* }/ ^
- ' ~( _2 }4 o) c
- LOW_level_read_time = micros();2 y0 X* H+ i8 m. J3 c
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 s7 |( |3 j9 T# N6 K2 v - {
( U' y: d; }( P9 K1 h. W3 A! ` K - return 0xff;" z" u- |1 Q# C
- }
( ]5 n9 A1 c% o8 [# Y
/ {7 v, I; k3 d6 Z% @, v* ^- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- F/ c8 q1 a. g) G g3 O* K/ ~8 m
- * {9 {4 ]7 {& S9 r7 C
- HIGH_level_read_time = micros();
: ^$ K3 X. y7 S' @& U) a; _ - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level4 r% q& S$ u5 Q- ]
\# S* r" b; o: Y8 \6 n$ X- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
* H$ G" M3 \; \, H% r" [# K! S - {" |. ^, M- s5 `3 \
- return 0xff;/ F! u C# s* R( n
- }$ c' R9 \3 R; T( G. Z
) y" `" J4 [* o3 ^/ j0 u3 Z- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))* P# H, n2 v6 u. ~
- {
" @$ g _& i6 [: o3 q0 D - return 0xff;
/ B0 _, l$ f& Y1 G6 D2 z. S - }2 [8 q! {3 L. Q c! B
# i; g0 Q9 T& f( Y" ~+ Z# S* S4 Z! a9 `- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );7 Z6 K& O! K% t% t. E
- LOW_level_read_time = micros();% f; ~* b! X5 i$ l
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level* f* j6 \0 z$ C; `$ F# T" y1 H- Z. d
- 1 ]! j t$ {) O3 O5 I, n
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out6 e5 y( R/ ~, s* T
- {
/ {$ I/ P! ?2 J3 w3 _1 t - return 0xff;# [9 Y2 O+ ?* ?& }/ \6 x9 @
- }
# }/ ?3 B/ l% {9 ]6 ]
6 `- Z& |0 L7 b; r- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)). o# ?4 m2 `$ `: f8 \8 a* k! G
- {
/ K0 p9 t' ~( x - return 0xff;
6 o, E; f) w3 M5 W: x; R - }$ F1 g5 I) g/ V8 p
- 6 I" X9 H/ l: a8 O! ^
- for(uint8_t k=0; k<3; k++) e" Y5 ^" f+ f/ `$ _: C0 R) q
- {/ r. g* Y( d% ?
- Sensor_Data[k] = 0x00;; u5 L1 A5 b9 ?7 g; G* p* u d. C
- {0 H C9 N e; i) W+ r. u5 p* X- for(uint8_t i=0;i<8;i++)
; H& I$ Z' D- ~9 m - {. |, N$ q" z; U# \ U
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
2 A4 D- V) q% ^. C" ] - HIGH_level_read_time = micros();
5 j# g- g) R; F: i+ c1 N - LOW_level_read_time = micros() - LOW_level_read_time;
3 k" e9 K2 F( |3 o! x* O+ K - % n# k0 o$ \3 q8 T5 o; s& W3 r
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )+ Q* a9 \. c( E3 \7 L5 A
- {; @8 h5 }+ q, ?# U( a9 d+ f
- return 0xff;2 A; q( M; X9 s6 `3 \- }; Z( D
- }
! K# t5 w" w- ?* n9 N$ e, h) V" I
: m8 D% b; E2 Z2 V# ^% {- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- O" h% D* G: b& V( k; B5 s
- LOW_level_read_time = micros(); \' [# b9 {9 |! ]% k! z7 T3 A
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
: ]8 W, B; R% T. B5 j - 4 L0 }% O6 `! a- y+ f2 n
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
0 j0 }5 \) \7 }9 E# ] - {3 G* Y* f- w% q/ x) |. k( {3 Q* r
- Sensor_Data[k] |= (0x80 >> i);4 Q: {7 a5 X: [5 V* e8 x
- }) a2 }2 M: y7 G7 O6 ]& m
- else if(HIGH_level_read_time >= 100)
$ b# L+ B6 G0 Q' C8 ?$ f - {
( s0 D- h/ q* C - return 0xff;5 \# |1 d) N: b/ n
- }: ^+ r- L) n% ^
" u, M+ p$ g6 u5 C; f- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
& N; _( `0 k2 ?3 G& ] - {( [; K: s3 b, w; K+ ^! S: [: y, P
- return 0xff;
( K, |! e. m, R$ S0 N. n L" z - }
/ }; e2 A0 n6 l2 ?/ T* { - }3 s& c4 F" k& E0 M
- }# g) D; |/ O9 V; a; b
- F; I5 c ?: ^2 e9 g, _- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
7 f2 B, `/ H) M3 O - HIGH_level_read_time = micros();
* g/ \( F. L1 e' [6 i ]2 M - LOW_level_read_time = micros() - LOW_level_read_time;
6 y5 k/ h6 y. p! B3 P$ D - & @' T2 f6 y3 j5 k3 I& Q# K' m
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )( V1 O! ^) p3 m1 |
- {
* C0 i% D/ i% K- C" g$ v8 ` - return 0xff;
+ E' A0 R- `, ^! ?/ s9 b - }
: X0 |' b2 F. K. s - . z$ [- _+ J" ?8 D# {0 |
- pinMode(_DataPin, OUTPUT);4 C0 p$ A, i$ V; H) ^* Z# y
- digitalWrite(_DataPin, HIGH);
& r X7 |4 L+ ~- h% V - * N! \7 `/ ?" q! ?) U9 N6 G
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
5 O& ?! w3 B4 C# W - {9 f9 V6 |0 l( D8 t
- old_data = Sensor_Data[0];
4 X* P3 @! X" N0 l - return Sensor_Data[0];7 C! E0 c. y* T/ N- H( M# P% S
- }
d2 D0 Z7 z$ J2 O" S - else
6 [. G( Z( n7 P( ]2 k q - {
" V2 [* k3 M& b, I k" j - return old_data;
" E. e- |$ P9 q- y0 V! ]! m3 n - }
/ H( s4 R0 d. z j - }0 \4 c. Q: Z( Z' N. l
複製代碼
! t/ Z7 p$ N) _" n L+ f/ H! H% aMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
% a/ I# F4 s3 ]1 X5 l& B7 \3 x8 B$ f
|
|