|
|
+ _$ Q3 y. ` d4 r' E: m
- #include <Arduino.h>& `9 l" c5 [6 G. F! k4 L2 r! Z; H! T
- #include <MeAuriga.h>
$ F* D! c$ `: w! j @$ k - #include "MeLineFollowerArray.h") s3 I4 o7 U! m7 w
- $ L% J! J) w S' O: C. O
- MeEncoderOnBoard Encoder_1(SLOT1);
n$ Q3 Q( m. Q$ N. h6 r$ { - MeEncoderOnBoard Encoder_2(SLOT2);
& Z, o) T1 m0 G, U - MeLightSensor lightsensor_1(12);6 s3 }8 C9 k9 Z" M h; A
- MeLightSensor lightsensor_2(11);# }( z, Y- i; n r. m
- MeBuzzer buzzer;3 `8 |3 L# G E' F# j6 O; M; L
- MeLineFollowerArray linefollower(PORT_6);% m2 u! o: ?1 Q4 J( T0 m
5 n. `5 W+ \2 d+ t& n- #define Error1 1
" I5 y' }8 F1 D+ d- T - #define Error2 2
/ Y8 I5 K( |3 V2 n* n9 y- f - #define Error3 3
3 @) B) V5 L7 N; ]3 J - 2 I" E4 h6 |9 T# L4 Q4 U: s
- #define Kp 15
' O- {5 p0 N3 W8 |3 O3 D. G( r - #define Ki 0.15
3 U2 k) `2 y& A: U - #define Kd 0.03
- b1 [2 j5 M* y* v% q9 q( O - * A" a8 e5 T) p& T3 s
- uint8_t sData;7 y1 l' A2 ]3 ^
- uint8_t D1;
" T; K% c- {: v, ]* A - uint8_t D2;/ G: ~, Q/ x$ K4 E n$ b9 n0 ^
- uint8_t D3;2 c& w' U- B6 Q! e
- uint8_t D4;4 V: r# g0 X# g# @4 A5 v6 Y1 \
- uint8_t D5;4 K( z, U: F# ]$ I# z
- uint8_t D6;) R3 T. U8 m/ p3 | E# k
; b1 Y6 q( {; ?! L- float previous_error = 0;0 v2 \! \( Y/ O5 O1 \
- float integral = 0;
3 g2 y5 C6 u+ A! y7 O) s - float derivative = 0;
4 i; g$ M# s2 L7 g3 ]1 ] - int Speed = 160;( y" j6 F0 e0 d! M- h
- float output;: Z% |8 W' l8 `) q9 S! [. l- {
% @9 K- Q# w" ]9 o0 n- byte Left;
5 a- T6 Z0 n2 N! | A
& q2 ~; ]2 X5 x- void setup()
9 }% ^5 ~6 b4 H' o; ~+ |7 A - {9 t9 {& A1 y" V; |7 g1 H" f7 Q
- //Set PWM 8KHz/ q1 {9 [3 u& S0 k# y
- TCCR1A = _BV(WGM10);
% N4 Y$ B% E1 I - TCCR1B = _BV(CS11) | _BV(WGM12);
) q: g) p! F# D& u* R+ y _9 L6 z - TCCR2A = _BV(WGM21) | _BV(WGM20);7 R/ o/ B j ~
- TCCR2B = _BV(CS21);
- D; J& K" u; V0 O0 I% G1 U - Serial.begin(9600);
# D8 L' o% R7 `# c - buzzer.setpin(45);9 _; T/ f% e5 r6 E5 g: e) a1 G8 w, U* g
- }
# }. h# F# @. J
) {- x1 Z" S+ K0 \: s2 z, _- void loop()+ G/ t' e1 Z C3 `
- {
8 I8 p* T" A5 l" x5 h$ l9 M - while(!((lightsensor_1.read()) < (10)));
1 D7 r; X* M0 ^/ \$ N) R - buzzer.tone(1047, 500);; E, ], r) D ^* P3 G: k: B
- delay(500);
8 L& [6 b+ e. U$ M$ P - do
5 L; z0 ]0 k: |& Z0 ^+ L - {
. C# N6 p/ P" A! R% V( D* M+ P - int Err = getErr();
# V4 F% X. i; l9 k - if(D1 == 1)
! F; B- \, C$ ]( z - {
) g9 Z- j. i- `; C8 m - Left = 1;. h% S3 r c6 H, T7 _- s& W9 }
- }
! P& \4 d4 o+ z% d0 u4 o" N - if(D6 == 1)3 G+ c6 ]# f7 d3 j5 p& R
- {
4 T* V A9 X. E5 e: G/ O2 _ - Left = 0;; C2 H; n1 E8 Q( o& S
- }
. M; U$ T) U# _2 \" s0 v% L e7 K0 s - if(Err == 99)
o3 e5 s6 v1 P+ o% S- S0 n* n& p - {$ I& q {% h; s' M! w2 ^
- if(Left == 1)
) ]; ~ Z' T" x x. H4 @" X/ Y& J - {3 Z+ n$ `/ M: @2 c2 q9 d) I% M
- Speed -= 5;$ V* h$ ^& [" B( q# A9 z3 _0 z1 `
- moto(0,Speed);
* l3 N+ B# F0 P4 c; U2 D - do
: N& U; q6 z, F: { t - {
6 [6 T" X% b" J# o0 J - Err = getErr();6 Z$ }6 D& [: k- W7 Y8 b p: K5 }9 M& L
- }while((D1+D6) == 0);( k+ e* }! R9 b5 K- A3 D
- }
$ k# U. N1 _' _; \/ l6 N l. S - else
# d2 f: ~, U |% ~" D2 g4 e. [ - {( E5 G6 U0 y; O7 {
- Speed -= 5;7 Z! q, n, r7 U$ i3 L
- moto(Speed,0);
1 x" ]7 l5 X$ w7 ]5 ~% P - do0 ~- w3 q* H2 C. R& L
- {
% M- X2 b0 v+ _6 Z$ B0 A2 F. J - Err = getErr();0 j$ p a! h$ }# @+ K* W
- }while((D1+D6) == 0);
& x8 `# J( u7 ~( M& y4 ~* Q - }7 P/ t) |' d: Z7 z1 h
- }) ~/ l# x% d2 j; L# C1 G; H
- else# f8 m5 J+ e" g
- {
3 \8 D3 k- A: d! a - if((Speed < 160) & (Err < 2)) Speed+=1; . V* z/ k, ]$ ]( | p+ K5 }
- if((Speed > 100) & (Err > 2)) Speed-=2; ! O- _$ b3 K M9 P
- integral = integral + Err;$ |5 g' P) m3 K! K
- derivative = Err - previous_error;
( F4 j. n6 c4 f - output = Kp*Err + Ki*integral + Kd*derivative;1 j5 P% T/ ~) W# v! G+ L
- moto(int(Speed-output),int(Speed+output)); `' X4 ?4 S& l& g1 t! J
- previous_error = Err;
6 z/ m. u4 M, M y - }. T5 E" f8 X0 |" p( s3 D' r
- }while(!((lightsensor_2.read()) < (10))); }! o4 B$ S |0 b) P5 e
- moto(0,0);
8 j$ H- `8 ]7 M# i - delay(500);
& C6 H& C; Q% p3 O. ~$ M$ I W+ _ - buzzer.tone(262, 500);3 Q/ J& I/ K' H
- }' f0 b& R; O' q' C8 y
- 6 l2 M Y1 l. c
- int getErr()5 k. z9 m& f0 E2 y4 r7 O T
- { . D( ]9 G+ d3 x
- sData = linefollower.getValue();! }. P- e) \" ]2 ~: ~6 f2 Z
- D1 = ~(sData>>0)&1;1 T% K q' u% G+ @9 U% j
- D2 = ~(sData>>1)&1;
& B- n+ R3 _7 B" y2 x0 R - D3 = ~(sData>>2)&1;
! {) H) s% ?- S3 w2 c& g - D4 = ~(sData>>3)&1;
7 _5 h; n' i* i# C, R& S) {5 X - D5 = ~(sData>>4)&1;# W* `! ~; r. N9 @
- D6 = ~(sData>>5)&1;
( }) s' N! @' `% h3 l" R- n - int downD = D1+D2+D3+D4+D5+D6;5 b* Y! Z6 u o
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);7 C" b( h( ~5 F& V, y) @/ p0 Y
- if(downD == 0)
9 g4 ?) v9 Q( m0 g: N# F; P: h# V6 ] - {" Y9 q& U* a% ]$ M) |5 q9 l, D
- return 99;
! F8 e3 k1 Y) D - }
/ y: W% M. P Z6 T4 Y - else
9 Y' Q2 T( o7 s% W) o% f8 H - {
8 P; b& ? B; q, j1 D - return upD/downD;
) D/ j# n0 Y: f7 N+ [7 N4 X - }- o* h& X& b0 r/ H3 |9 i
- }
! f; {3 A3 X2 v, Z7 B- X1 ? - * G% s: }+ A9 h$ L& L+ ]
- void moto(int Speed_L,int Speed_R)) x7 f! M: F3 h5 a/ t# k' R4 N
- {
% W" x! u6 U# U% Q0 S& @ - Encoder_2.setMotorPwm(Speed_L);
' J1 ]9 @% F- m: I% J' j- _5 L: h - Encoder_1.setMotorPwm(-Speed_R);
! T4 a; V9 X( O% l4 Y/ x - }
複製代碼
( {* G c* R5 c6 p; kMeLineFollowerArray.cpp
, H M, u" w% G- #include "MeLineFollowerArray.h"
& l+ @) k( b7 d9 `+ s# i' c* S
& l. Y1 v# I3 o0 @& B9 [, K. H- #ifdef ME_PORT_DEFINED
6 A% Z9 w% h g9 N - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)( d+ E; a- e% }5 {- `: w* {+ R6 u: E
- {7 y l$ Z+ }8 i' O
- . s0 X" q/ ^5 I- e
- }
3 `7 q: T( L+ o& G3 `/ K2 g - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
& [; ]! m2 w; Z* b) l: A( @+ b - {
/ m q; K3 o* P - _DataPin = mePort[port].s2;
1 \# v: O6 t5 N% a6 x - pinMode(_DataPin, OUTPUT); 8 ^. W/ C1 d- M; T/ h' `# q
- digitalWrite(_DataPin, HIGH);4 [( Q5 `+ E c" ~- T
- }; |. ?4 X- ?+ x' j6 M$ |
- #else // ME_PORT_DEFINED6 b+ l h9 v# K+ \6 n# r
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)7 _% E4 a/ L% s4 K& N$ g/ H& C& v
- {% O2 n5 k! F- I5 j8 Y% U( w8 I
- _DataPin = pin;; U7 A8 i% |2 B$ ^: V" }; P2 C& E8 _+ _
- pinMode(_DataPin, OUTPUT);
$ Y. L; }. M4 W6 Q3 y! K' | - digitalWrite(_DataPin, HIGH);
5 G4 O4 q |* o - }
. R, Y8 N/ [ K! S, h8 X$ Z - #endif // ME_PORT_DEFINED$ V5 Z4 T$ }; ^0 I- h2 A
8 z2 L. h, t' s2 ]4 |' x5 K
( P" O: ?/ |0 {- void MeLineFollowerArray::setpin(uint8_t pin)! b b9 w5 ]4 Q7 Y+ T# E5 a$ o' q9 a
- {$ u& Y3 }5 A7 Y; K
- _DataPin = pin;
% ]: f9 |; M+ z7 R m$ \! g - pinMode(_DataPin, OUTPUT); 2 c$ h1 t$ ~# I
- digitalWrite(_DataPin, HIGH);; ?" @) O! P7 C- g
. [; U# V0 O. Y- #ifdef ME_PORT_DEFINED! ~+ c' \& k8 e4 h3 b( @
- s2 = pin;
$ A7 ] `/ o) y1 C& {% Q - #endif
- S2 B9 v/ X5 i( t& x; ] - }* v, g" ?; `. w5 f
- 1 X+ V$ k; v# c9 O8 b! |( d
- uint8_t MeLineFollowerArray::getValue()
& r* V1 A( p* C0 U/ z& ^/ D - {5 X, O: E8 [: F- }4 |/ `
- uint32_t LOW_level_read_time;9 c) y. G0 f* ?6 X
- uint32_t HIGH_level_read_time;4 M" f- l1 D, K% N
- uint32_t time_out_flag;5 h) }# t" A p
- uint8_t Sensor_Data[3];5 \- e; k2 p" S" T5 _
- static uint8_t old_data = 0xff;. a( q7 D- {* {( D/ K# A
- 4 }! k+ i% M$ v1 m0 [
- pinMode(_DataPin, OUTPUT);
+ |4 f9 [ Q) M: d; Y3 e - digitalWrite(_DataPin, LOW);) V7 C; E$ j$ Y, {" \
- delayMicroseconds(980);
+ C0 X2 [; Q+ u - digitalWrite(_DataPin, HIGH);
% V0 v l8 q% n! g/ t5 u
' ?7 u% E2 R& ]* d$ L, H: j- pinMode(_DataPin, INPUT_PULLUP);* n+ m" I7 S, R; _" S0 P: l$ M
- delayMicroseconds(10);
( S9 V# ^9 R+ d6 v
7 U5 Z5 O- \( u& n+ t* p! O- U- time_out_flag = millis();% M+ X& ]5 N, u* R& ^% ]
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; K% B1 |* e/ W7 H5 \
+ ?6 b9 X4 d( |9 x7 T/ j- LOW_level_read_time = micros();$ O: V2 v8 o+ V" T8 S+ E, v
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
) C8 k0 a* H2 D; @: a! a/ z - {
0 J3 k# r9 X1 A9 b! M6 S% K; Y - return 0xff;0 S, n( D: A4 ]5 {; d) i( e1 F
- }) D6 Z% i2 Y5 W; y& i. \, k: o
7 ]% [! B1 g$ @5 P( O5 v. O- I- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; o* M. i d8 ] - * V; x2 g- H' N5 B
- HIGH_level_read_time = micros(); I( B9 K9 v, o, p0 K
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
5 j% U( i: |# x9 l) x7 d - 6 _# X1 A7 y7 c2 S8 o5 \
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
, v* j# X& |5 }5 l+ m4 v/ o+ A' @ - {
# Y2 D }- s# c! O, {! z - return 0xff;4 ~ r% E: B- A, n( g+ I
- }' a, t$ e! d4 y2 q
- 8 W9 }; K+ d. N; U- @$ A4 g
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))( z8 Z! j$ r% v! l( r
- {7 l4 z! s& ?* W/ a* |
- return 0xff;
2 e% X6 j4 U' z - }. ?) l& U9 d6 Y8 w9 H
& r, K+ B2 x" ~! e5 _8 j- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
T. g! j2 j# d3 J3 v2 e. n - LOW_level_read_time = micros();% T6 Y! _) F# k% u
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
3 R! ~( B. w$ W6 @
2 L: r' o' Z8 s6 v/ C1 p8 S- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
8 W$ Q6 n( e! |6 v - {
; E& d6 V- J) f$ R - return 0xff;
0 u$ a1 G9 \# z4 Y3 A - }
( @. g" x7 s7 u1 l& W. t - # ?) _$ |5 v9 M [. r! }
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))7 j/ {2 N9 _/ u$ ]7 T" l: C& R% R
- {* g( y' C U4 J7 b* O, \
- return 0xff;
& q9 f7 {$ I7 h8 _ - }3 F# M2 h+ h1 I0 {# L( n
- " h4 t R% i3 o5 y
- for(uint8_t k=0; k<3; k++)" d, X" v) B0 u3 c$ F2 Q
- {
* S6 X! N8 w! M# O) x* p" M - Sensor_Data[k] = 0x00;
0 u% z) p4 W y8 p - 1 V) Y9 ?7 t/ a' {$ G
- for(uint8_t i=0;i<8;i++)8 i* E4 `+ `& r" l
- {
# m, }" r. c8 P - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level/ [! S+ u) ?/ A- K: K, {
- HIGH_level_read_time = micros();' G8 F: U1 [+ I3 n" Y+ D. X7 X
- LOW_level_read_time = micros() - LOW_level_read_time;
$ B) w) {: m8 @) F: z: n
( o: F7 S8 g* @- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )" f! {: t8 w) ~8 j
- {; W% ]3 h/ h7 @
- return 0xff;6 V l* H8 U" N- X5 d
- }% D. i2 x# m, W
- " I6 F" Z; N. K
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 U0 `5 S+ n" |. r- V
- LOW_level_read_time = micros();. f' I8 S- z4 O1 `2 h' {( Q
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
# V% Y6 L7 o w9 M$ M' U, F+ H3 m - & f/ }: l' U; O/ k8 v; D: h, R+ |( C
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
) @$ n8 j: d, n& a& N - {1 U1 [3 t0 _3 l8 a' l/ `7 t0 k7 [
- Sensor_Data[k] |= (0x80 >> i);
8 |% j7 X3 T) b8 c% c - }
- I' A4 M7 x4 _. j5 ~8 A - else if(HIGH_level_read_time >= 100)
: X( d7 w# O v - {8 ]$ k- u) |: B$ |' {+ `$ K
- return 0xff;
- O1 A) Z; N1 }1 Q4 @, S' x - }
1 k2 [/ W6 o% z& f4 q - ?5 ?1 `0 ^, f) C" v w0 k) n
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
# k7 ^2 r2 M6 A2 q) a - {
) m; N; S q) n0 G) w4 Q* h, } - return 0xff;
/ Z( R: a: k$ p% Y - }# G, O/ A: p( k9 X
- }- P" S$ r7 X/ e
- }
8 U$ w. w6 q, t p6 o% m
* h: z" _1 J' B+ A0 z- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level+ S D2 A; g5 b
- HIGH_level_read_time = micros();
, D7 d1 R+ _4 | - LOW_level_read_time = micros() - LOW_level_read_time;1 z1 G) P/ ?- F9 j* n
1 M1 O/ _4 g; G5 C. `( o- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ). t& n/ Z) x. @; i: ]+ @) P2 z
- {
0 f; K, U8 g' D6 \ S - return 0xff;
$ @5 u6 `1 O3 W9 L - }
. N1 p8 Q0 d. v$ K7 V2 }& b
6 j! ?% a2 E5 P5 y4 M- pinMode(_DataPin, OUTPUT);2 j3 W% I! c( U4 S. z' o* C
- digitalWrite(_DataPin, HIGH);
# T: v) {* g) q( n1 U - ) y8 @) B& e3 n8 K. q3 Z
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))5 [; w3 B' B& B3 Z. ^3 d
- {
}. I. R O) f2 ?2 |$ t - old_data = Sensor_Data[0];
" ], A: P- {8 k - return Sensor_Data[0];
a; X/ o6 F7 z8 D7 Y. Y3 V. ? - }
0 m0 y4 L) \, s( ]( Q3 b9 [% _) l - else- u4 {) t1 ?1 b) R" l
- {# a' N5 y# \5 |5 k: A
- return old_data;/ N! [- k4 h# E6 n
- }( {" g% Q3 q! z3 R3 w
- }
: o- @0 X) n, h, d
複製代碼 5 ]: I$ Z1 c6 D9 j) e6 E% o6 W1 r
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
h' l, }( h, }
- g: ^9 n9 d0 T; o# e
|
|