|
|
3 P4 B! \) K8 H- #include <Arduino.h>
& O- Q+ E: h5 p5 ?' `( w( R - #include <MeAuriga.h>
/ a3 X5 P% c6 P- \( |0 b F8 [8 j - #include "MeLineFollowerArray.h"
$ a5 c$ b/ E1 W7 A4 {! ]7 Z: \ - # m, L0 v( F# d7 K
- MeEncoderOnBoard Encoder_1(SLOT1);7 }3 X) f- _8 [3 o* j
- MeEncoderOnBoard Encoder_2(SLOT2);8 z. \5 s l$ { o" W! a$ l4 v- X
- MeLightSensor lightsensor_1(12);
% N. _; `8 b! V8 E- m- B - MeLightSensor lightsensor_2(11);) V* W- O& E$ l! ^! y9 A9 e
- MeBuzzer buzzer;1 q) _' v! l9 w$ G. Y# Z( h
- MeLineFollowerArray linefollower(PORT_6);
) F8 U0 }7 n0 N. G1 `. d: X" b
5 A; h0 Q7 w1 A0 K! j9 q; @2 J- #define Error1 1( U5 Z! v- [. Q0 w6 k
- #define Error2 2* R9 H6 f6 _9 J
- #define Error3 3
" \+ \+ w& {3 w! x% W* Y( g- g0 C
+ X' B; z; s; m! N- #define Kp 15
. J+ @0 I% p& [0 C& G( b7 I& D' _ - #define Ki 0.15
- a/ ?4 G4 }' I4 j3 u) b - #define Kd 0.03
" j3 V3 w" h8 i* F1 ~/ o m
' b6 F- x8 e; Q# t& Q- uint8_t sData;
! G$ S j5 X% \$ `) }& Z' v - uint8_t D1;! [8 Z, B m+ Y3 e$ N- k
- uint8_t D2;% b6 @ O8 I5 X: p5 ^( s$ O
- uint8_t D3;
0 J& C4 Q; f9 d0 v2 j - uint8_t D4;# m* _$ N- I; a' m, @. q1 A
- uint8_t D5;
3 z( ?; H1 R( Z u2 I& y - uint8_t D6;
! U8 c- p" O& F
- ^0 O6 ^( Q- I9 T& \ e0 }; Y- float previous_error = 0;: i. j3 p3 Z. U0 o7 B# D% x
- float integral = 0;1 _/ X+ ?4 K* F# |" S- W# X
- float derivative = 0;
! G& z9 A# X! x, d# u - int Speed = 160;4 h( n" X H) k1 L" d3 m
- float output;3 I# z! B2 B: }' a+ m% b9 f
+ p$ t9 w7 M0 V- H; }5 |- byte Left;8 t# U4 Y$ \0 ~2 x5 ]0 ]
- % A+ m$ v' m" g$ @
- void setup() 1 M* n3 ?1 n8 Y5 t
- {' g% {4 i- \) Y. K- Y9 l: O* J4 }
- //Set PWM 8KHz+ Z! m- @* c! w1 J6 m
- TCCR1A = _BV(WGM10);
+ R7 Y& n& `1 e" O/ ?* C - TCCR1B = _BV(CS11) | _BV(WGM12);0 \; _0 h+ ^, ~' S9 Z" T
- TCCR2A = _BV(WGM21) | _BV(WGM20);. G# G. V6 Y' l9 r) ]' [ Z2 [
- TCCR2B = _BV(CS21);6 {2 J9 ~! G) [. ]' M* i
- Serial.begin(9600);
0 n% h Z9 r* O - buzzer.setpin(45);. u# m5 F, Y6 T4 b
- }; H% b% @3 |- c4 T* i6 r
- " G( X5 f. z% Z& A/ M
- void loop()3 _0 a1 v6 B* Q
- {( v5 l/ B6 T, L2 H0 ] s
- while(!((lightsensor_1.read()) < (10)));, \& I& q6 I7 U/ x8 |1 X/ P
- buzzer.tone(1047, 500);
; w, a5 B" ^6 \; v) E1 C - delay(500);8 Z# f7 C. K; a5 v5 e( ]7 T
- do. c9 a5 R0 o; L
- {/ M+ g4 n( o4 q& x* e/ Q! x$ h
- int Err = getErr();
2 F5 A. I0 u# I& h+ z* N4 w - if(D1 == 1): ^# W2 w& d9 Y& E% a8 c- z
- {7 q. V, O0 g% p5 e& I
- Left = 1;
3 b! j$ } o4 M1 F& e - }, j' A R4 M& c$ @+ C/ n8 k2 i
- if(D6 == 1)" u2 X" H& q% S( X0 V, |
- {9 |. W. U8 _2 v @
- Left = 0;
6 {. q" T* u g - }
; d3 d; |9 D: F, d2 p9 \! Z9 F - if(Err == 99)
7 A! Q' P, K$ R& m# m# f! H4 u - {$ r; j" E2 J4 \/ W' [* c) `, C+ X
- if(Left == 1)
3 v8 d$ N9 U9 `: _7 Q! T* W+ T4 B! n- S/ ^ - {
: F5 e2 P+ c: M' z5 k+ n - Speed -= 5;4 Q' j$ p" [5 G8 L# t
- moto(0,Speed);
, }+ \" |$ ]; i' f" k0 K# d1 u - do
9 G* G3 h1 H+ [5 q9 F( ? - {
9 V4 Z8 L7 ]- p2 p, l) Y" K - Err = getErr();- N5 l& i0 {; ] z4 h
- }while((D1+D6) == 0);* v, r- i! ^$ e& x1 \3 t% x
- }0 }& T* D% u l. y5 C
- else
q3 a: r6 N6 d3 X - {$ w! V6 p4 |7 t) f7 h0 R' [. Y! i
- Speed -= 5;$ A/ M& H( |4 t) N1 T& s1 N
- moto(Speed,0);
) l$ K& ~4 p2 O+ F# d - do
7 L6 n/ Z- W7 T- i$ ?/ O - {( j9 h: C! T3 j6 a. B7 N& \6 c8 H
- Err = getErr();6 e% |% q4 ] T5 d9 s
- }while((D1+D6) == 0);3 w6 X5 Y0 ^2 b3 y( l
- }2 _9 ?: v& ~/ F$ d) Z: |- }
- }! N' y( ?+ H. D. Y/ i# ~/ f
- else
+ r5 Y9 w. Z! Q# c Q U! X; V$ I - {7 z9 c* T& |% v% { k5 N J2 W5 [
- if((Speed < 160) & (Err < 2)) Speed+=1; 8 I: C( n( s; G3 S, A: B* q
- if((Speed > 100) & (Err > 2)) Speed-=2; % i, y8 {2 A3 ?! \
- integral = integral + Err;
: ~# v: z( i6 W( q( _5 t2 s - derivative = Err - previous_error;$ ]3 e0 f1 Y7 {+ @( f
- output = Kp*Err + Ki*integral + Kd*derivative;2 e- y) S( n. c; R5 O. [! r
- moto(int(Speed-output),int(Speed+output));/ \) k; p% H! G: S' ~/ v# a
- previous_error = Err;
* W1 h$ z0 m; p. d5 P) O - }
8 ]! S. h2 t1 y - }while(!((lightsensor_2.read()) < (10)));& x7 V, j+ O8 l- \9 w3 W- n
- moto(0,0);5 {" O7 ?) U- u8 }% ~0 q1 t
- delay(500);
- n. g/ k& w8 s) k - buzzer.tone(262, 500);
$ G. v. D' V) n# w( k1 f - }
2 ]% I0 T0 l7 u+ C3 V) V
) x. Z) v# V- O; R' G3 R- int getErr()
5 u ^. Z4 B, I) n- Y9 r( N- l. \+ i - {
2 P4 e/ Y& }. _& Z - sData = linefollower.getValue();4 q7 Y5 C) U( K# D$ S, \
- D1 = ~(sData>>0)&1;
; W7 t" ?3 a* i - D2 = ~(sData>>1)&1;# k; Y% ~3 E$ e4 S9 f ?: r; B
- D3 = ~(sData>>2)&1;! O% w; v" k4 Y+ M0 a- }
- D4 = ~(sData>>3)&1;
! o5 b! t3 C7 o$ E/ P9 ?/ v* u/ t) G - D5 = ~(sData>>4)&1;2 N) Z6 O7 f: ~0 G
- D6 = ~(sData>>5)&1;
0 c0 u9 L; q' _9 Y' v2 } - int downD = D1+D2+D3+D4+D5+D6;% E- Q- R. {: E4 X7 K
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
! L {" U7 I) t( j' S! r7 M, ^ - if(downD == 0)
6 B% n3 d* S- c- B$ u* A - {# O V1 Z1 V' ]# ]2 t
- return 99;' w4 C F5 X+ ^# Q
- }
, x4 I5 i9 v* Y1 v6 C% A - else
) [# F( ?( O" L9 P - {' r1 Q) W$ ~& E
- return upD/downD;
' @/ E: `6 r; q& ^" J! A" z - }/ A6 Z, ]- ]9 z
- }/ g) b' R5 H8 ^* }2 L
- $ x; x0 J$ d( p& G* j# c+ C
- void moto(int Speed_L,int Speed_R)+ Q' X4 p: i, y# j! w' k5 L; _2 w
- {# Y+ i; u$ ~3 Q& J7 `8 Z! D L
- Encoder_2.setMotorPwm(Speed_L);3 h" M% A! K, c
- Encoder_1.setMotorPwm(-Speed_R);
' ]" C8 o% ^6 p6 G - }
複製代碼 5 b8 s, g: h- ~$ S$ m
MeLineFollowerArray.cpp5 C1 z( u! Y" U; ^. G4 U N
- #include "MeLineFollowerArray.h": n/ \3 W* v G
- & A3 M$ J3 a' R
- #ifdef ME_PORT_DEFINED
. `) |, d) j8 ]# [7 \ - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
~( I, N) y7 |$ D% T6 S - {, j2 o+ }8 m' H9 s/ d
3 K- b( L1 _3 e5 y v2 e/ h, Z9 B7 G- }
$ O% E& Y1 x' a8 c' p1 | - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)/ l( c# B. \6 x7 v& T! {
- {) B- H# l+ D4 B8 o: `
- _DataPin = mePort[port].s2;6 z H, K7 j! o2 J6 o$ G% a
- pinMode(_DataPin, OUTPUT);
7 W1 }% P7 a$ n - digitalWrite(_DataPin, HIGH);. v$ u: [, Z' Z) I7 t
- }
( m& g6 C6 v/ Y, { - #else // ME_PORT_DEFINED
3 q& E. y- O8 h( Q) f9 ` - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)& b7 m! L7 ]" o' U! E
- {5 d: ~5 F4 p) o+ V
- _DataPin = pin;: E/ ~: k& `: ^3 v* F4 _/ Y
- pinMode(_DataPin, OUTPUT); : [+ B+ w1 f" t+ r4 G% H
- digitalWrite(_DataPin, HIGH);
1 R$ j6 n' g$ g- Y# Z" D H - }
2 Q2 K, v8 h9 N8 @4 r2 ^ - #endif // ME_PORT_DEFINED
% B6 x8 ^; V% k. G3 s/ V2 _
- k+ \- v6 R' E0 {/ s
( z* L6 a% |; W, Z P! h4 Y* Y4 `- void MeLineFollowerArray::setpin(uint8_t pin)
- g3 N, ~8 J, m- V - {
; {& g+ l* P/ v: V0 b7 | - _DataPin = pin;+ E: ?! e( y/ A# N. }0 C' {9 K
- pinMode(_DataPin, OUTPUT); : d0 X ] i; J! J4 k, i! Q
- digitalWrite(_DataPin, HIGH);
& a) n; ]3 j5 o
! B! J' m& k% i' ]9 h% Z$ |) V- #ifdef ME_PORT_DEFINED
h+ w. J5 T; i" c% I - s2 = pin;9 W/ F% }( _8 l
- #endif8 D% o# j; s. ~: Z3 Q7 f4 b! q
- }; b& a" `" w0 }* t
% L7 C8 W- P% w/ x h- uint8_t MeLineFollowerArray::getValue()
7 s, Q5 B1 h4 a4 [9 ?9 a- s/ U5 Z - {
' ?) q8 s. \: \4 e% j' d - uint32_t LOW_level_read_time;
0 d+ L% y, A) x6 I8 r8 L - uint32_t HIGH_level_read_time;
7 @# g! V: A E5 }! Z - uint32_t time_out_flag;
0 ~; e2 K3 B; i; M - uint8_t Sensor_Data[3];
1 \1 D, r A4 `- j - static uint8_t old_data = 0xff;4 k" x' m, u2 e2 m* s) ?1 f& I5 B
! e; V" l$ E" [( e- pinMode(_DataPin, OUTPUT);# D/ ?; B1 j$ j/ B/ B* d5 o
- digitalWrite(_DataPin, LOW);
% P- Q% ]+ p) ?2 K- G - delayMicroseconds(980);+ W( _3 ~9 ?$ e% _2 _
- digitalWrite(_DataPin, HIGH);
( r. x& [" @# }: x- E2 e' }
5 A9 z' w; D' m3 b8 a% J {- pinMode(_DataPin, INPUT_PULLUP);
0 X$ g6 o8 r, S, ^& F - delayMicroseconds(10);' X ~7 b! L* l$ b2 E& C b9 K* W( z! w4 \
3 H" e' V6 w8 M+ m P( }7 P- time_out_flag = millis();' k8 D; \$ M( s9 I2 B; `! s
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 G* A4 T5 s) m1 p; \6 E( d3 T/ U
- 7 {3 ^- [' b4 G8 [
- LOW_level_read_time = micros();
& ]) N% n$ G! D2 K" P- p - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
$ h: V, ?' A( a5 o: Q - {
/ A# R' S+ f! F- t0 A - return 0xff;
t: q" Y; ?. v3 Z - }
8 ?& h5 D+ k* o - 9 Y2 k( S! m( `' C0 H
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
' G7 e, c- H5 e9 s; E
9 R9 j( |& G0 t. e( E! B5 O. c- HIGH_level_read_time = micros();
H4 K/ d5 X6 E/ Y1 t; v) K0 c% J - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level& o0 O( O$ y! h
- f( O$ `2 p( ~ k% y( E
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out4 |4 {* S" V, j( q, |6 `1 ~% M6 d
- {
S$ C+ e8 t7 T; X2 }6 N - return 0xff;- F ], x0 {5 k4 t
- }
3 g' l6 g$ Q$ p. v' X4 H
+ `/ M9 J7 ?( \8 @+ y" b- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
. r" B. @$ _* H1 F! e - {6 C" x# ~( T! f% S
- return 0xff;8 l. W s2 g! Y! p1 F% C
- }: U1 x* x0 W, y! A
" C' Q) M, \5 P) U W& A- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );/ k+ [" F6 |8 w; I
- LOW_level_read_time = micros();3 v+ p% r; ?. w s$ G2 V+ S& X
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level2 R5 l1 F" j1 C' U1 O
- 9 R% ~+ e) `+ O
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 W0 T9 h9 ?5 s" j* ^ - {3 [0 o; m. q. s Q7 _( @; L
- return 0xff;
/ P C& L! P- ^" X% |$ I - }& f3 W: p+ n5 \
- 6 X# q0 n. S) ]# r! R; s( O
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
# J* E, c# {/ G4 G. y0 s - {
$ w( Z2 `0 L1 X1 [5 r - return 0xff;4 L% T' a. \8 [/ ]) [! T2 ^; n- H
- }
- p0 d' G/ B" q- E/ e: X& }/ l - 6 G' u! a4 J% Y" r- U
- for(uint8_t k=0; k<3; k++)
( J& G7 L; W9 f" [9 D* S. Y1 l - {
" L, e9 g/ t9 _8 H. }/ d2 d- D. B1 R - Sensor_Data[k] = 0x00;" y7 X0 @, y, }
- ! z" E& x4 t& O5 V" P# p6 n& n; d
- for(uint8_t i=0;i<8;i++); Q( w* V5 p" \# s2 W+ ^
- {
9 e8 z. W( ^: `% }# J. \ - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level% d2 o% T3 ]$ Q6 x
- HIGH_level_read_time = micros();0 A, T7 U0 }: l, A, T4 s8 l2 p
- LOW_level_read_time = micros() - LOW_level_read_time;/ z% p" h6 a* M5 f9 c: U
- 1 k' ^1 n% R1 F% `7 k% {
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )! r; b1 R5 c" ?0 `$ m3 O
- {0 q, N/ e7 G/ u3 n
- return 0xff;# T1 X) _# L- z
- }
5 x8 j9 c) o3 t6 o2 C
1 l* H5 o! l9 D+ F- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
& K7 n/ X' }5 b. n% }& c) I4 H - LOW_level_read_time = micros();7 w, a8 W) R2 i z6 g
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
; _' E6 Q6 M @( g: i- ^$ A
. L- B# T0 }0 n- K+ u! D7 e- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
8 `0 F r) I. i - {
4 N9 l+ G# I4 s - Sensor_Data[k] |= (0x80 >> i);
X2 @( o% ^- V6 Z/ g( C4 Y - }
# z- H- j! r0 n ?! q" I, T. W - else if(HIGH_level_read_time >= 100)
, Y& N5 `% ?& ?, O% P1 X - {
& @$ x$ M2 `9 h# B& Q- d - return 0xff;
4 w7 a8 I0 G: C - }
9 C5 i0 m/ @/ | - 3 z u# C: y7 [+ G* I) k
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
. Y8 I! W2 E X( j! V, K - {6 v1 H7 a: x- B
- return 0xff;
% T! S0 e' ?% L9 y: c' F v- v - }
' {2 m9 D; [3 a2 X" o1 }5 @9 R% j" N - }
( D" R- ~+ `! B+ N - }
7 j+ N8 {8 K8 c& q- X4 S, _ x
) C0 l# \* m( B# \* i- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
9 e& W) E% z. @+ s - HIGH_level_read_time = micros();% p) u/ F, X: T
- LOW_level_read_time = micros() - LOW_level_read_time;6 h8 ? x* o( j* V
- : ]3 `4 q/ h/ K% d% t, c
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )- H+ N2 H6 `9 X& S5 H& ]
- {
; A O6 `, F2 P! [% D0 y) E - return 0xff;
1 d6 @, B+ y' v" b1 T - }( C' D& J& p6 f9 `( e
- 3 C! v. Q- z1 R, G8 k0 s
- pinMode(_DataPin, OUTPUT);
1 X* c v3 ~% l, N - digitalWrite(_DataPin, HIGH);8 @: q: S, S: W# m& s
- ~: H, d6 ^) ~+ b" u' y3 G
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
: \1 s8 s5 N% I) k5 j$ M - {
/ G) r1 e' }0 m7 }/ ^3 I - old_data = Sensor_Data[0];
) a8 m U( N# {3 M8 d# }4 m% ?* T - return Sensor_Data[0];- F* Y# W8 ?% z+ c
- }7 l& ~' N, E$ q; s# A0 l
- else
* b0 f$ r. Z) a5 ]& O S - {2 F2 V/ L a1 L6 Q. |: r3 X0 ?$ y
- return old_data;
( m9 _# w, z6 w! R' @4 {9 k - }
6 H- L9 [% P6 a/ N2 r - }
+ z7 h" x7 f& D1 B; b7 P3 T
複製代碼
9 e) }" s0 o) w0 MMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
/ t, s" p: R" D/ D# f0 q( [+ d! k+ E% b, N# u M+ G
|
|