|
|
9 ?3 V9 W' b% ?" n% j+ S- #include <Arduino.h>3 d6 Y( N7 M: i% Z
- #include <MeAuriga.h>6 u: |7 l( F3 X4 e" a' L
- #include "MeLineFollowerArray.h"
6 G7 D& e1 M' w3 a/ O7 U
8 B% \! e# A) _9 N- MeEncoderOnBoard Encoder_1(SLOT1);
4 Y- O% J9 _4 f( m9 X u" u% } - MeEncoderOnBoard Encoder_2(SLOT2);7 c$ T% M) x* _/ H2 v, w
- MeLightSensor lightsensor_1(12);
: l& }. X4 |! o# g+ S% Y t* Z9 y - MeLightSensor lightsensor_2(11);
2 H+ u% r" |" j4 I4 t$ ?+ k - MeBuzzer buzzer;
' q7 `( Y0 b: V4 L; C' r - MeLineFollowerArray linefollower(PORT_6);
2 \( |' a, o, Z. ]
1 F" `9 D, J2 V# a- #define Error1 1/ h; G! I* F5 K7 S, _& @
- #define Error2 2
Q" O. k; F1 g& r; m2 J; I - #define Error3 3, W4 i' _" k5 v! `
" u; y8 X; U! D; v' Q- #define Kp 15
) }) i8 `6 h2 o- Q - #define Ki 0.15. v4 J. y0 S1 v: q4 A$ }* W! A
- #define Kd 0.033 W& r9 U7 I6 |2 Z% T2 U
- % V" v5 \" L' ` v* y
- uint8_t sData;% }. k; g* |# _" p
- uint8_t D1;
# ?1 g# s, l! m& E: N9 z - uint8_t D2;, w7 U) @. _8 z' _2 t
- uint8_t D3;. N& `3 D% [. n1 Y
- uint8_t D4;) S/ f5 F: j& ~$ w4 n% z" s, l* \
- uint8_t D5;$ p: j5 Y# O+ i ~/ C0 [7 m
- uint8_t D6;
0 O- g2 h% u2 A( U6 W - , J1 g' V. s" ]! \
- float previous_error = 0;: M# B: I) X- C
- float integral = 0;
' G& |/ F- P; R2 G* q6 o - float derivative = 0;; u; f! f. P F2 Y/ g
- int Speed = 160;
" o: l2 h1 ?/ Q+ s& @ - float output;
" y# R7 z8 d2 M% u! T7 T- T3 m - # w2 N) ] i& k2 n. J& A; q% |
- byte Left;
4 i) F& j0 H& T# b1 `
! p, b/ C& j. y; [% Q/ C- void setup() / y# H; k9 G- Y W# _) U; Y- {# r
- {. V9 e: Z4 w0 G/ `- y8 E* p: h
- //Set PWM 8KHz
9 }3 H% g) C: H3 T - TCCR1A = _BV(WGM10);0 t5 b3 @3 T& M. Z" L+ G2 W3 e
- TCCR1B = _BV(CS11) | _BV(WGM12);# I0 o$ A" J9 {
- TCCR2A = _BV(WGM21) | _BV(WGM20);
% T k* I2 n# l* A - TCCR2B = _BV(CS21);# t% q# o0 ^# K* n* q
- Serial.begin(9600);
( j) M! G1 ~( I - buzzer.setpin(45);
1 Q. Q* l" B# D; a+ y8 a- p - }
$ E# d6 j' \! v* M% C
9 A5 u5 K3 d$ o3 u% t1 z- void loop()4 F% n- n. P2 @: S# s$ m
- {
) X9 H) D% {- Y; M+ P - while(!((lightsensor_1.read()) < (10)));
% F l% ?9 r1 T# U, o - buzzer.tone(1047, 500);8 }, I" c: ?* p9 a# V( y
- delay(500);
- z* X D- G# Y - do' r) `! G# {# s6 R; ^0 D
- {7 `# }+ f, w1 L7 y
- int Err = getErr();- S1 t! K' J/ \; a# z5 D; Y
- if(D1 == 1)9 Y! P4 _ m. g: y/ b7 R
- {9 M8 d& }0 P- L' M+ n! j
- Left = 1;6 m/ M: e G3 U* y! e
- }4 p" P8 A" V* a9 [( V
- if(D6 == 1)
, q9 R3 }6 B* a4 R/ M% v: U5 x: R - {
1 h* \; v7 Z' F" J9 v, X - Left = 0;
) b" I7 l5 I7 T; c7 t$ R v+ `/ B - }4 e1 v$ n& }6 } l$ d
- if(Err == 99)
( s2 f$ W* J [- x" O - {
, u" z( ^8 J" |/ u4 d - if(Left == 1)
' ?% [) J' V. x - {
4 d$ T3 D5 \! f( a a/ \ - Speed -= 5;
8 _4 }% x- A! F' ^3 V - moto(0,Speed);
( d0 D: @ S6 R7 N9 N+ h - do
% o& W2 v- Q" }7 r0 }/ I - {
. O/ A6 p7 L, O2 M, b - Err = getErr();
* C6 o P3 _9 ?6 d - }while((D1+D6) == 0);( u9 \5 c6 r+ E0 t' z7 N
- }
( v9 }8 E# a0 R; c7 _4 A - else! J1 n3 q1 Y1 b+ v/ i# P( s
- {
# U" s9 t/ N9 d" e& |0 `, H - Speed -= 5;
3 u# [8 Q9 u( o |# _ - moto(Speed,0);
/ Z2 u1 L! g2 T" B& L' z! J - do% S! i2 F; A3 ^1 C
- {
, g9 _+ O* q; i; y7 G - Err = getErr(); B: t. `' o! s" c3 P
- }while((D1+D6) == 0);
' H6 q; C2 v+ M' `9 _6 G - }5 {+ V! x* }4 Q! ?" N# x9 X6 z
- }
+ n0 g* B# F' e% a8 z - else- M B! Q A6 O0 Q" b& r8 c
- {
9 {! s4 J7 q5 w2 I/ T( G - if((Speed < 160) & (Err < 2)) Speed+=1;
9 n5 w+ f* G& N, A- d0 p% T - if((Speed > 100) & (Err > 2)) Speed-=2;
8 v; K6 ]% S8 b! u - integral = integral + Err;
$ V* X3 o- y; w; H# T) Y9 q - derivative = Err - previous_error;: X* Q5 f; Z: ^# E0 V
- output = Kp*Err + Ki*integral + Kd*derivative;
& ^$ @, L8 j5 V& o! ] - moto(int(Speed-output),int(Speed+output));3 ?" t5 D+ |% S# l: F$ Q( }
- previous_error = Err;; z- S9 l W/ `! {3 Z! c
- }
" s- E9 [- _1 |; Q) r - }while(!((lightsensor_2.read()) < (10)));
; C+ m) `: t! D: r. t" P$ P - moto(0,0);' a- H$ I4 a# M+ o! c
- delay(500);
+ m0 @( E3 g% A1 T) M) |2 x+ N1 U4 E - buzzer.tone(262, 500);5 V# T# Z8 f ?8 y
- }
" y+ Q7 o: E' x' M5 t
# A1 P- w" W3 }9 m" Q+ k" i- int getErr()3 X/ i' G2 S8 m4 C' ]) j2 X t
- { ) @- Z$ s, K- q$ P( g
- sData = linefollower.getValue();* t5 T3 X! U# q- t1 X6 ^9 ]3 x
- D1 = ~(sData>>0)&1;
) p& I: \; H+ ~: f - D2 = ~(sData>>1)&1;
+ j6 s9 h* P$ x% A ]6 F* V1 P - D3 = ~(sData>>2)&1;
6 i9 N5 C) T8 l - D4 = ~(sData>>3)&1;* Q% s' k% T* q' o' O& W
- D5 = ~(sData>>4)&1;5 N7 D+ I! [* l
- D6 = ~(sData>>5)&1;
# q7 g0 E4 j7 Z3 l - int downD = D1+D2+D3+D4+D5+D6;
2 S; v5 e/ N! _* `3 J$ n8 A - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
' |9 h, Q! V7 q% C% D - if(downD == 0)5 g7 I# t/ i, V6 u' N9 a
- {
( B- l) H! ^) Z. D. o7 V/ K* M - return 99;
! B9 o. `5 u; @, M) } - }
+ d& l& Z+ I4 M0 u& I$ M& ]# E - else3 Y, ]9 i& N3 x8 j; s/ {% t5 Q
- {
/ X% T1 O3 W) \0 a; P1 o& j - return upD/downD;
+ k& g' y* c3 m6 l - }: V: T: W4 s8 j* Z7 o
- }& o' n; k9 _" g2 U& @
- 1 `) W+ @/ F8 ^& P$ c# q& \" Y
- void moto(int Speed_L,int Speed_R)+ L h9 }. { x' h# k( \
- {
1 A, ]9 \4 L) { - Encoder_2.setMotorPwm(Speed_L);0 p0 `" _1 P2 ?+ ^: t0 X5 n
- Encoder_1.setMotorPwm(-Speed_R);9 c* }2 F( V: \' H, m3 \5 p
- }
複製代碼
* X' u* p5 u* [' I6 J/ ~MeLineFollowerArray.cpp! P2 i& Z6 F4 w, n; J& q) M
- #include "MeLineFollowerArray.h". ^* ?) f6 S9 e6 a* a1 }" \- R
- $ {" |8 I0 F2 [5 b; F' ~
- #ifdef ME_PORT_DEFINED4 X4 j8 }" _' R% X* z9 z
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
4 l- e9 M4 h$ t0 Q8 K - {4 D3 B; \9 o' I8 T, n* Z/ |
; M, v3 c& k. P0 [( r- }( M4 |. m9 \$ ]$ E
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
+ Q$ u7 e9 p, f( ~& d - {5 Z6 U6 R4 V, G- v( [
- _DataPin = mePort[port].s2;' C# ~9 P1 H" D
- pinMode(_DataPin, OUTPUT);
; C, q$ D) d: \+ y8 X2 s2 _ - digitalWrite(_DataPin, HIGH); M! Y- g7 P3 x9 h: i
- }
/ v+ V6 D2 w& A( d* ? - #else // ME_PORT_DEFINED
. {% m' i" B7 T1 [6 _+ n - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)3 Z7 C# [" \& d+ P! {
- {1 v) l3 g# _2 A7 e
- _DataPin = pin;" P7 ^* \% f4 |$ T, }* F* {3 }
- pinMode(_DataPin, OUTPUT); & N+ z7 V) `( s- Q6 w1 Q) Z1 X
- digitalWrite(_DataPin, HIGH);( ]6 y- R" Y; q! L- E
- }( a* A+ D" D+ g5 Y
- #endif // ME_PORT_DEFINED5 \, L, k) A! w$ j7 n7 b
- ( J6 Z C8 R+ b4 a# ]& z
( a l# t# V3 g. X# a" D- void MeLineFollowerArray::setpin(uint8_t pin)* m6 m# P O' n- V( U+ b/ L% T8 Q
- {
/ m0 I$ m. O( G; U$ O - _DataPin = pin;
+ F- q2 r1 s+ T& o# L# J - pinMode(_DataPin, OUTPUT); & `: X1 x0 s) H# \' s3 V4 U
- digitalWrite(_DataPin, HIGH);
: j2 k1 l! i# S1 e
& Z/ U8 h8 X+ ^- #ifdef ME_PORT_DEFINED
^) v( o8 N9 `0 ]& }9 I - s2 = pin;
( D9 p4 X& m ?$ h, G7 Z { - #endif' i3 u$ l* V+ L1 @' R! E
- }
6 ?) L, a/ m' A4 \2 d$ _6 t - ! J0 S- p' V' V& ~" H5 c
- uint8_t MeLineFollowerArray::getValue()! s" Q- B9 g# ^6 n4 z2 \
- {3 h4 w: b0 q u& U; K
- uint32_t LOW_level_read_time;
2 v) v( ^ y! M7 B" i* v - uint32_t HIGH_level_read_time;2 `3 Y, L3 F9 c3 j5 u0 E
- uint32_t time_out_flag;
5 C3 X4 E. D- x! x' C0 t+ { - uint8_t Sensor_Data[3];
" w1 l' K3 j, q5 {/ ]& n - static uint8_t old_data = 0xff;, }; l+ Q- @6 K* L
- * }8 p ? H) _; T- c9 P: t$ H
- pinMode(_DataPin, OUTPUT);! x% L: B9 f$ `0 X. a. Y5 y
- digitalWrite(_DataPin, LOW);
2 N+ Q4 Z( B v. {" V1 S - delayMicroseconds(980);
9 W7 T1 [* M! |# K- V/ b - digitalWrite(_DataPin, HIGH);* S0 ]8 n+ r2 d. A. A* t; @" f' j
- & e& q2 Y1 Q* q, w. v/ S2 z9 p
- pinMode(_DataPin, INPUT_PULLUP);
/ Z' z( |6 Z& n, ?, D - delayMicroseconds(10);
7 n3 U& x2 E) f
. a8 N; n. I7 n9 X: Z- time_out_flag = millis();8 r- ? ~2 @( z' F# U4 S
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 A1 d0 G) @5 h6 Y( R3 M
+ d+ \0 k) ^5 G; X0 A% a- LOW_level_read_time = micros();' a) ^# s/ o0 V5 ~9 u
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
+ v0 {- K; T! K& B - {
% m5 V' L. R @. V - return 0xff;& ?+ b* B6 Q7 j, C
- }
1 N* ], T% _6 L5 M6 X# c4 U
, N& x2 S- p& k) k' g- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. l( l$ d, N- w8 ?& A( o% e
- ) r/ f5 p6 H6 n& x- B
- HIGH_level_read_time = micros();
. Z' q* b: p; i7 ~: [* b - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
5 D( t# R5 |: U/ ^4 O/ Q! C% F - + P6 |" I+ a, c- N; }' q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out8 d4 A, G3 {& F2 h N
- {
/ [+ ?: S* T/ Q; m - return 0xff;
/ L3 O: q& K" c% x$ \4 T3 W - }5 d( u4 o+ T1 {2 _
, D" _0 @, ^8 [, [5 J- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))7 c( s" a8 v; H
- {
A) ^$ _, ~( r3 B4 w k: ~6 t2 ^7 n4 { - return 0xff;
: Y7 Y0 K3 a' x: w6 K, c& i - }8 P! N4 Y( R7 ]) A; C _& Q
7 s: |! Q t) T7 p- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 s/ M4 i% P4 ~) t
- LOW_level_read_time = micros();
I) M% U n( }9 H- {% L - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
D5 g) s$ `+ b+ H9 a( @; |. X3 H - ! W# o4 K. r9 F2 {
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out) q/ Y0 |1 l# a' \* |
- {
! J" o7 M# J5 w' Q - return 0xff;, Y9 i7 ~7 E- |3 A! \8 e
- }
* V" @/ p4 `+ n+ j' E& P9 E
2 W- ]: C3 I# o/ U- k u4 L& K- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
* P* \. _( m5 x5 ?- F: y% Y - {9 Q( I( v! V H- @; a+ _
- return 0xff;# U+ j% m. u- E( T- |
- }
I$ T: e) O" \ - 8 p# A& Y( U5 _+ ^( e0 g! `7 A
- for(uint8_t k=0; k<3; k++)
0 ?0 \- Z `7 n1 y. _, l - {; H5 M" f: y" o. \ x
- Sensor_Data[k] = 0x00;) h$ O! R* {" |( X8 j
- 1 {1 r7 ~' [' _/ ], N: v
- for(uint8_t i=0;i<8;i++)
- \" y/ Y( f S4 B1 Z - {' e! l0 M3 z# r u. K
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
& C7 [( k' @# V& x" B - HIGH_level_read_time = micros();
# U( B- T% {! _" p; _ i* ^ - LOW_level_read_time = micros() - LOW_level_read_time;
& Z6 ~5 Z- j, I6 I. g4 _ - 3 B8 F9 {; I: e; e) _! t5 F; a/ L
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
: F# X' ~; }% K* u1 E" P( w, n - {3 U0 d+ i8 T$ B! `, [
- return 0xff;
( u3 }# K8 h; o% g5 r - }
0 M0 T: B4 }2 q+ N - 6 N$ z7 x' ?% M5 D B* P
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' G9 k, l7 [( @& X i5 Q
- LOW_level_read_time = micros();" y3 n3 |7 Q4 u e- R6 @. Y+ A: `
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
* {0 @& Q$ ?; i) l, L! b/ b4 [ - - S+ U+ I: M3 g% j( C# {
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
0 G$ L' Z( v4 U ^ - {/ g7 v' W; j! ?& T: ~
- Sensor_Data[k] |= (0x80 >> i);
, f7 F$ |" e" H4 A0 \ - }( A- b' z9 w, J4 Y$ K' T; X' D. e
- else if(HIGH_level_read_time >= 100)9 _5 }. ]* E2 a! }5 |
- {, W$ e) e b, n- x2 r# m. j- E8 P
- return 0xff;
5 O5 [1 L+ c. J" R - }! E8 j9 n) P/ L4 A2 [
- ) L2 Y/ z0 t. a% V
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)( N9 w- H5 C: a3 w% R' T5 p4 W/ U
- {1 A: a a+ q P4 _5 b9 I, f
- return 0xff;
( h0 e/ q" P3 v9 K9 b - }
2 k" t6 \# }( \5 r - }. ~* x( {9 Y, u6 l" d8 n8 m
- }
8 i. v3 ?) P$ Y/ i! \, q' b - + v& I/ s8 D- Q& u9 o2 N
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
6 F U9 t- s( ~4 W9 A - HIGH_level_read_time = micros();
/ K2 k. d" V2 p1 k1 s3 t - LOW_level_read_time = micros() - LOW_level_read_time;
) P0 J6 D. m9 m& u# n, X. m# ~6 } - 4 Q7 j9 N/ j. v. N2 L4 W
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
/ {/ _( G V9 S- V( Q - {
* r" h0 v( X- T4 U0 x, l' n) V - return 0xff;
, k0 P4 ]! E& K2 f: ^4 [1 D! _ - }
# y: H _. F! a* Y& W- l - # {3 t$ w2 Z5 g, f& N
- pinMode(_DataPin, OUTPUT);
% V* X4 d8 k7 {( ] - digitalWrite(_DataPin, HIGH);6 _- U1 r- B* V5 p% ], w3 u: H
5 p& }0 X& T9 D6 C, g- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
+ I- L. v" s# p+ @5 a# M$ @ - { F7 T+ Y. I6 n5 d2 I% c
- old_data = Sensor_Data[0];
+ \1 M& p$ U+ D& P, r4 O z) P6 M - return Sensor_Data[0];
+ l* A+ q% T0 {4 D% ^& c7 M4 u - }( C9 o) ~' A! C/ R+ L' Q" k" s
- else9 O- n2 z8 N' k1 H6 c$ a4 c
- {3 ^; g) v* Q) W l7 x
- return old_data;
8 _3 C$ V6 ~) ] - }
8 ?7 o& r9 e& B" u2 J - }( `# ^' X9 `" T/ R2 K
複製代碼 # T( D7 J3 s8 `3 c7 l- _) V" ^
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
3 i! o( l$ j( T& z
( E4 G0 q: q6 A |
|