|
|
]5 r4 U J: v% l" |, p- i7 t) D- #include <Arduino.h>- z2 S( \$ A3 F( \4 t7 J
- #include <MeAuriga.h>) Z! L3 H" H( z
- #include "MeLineFollowerArray.h"
4 j7 C$ G* M+ h1 i9 h/ Q, x
$ r7 q1 u2 w3 m5 Y0 F* L- X- MeEncoderOnBoard Encoder_1(SLOT1);- D6 ]0 s, M5 _9 R, |0 G" P% n
- MeEncoderOnBoard Encoder_2(SLOT2);" P# I. N) Y0 x$ C2 R) ]+ j
- MeLightSensor lightsensor_1(12);
/ R' t$ F# E, h$ a+ ]- _ - MeLightSensor lightsensor_2(11);
, ^( H2 ?; M: U# h) N/ K, Z) w - MeBuzzer buzzer;
$ @3 y- V/ S( V* N. r" c- i - MeLineFollowerArray linefollower(PORT_6);
0 S! y' d. A: p o# M% g, K - ! E( x8 D; o1 u4 L$ U, h
- #define Error1 1
3 H, v% y5 _, q4 W - #define Error2 2
9 s# |7 t3 G! @" V4 Y& O3 k - #define Error3 3. Z- g# i+ B5 m- @) E
0 Q. Z0 o# D+ c* g$ h- #define Kp 15
v2 x0 t) f" Y# q. Z9 j) f - #define Ki 0.15. Y/ k' q; w# K2 n* t1 ?1 E
- #define Kd 0.03$ S+ o% N% G$ B' T
- - O; }" v/ K9 W q2 |- }" k
- uint8_t sData;
& E, z* m; P1 P6 `: d8 K - uint8_t D1;
! t$ V4 F; M! U' Z& x, X - uint8_t D2;5 q5 }! U6 {: l3 e7 V Y
- uint8_t D3;
) a& b T% @: Q& s, P5 r - uint8_t D4;6 \( f$ Z7 L- I) y
- uint8_t D5;
$ s" s" R5 C0 P' c% ] - uint8_t D6;& I1 G9 @7 M. H, i- C0 w
- 4 J) h- s0 R$ `6 d& v
- float previous_error = 0;% r0 k% l6 q# W4 [& @" f* P2 U: J
- float integral = 0;' m- W" M& \' ?6 h7 @; ?: w
- float derivative = 0;
4 @1 a' _" {1 l4 c - int Speed = 160;
/ u5 A2 Q& W6 Y; }* r$ U - float output;7 c) a; h/ S2 q* N. ^; m9 p' g
- }' E! v( q N3 d# t* R% r
- byte Left;
( J# R, J }7 F - + c. r: f* i- B# S( f/ N% v" D, {
- void setup() `2 m& c" D: n- K* `
- {# A" x, o5 W# `7 b9 ~
- //Set PWM 8KHz+ N+ c) S7 R/ O% o
- TCCR1A = _BV(WGM10);
6 P* W& D7 y1 |5 p( d Y0 l" K - TCCR1B = _BV(CS11) | _BV(WGM12);8 {. A% d" m4 O
- TCCR2A = _BV(WGM21) | _BV(WGM20);
7 Q& N6 m" R+ |6 m5 E - TCCR2B = _BV(CS21);; G M. ~1 r: D6 p- j
- Serial.begin(9600);
4 L% S% b+ v$ m4 z ~6 Z - buzzer.setpin(45);
0 ~/ A( n: F5 c+ V4 c - }; Z" F( D2 x0 V
- : }* b4 V$ k. Y% m$ ~+ G0 r
- void loop()
$ f8 J4 W9 \2 w2 C- g0 T: m1 l' u - {
3 o4 g; ?# D# M) s: ]7 | o - while(!((lightsensor_1.read()) < (10)));
. t6 x% ?5 k5 s5 h! o1 \ - buzzer.tone(1047, 500);
* u: G( L% G( B& j6 G# T s& s {3 F - delay(500);6 a: z7 a3 w( O/ t
- do+ R$ N J8 i" D" j
- {
^% R R6 P+ @% R - int Err = getErr();
" y* {& d6 [5 k- v. O - if(D1 == 1)6 p* Z# S8 R9 u) V8 \$ H' z
- {
& ~% H% J. _/ v* |; i1 L9 B: O - Left = 1;2 q7 I: A3 c4 s$ I1 w
- }
2 d- K, H: a, X - if(D6 == 1)
$ d. T! X ^- A; I0 s - {/ D# W% |! F9 c% V }- m
- Left = 0;
( A- o: \3 h9 m' ~# h - }
5 m' n4 ?6 Y8 @# e h: Z - if(Err == 99)
' g% r; v" G/ } - { @* b% @0 w8 \; D
- if(Left == 1)
- t0 c% i D+ g* j - {
1 F+ A( P& ?. U v - Speed -= 5; R' H3 k' t6 c2 a1 T
- moto(0,Speed);
/ r: \2 V+ I. W' |6 S; k - do
1 @* t6 J! y" Q# N. Z5 h. B - {2 Q; c2 i0 X# E! w; u: `
- Err = getErr();, y( D5 f. i. F C4 m
- }while((D1+D6) == 0);
" V5 L; U+ Q" r - }
5 k" |3 u0 H, z. D6 f, W0 _ - else
& r$ O+ o% c: s+ x4 D' |% | - {- B: b* F+ @4 t6 H0 Y, o# L
- Speed -= 5;' h8 m- c5 J7 m" Q' k7 x
- moto(Speed,0); 5 l2 x8 n7 n2 B9 x9 Y- W
- do
% E% {7 x3 t% o" X* [/ }2 ?; ^! X J# L! d - {" t0 q& `# l0 Y3 q# w8 H; |
- Err = getErr();0 @9 b" s- L: Z' h N
- }while((D1+D6) == 0);
+ H7 T, ?* u6 ] F- G- g y1 u - }
/ Y5 A+ `* `6 Z% Z3 a2 V - }
% ^, k4 @4 U# }* g5 ]. p - else
2 y( ?7 L3 i5 U6 O+ Y& D1 } - {
$ [" \1 K9 {. ]% V" R, f' E - if((Speed < 160) & (Err < 2)) Speed+=1; / x9 I0 Q% p' Y6 ^6 X" H0 v& j
- if((Speed > 100) & (Err > 2)) Speed-=2;
& h2 w+ J% |6 c2 u - integral = integral + Err;
3 n, M& y' c8 C/ r- v; o! W - derivative = Err - previous_error;3 }! S- t+ y: S: e
- output = Kp*Err + Ki*integral + Kd*derivative;5 b- r8 S" Q3 B2 C
- moto(int(Speed-output),int(Speed+output));" x. B+ b2 J. f6 C; `0 _
- previous_error = Err;$ d% k6 B8 ^: |$ A. q9 U+ i D3 v2 v
- }& M3 E3 O8 p) }8 q
- }while(!((lightsensor_2.read()) < (10)));
/ C: T* i- N; |& t* ] - moto(0,0);. g5 X7 W- K) W% y ?
- delay(500);
z) U, m7 y2 O* [ - buzzer.tone(262, 500);
3 d O+ l/ P j( i& s6 n& i5 o - }
9 N# x3 Y9 |) R: k/ F$ G - , d7 B7 M+ K) d$ V8 y
- int getErr()
( w" {/ }. \; Z! N) B2 x* q - { 0 D4 E% t7 N) Y9 ~9 L5 _1 D
- sData = linefollower.getValue();6 n7 G; f) K6 H' u7 ~" {0 Q# }
- D1 = ~(sData>>0)&1;
$ i% _) m2 p2 k! f/ Q" T - D2 = ~(sData>>1)&1;& R' n. l$ _9 A) W
- D3 = ~(sData>>2)&1;
2 b: K- e4 L/ o8 ]/ j* X - D4 = ~(sData>>3)&1;
; `" R) f- q( m; @5 f% S - D5 = ~(sData>>4)&1;: b, n) c3 |; v$ d3 ]
- D6 = ~(sData>>5)&1;
( ~1 Y! a- n+ n8 _* c8 f" C - int downD = D1+D2+D3+D4+D5+D6;- ~8 Z) o( F# y' t
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);4 a" m1 c9 p' `* P- o. w
- if(downD == 0)
* F- p1 O) d0 z" W+ q! X! l q - {
. C5 u) Z. |) v: A3 D, x - return 99;& x/ a8 \2 U0 t; `) f" T1 A
- }
( j. m# A. U( f - else
8 a; ]- B g# _4 Q - {
2 Z3 O+ \0 C g8 p4 x2 _% s, x6 { - return upD/downD;
4 X# G# I1 E2 c2 k - }- ^2 Q) E. U7 e4 b
- }* H7 S0 A" A9 y3 E: }4 J1 l
- 5 D( a( u7 Z2 m; _
- void moto(int Speed_L,int Speed_R)
7 }9 _/ p& U; y - {! E; e( `$ i: V2 _7 z t
- Encoder_2.setMotorPwm(Speed_L);9 j' S; t( C( A+ n5 o$ ^* c* k% x
- Encoder_1.setMotorPwm(-Speed_R);
( I" T& ?. S* U, O. d& ~! J - }
複製代碼
J4 s) \# K7 s9 GMeLineFollowerArray.cpp
+ M$ l2 |( G" M# g' g- #include "MeLineFollowerArray.h". l, b9 ], G. R& T% {: v
- : {+ U" o) [/ v
- #ifdef ME_PORT_DEFINED
0 s' D# Q+ s- v - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)" P, |, X5 V5 d$ h E
- {( D" _, B5 q8 w. a3 G2 K
- B* t! Y% c1 r4 V! F- e" P
- } `. F0 w) d. \% m" U
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
4 ?, @& c6 w% T( U3 ?5 z6 e+ h9 r: {' \$ t2 b - {$ S3 x: r+ A( }& R
- _DataPin = mePort[port].s2;
( S- e5 z% }( t - pinMode(_DataPin, OUTPUT); & u1 }3 A5 {+ W3 ` ?5 H+ ]' U* D
- digitalWrite(_DataPin, HIGH);
- m( M* f& D/ N& s - }
) |+ I9 L4 k# Q! C/ l0 C8 p - #else // ME_PORT_DEFINED% P+ s ^: d6 C# L
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
2 T# I4 `/ h9 K1 h/ Z - {
3 x3 k( d& B' T) q& \ - _DataPin = pin;4 b% _. E+ j" }: |% R4 f4 R
- pinMode(_DataPin, OUTPUT);
T- X' T2 w# }" M) s - digitalWrite(_DataPin, HIGH);
, v& n& r8 f d$ x - }
; O& Y7 {* y$ p8 N7 ^ - #endif // ME_PORT_DEFINED: @% g$ o' k) c
- 6 ~: ^) c+ G/ T
- ; p0 s$ J6 ~) H7 q) b
- void MeLineFollowerArray::setpin(uint8_t pin)
: M; ]6 J6 E5 g. ^3 X+ P% M+ } - {* a7 B! t8 S l5 Z& u8 N. J
- _DataPin = pin;2 O$ ?+ s6 m' B; g( S" B
- pinMode(_DataPin, OUTPUT);
; F# ?1 r$ f7 A0 B3 \+ J1 p - digitalWrite(_DataPin, HIGH);4 Q' x1 ~. y5 _* R
- & u8 R- ]! W; v: m7 f9 K
- #ifdef ME_PORT_DEFINED
$ t) d0 ~( l0 f9 @1 B& ^( N" F/ ~ - s2 = pin;
8 d# u2 n; {, k - #endif
: b/ W+ O; n* ^ B! v! ? - }( u% W9 K7 R D P
- . ~7 S3 S2 Z6 ]$ X$ M4 c: c! S1 e# g
- uint8_t MeLineFollowerArray::getValue()
0 t) c/ y$ f% E- e3 _ - {1 }0 J, y+ [: K5 G6 C- G" |
- uint32_t LOW_level_read_time;" X6 K0 x3 r) M; r& l$ J) B
- uint32_t HIGH_level_read_time;
8 H3 j. D' ^3 A) H+ g - uint32_t time_out_flag;0 X3 l& ]+ j$ E5 ~ g
- uint8_t Sensor_Data[3];
: G8 m1 c( _5 I' |8 ?! |1 U4 I - static uint8_t old_data = 0xff;
9 T, x3 e, r3 \# z: w& u9 r - . T1 v' r3 \; u1 h' o" M7 l8 B, o
- pinMode(_DataPin, OUTPUT);6 r* o& [0 X0 X B4 _
- digitalWrite(_DataPin, LOW);
W) _# g. N. W7 G) @ - delayMicroseconds(980);
: E% g9 M% c6 x+ Y - digitalWrite(_DataPin, HIGH);% g* c& T/ W1 e- X$ b' n& c
$ m2 R( S: j0 I% p- pinMode(_DataPin, INPUT_PULLUP);& [7 p T2 a' d
- delayMicroseconds(10);
4 ?; L0 @" s& A$ t5 P* X' M) B/ V& k
+ t& M( s0 d3 H! t! T- time_out_flag = millis();$ j$ {/ Y, |2 a- ]: Y1 f
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );2 `% T1 |' h( a, N! Z7 b
- ' L1 V& n2 ?8 H
- LOW_level_read_time = micros();
1 x, k7 P4 S) ?; \& D - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out0 F7 X& E& g5 i* X/ q
- {, E% v$ [/ b! f
- return 0xff;
. v; p: I+ ?/ S - }
6 x: N. D5 j4 c+ C& F& r
5 P8 c: ~. V' k4 C5 b7 o- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 v: `5 W- \: K' o- g1 H - \3 b$ Y& G. j" \5 I! X8 X
- HIGH_level_read_time = micros(); K$ E: o; _) E* f
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level4 Q" Z" n, C \/ N
- # F" @1 e4 j8 f8 }3 Y
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 [, \9 `) I% ~0 b, B" {1 j/ I - {* K% h, [2 w9 {; z) |2 U
- return 0xff;
$ \' F, {0 ` W - }( l' R: Y0 G/ H+ w
! d( w# }/ k- E' B- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
E# B8 l4 q! k# [. e! \/ x - {% d, D3 g% _7 I4 }! R8 e% b
- return 0xff;
9 S q) H {& h0 I - }, l8 A5 K1 S# b; G( i
! b9 r; h' z6 L. N- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* t, ^+ }6 V' R% k# X4 n( L# r- E
- LOW_level_read_time = micros();* K& B( S0 H ^) b0 A( f) _
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level( U( i7 ^! A( n1 ]. x0 l
- " Q5 f- x* W" F
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out& g4 ^6 ~- J: z. l4 T( J& e
- {' b7 b* L$ `5 l' ?
- return 0xff;9 Q5 H( P5 S; {, L0 c3 @
- }- N x& e: t6 Y
- ' T( K! _8 q j
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))5 B# l$ p, ?6 @" b
- {
. S. G c# y9 J s, }4 E5 y - return 0xff;
/ `: v$ e& b% E# |1 Z0 a - }; j4 \1 O/ ~2 T2 R
- 6 x6 k0 g6 ~* G6 w! K+ M' L7 U
- for(uint8_t k=0; k<3; k++): F6 e9 E! Y* \% t; h0 L" Y; K9 b
- {
; A- _3 d @5 L" S3 p - Sensor_Data[k] = 0x00;
8 ~& ~/ U7 Y. A. w* j
. h6 o6 W, S7 [# k- for(uint8_t i=0;i<8;i++)
1 l+ `5 Z ~* {8 D0 w, d - { S: c& D; C# k) k2 j
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
# J4 h o x$ s8 c0 `. j* b - HIGH_level_read_time = micros();
# ?/ l: M& ^) F K0 F" | - LOW_level_read_time = micros() - LOW_level_read_time;
* C0 o' C7 Z: j& h. o
# k# ~9 \( ~7 m3 K7 u; _- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )" X0 K- A* N+ X& o) d& \, |4 I) H
- {" e8 n! J0 V1 M; r% Z
- return 0xff;4 C; F8 |: n Y
- }0 V k% ? Z# V6 R6 G2 n" a
4 I7 g: @& k* z- C6 i5 p- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 c: ~0 r* }# W' e1 m: G4 w - LOW_level_read_time = micros();
2 C/ \, S; O- v ~& f! F - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level' @6 ~: d0 E! E$ y+ ?9 b i
- 4 [: y- r, c# M
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1; D% m7 P; Z: h: u
- {
9 Y: ?3 f' | C: O, Q- D# M - Sensor_Data[k] |= (0x80 >> i);
8 E6 C# K$ c" L# f; { - }
) Q! ^$ [8 f7 H6 D* b) Y/ g - else if(HIGH_level_read_time >= 100)
, D8 M2 a1 p' X) s M4 v# F6 [ - {1 Q& U0 q9 j5 ]3 V
- return 0xff;5 ~. c" \& \3 R1 b M
- }
* t0 v0 G6 |6 ` f# _1 W! p - 0 X% W. v( o8 I' ^2 P* g0 m7 ^8 @" l
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out): V9 f# n2 W1 n: e
- {* I7 L+ ^$ ^+ ?5 @0 V
- return 0xff;
3 n/ e& V* f+ Y# n8 f - }
' f* I P8 U H! a3 p! f0 z - }
# _: F0 p" o, T+ l, Q0 P - }
, p1 a: r1 h4 u" W
q( e' ]5 F1 ]9 d6 l+ B- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
1 W+ {/ j7 g+ V. ?* t. S/ R0 f - HIGH_level_read_time = micros();( m: H9 k" U0 y: m& m! q5 b
- LOW_level_read_time = micros() - LOW_level_read_time;5 ]0 s [& i/ s" N$ }3 e
- 0 k* q9 k1 I- M5 Y% }/ k9 t
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
# F* h/ |3 U+ c - {
6 J! s2 n$ r9 X1 r$ U8 | - return 0xff;3 B8 M% n6 l0 w' @# ~0 S, c
- }$ a; W$ }& N- ?" q P" ~* u' C2 I
* M7 l' F7 Q% A7 C1 B3 T& f- pinMode(_DataPin, OUTPUT);: s0 H8 G( _6 O6 _
- digitalWrite(_DataPin, HIGH);0 e( K* n& m2 @5 V* `; u/ l. c) U
- " J' ~3 _) K/ i v
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
. `! {( ?6 _. K% g3 ]: s - {
( B4 N/ u( X% |2 z& g! L% ] - old_data = Sensor_Data[0];5 Y2 e u I7 c& F
- return Sensor_Data[0];
0 G6 q# y6 m0 l - }
" Q+ m! C& Z* \4 Z( w8 S - else
9 z6 R& }8 b h9 L, Z6 I - {
! W/ ?% _5 N/ E* ~; x) l5 c- k% }# I - return old_data;! t: s$ g6 P0 K3 j7 p. }- o% c9 |6 V
- }
" T% O2 |' u Q0 E4 n# {9 `: S - }
' T) k3 d4 b* v( m! O
複製代碼
, w3 I/ ?6 A s& w" D) G; M$ FMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
4 Z8 ]0 {3 u7 w1 L5 S. u; A
* c6 l$ H! r; |6 H, S2 r |
|