|
|
( U8 K9 K# k! z2 f, L' K- #include <Arduino.h>. J/ s% t1 h' r4 [& }' K4 v
- #include <MeAuriga.h>5 ?! @5 }- A! ~) ~5 p
- #include "MeLineFollowerArray.h"3 k$ M: b) X/ ?( [6 z
- : e2 e2 J H( D2 Q
- MeEncoderOnBoard Encoder_1(SLOT1);% @& t; X x) c9 @6 i( A+ q
- MeEncoderOnBoard Encoder_2(SLOT2);' E" |' Z8 m- G
- MeLightSensor lightsensor_1(12);% J4 ^& r+ b3 |, `
- MeLightSensor lightsensor_2(11);
8 {. F1 b3 a5 E! S- R - MeBuzzer buzzer;2 O n/ \+ S$ t, Z
- MeLineFollowerArray linefollower(PORT_6);0 \3 A6 f3 O- A! M. _/ f
: H$ m+ r! R- p2 t8 ~8 F. u- #define Error1 1
( d2 H) m$ k1 c: ~! S3 ~; I- | h3 v - #define Error2 2
$ L- f0 W% N3 W - #define Error3 3
. t6 r0 k. m5 K G - 1 d+ `( k3 R: D9 l/ \& r! E2 d3 U2 Z
- #define Kp 152 {7 @3 ?" s# g: a0 v, { a- `
- #define Ki 0.15# H: n5 x" l3 j' h
- #define Kd 0.03
5 v' h& O, Y0 F7 _ - 5 Y/ X- I2 R6 o" j
- uint8_t sData;
n' p0 x3 }- o3 T; { - uint8_t D1;( y, s, N: v# _# Y1 g& n5 W* B e
- uint8_t D2;1 r+ V3 }$ \- t6 ^4 ~- i
- uint8_t D3;! F0 E X+ O* F) v# ~. Z3 B' x' b
- uint8_t D4;0 ~ X) ^; _7 ~3 }6 @# H
- uint8_t D5;( _5 o# L; R. `1 n
- uint8_t D6;
5 X3 d) k& A; ? ?, j! Q
7 P# e5 u9 M9 ? c- float previous_error = 0;
; x: l' A( g' f9 A - float integral = 0;
2 X, g4 n6 ~; _+ H2 s/ q. y; v" w6 W - float derivative = 0;
" b3 U1 W+ |: i - int Speed = 160; ^0 p: m: L' h4 L
- float output;
7 M6 X& o: X3 ^* x* E8 X5 ~ - * p* v7 h; S- m8 x8 g! J8 ~/ M- x. K
- byte Left; }( c! ?- u6 |! r* D9 n
- % |9 d9 [) Y: n
- void setup()
0 S$ [0 R/ M1 a+ i - { u2 y5 {" g( d& N" ^
- //Set PWM 8KHz1 ^. |. B! o% Q: }8 L4 S6 h7 Z
- TCCR1A = _BV(WGM10);* U8 C6 x- ]3 e& i5 Q* L! Q
- TCCR1B = _BV(CS11) | _BV(WGM12);* w" q6 D+ m0 S* M% L
- TCCR2A = _BV(WGM21) | _BV(WGM20);
4 t+ D3 [$ t9 i$ c# n7 v - TCCR2B = _BV(CS21);9 y# ?# L0 P' Q+ K' X, \$ u) H
- Serial.begin(9600);
. }3 n! ~/ K0 H2 ^" o - buzzer.setpin(45);, m' `( u% o$ E; E% N D
- }) L$ R$ o- @6 _2 L. R5 L
3 q M* T$ X) q1 l- void loop()
1 _+ C- k% z( |$ u b - {
) P) q# _ f9 u - while(!((lightsensor_1.read()) < (10)));1 p: U0 _1 P+ P4 U+ B$ x1 ^ \
- buzzer.tone(1047, 500);
( N2 ~; L# w& N - delay(500);: f: J" `5 d3 d i& {; P! W
- do& G& s# J& r8 f$ i" o8 @9 U
- {
+ W6 A }1 q, e8 d - int Err = getErr();
8 B% V. e. c3 \9 K; b0 n& \; _ - if(D1 == 1)
0 Z, j3 d# S0 X5 E - {
: F0 E9 E* e+ `0 C& ^ - Left = 1;
. `% `8 @; x. y1 u- c+ c, h/ L% O - }4 c/ p: g. ~2 R$ U4 m
- if(D6 == 1)+ k% f. A- A* C B) e9 C* B
- {; {+ T; ]$ s( l$ }& @
- Left = 0;
( }6 m8 _5 h7 a7 O. G - }( a1 P6 D1 r* M. q8 ]" J
- if(Err == 99). L8 \( `% a* ]7 y& k7 f+ L% S
- {" V: L% m) S6 t
- if(Left == 1)
) b7 M( z' P5 V. F8 s8 |# y& M8 g - {
+ u( h- _! U8 ]1 E - Speed -= 5;/ s% \" r2 e H: U% u; Y* O
- moto(0,Speed);4 f) Z5 n. G- p& ?9 L; |
- do, ~$ N2 m0 @$ J) L% ?" @9 Z2 Y
- {9 `( q" L8 d/ W1 L
- Err = getErr();
' n* F$ A5 I- Q0 x- J - }while((D1+D6) == 0);
# D/ y4 ^0 {1 C1 f" ~. A( G - }
0 U7 r& ] b0 {( w3 P - else
6 J) H/ x. R( g - {
1 U& e5 P n! D. ]% y: u3 L% s, { - Speed -= 5;
3 m# J0 g6 ], w @7 S1 F8 j" a/ _6 P - moto(Speed,0);
1 b5 O$ C( k# Y( M - do* {+ Z$ C p# w$ D X
- {) n4 ?) E2 g2 }* e0 B$ C5 j- K9 q
- Err = getErr();4 _$ Q$ S# _! C F8 ^
- }while((D1+D6) == 0);0 J3 T+ b" N9 H, }# }0 N1 y6 U& n
- }7 K! _ X0 j' q6 c. A( N5 x v
- }1 h" r p' h; s$ Z3 P% X' r
- else3 F8 S l7 j& S) P V
- {8 J+ }. C1 g$ l( N
- if((Speed < 160) & (Err < 2)) Speed+=1;
3 V4 h9 r, S0 U+ h7 Q - if((Speed > 100) & (Err > 2)) Speed-=2;
1 f7 @7 x& |/ H: l" t - integral = integral + Err;1 c- Y% G6 d5 u# Z
- derivative = Err - previous_error;4 V" Z6 B0 N- E6 ~
- output = Kp*Err + Ki*integral + Kd*derivative;
' y( [* q% P* k, W - moto(int(Speed-output),int(Speed+output));+ m' b$ ~( g% h1 ^! B0 [( s
- previous_error = Err; e- {1 N1 `3 y& i; U. b& D
- }
6 k G3 n. U0 r - }while(!((lightsensor_2.read()) < (10)));
. y* z+ B3 E& ]8 p( G" f: } - moto(0,0);( G* o1 h+ q' U G
- delay(500);) K& J5 w: y1 W, k& {' v2 S
- buzzer.tone(262, 500);- e }! i) S& ~3 B, H' v1 ]
- }
. H" o% s/ \6 p$ R: i - 9 z* h! p8 \; N& \, G/ q% H9 l& u
- int getErr(). S+ P8 X- {, L
- {
) y$ h2 {3 E7 I* X" F$ e - sData = linefollower.getValue(); Z* C, M& o5 d" I1 t
- D1 = ~(sData>>0)&1;: g) Q- D% I3 X7 b+ Y }
- D2 = ~(sData>>1)&1;% I' I( q2 O* _, y/ Z8 O
- D3 = ~(sData>>2)&1;
. [8 {$ }, m& X# o; e( L$ ` - D4 = ~(sData>>3)&1;% u# m% m' R, r1 m, [9 r. u4 L5 p
- D5 = ~(sData>>4)&1;/ o5 g, a O) ^3 | ?$ R
- D6 = ~(sData>>5)&1;
6 G# K, w5 y9 W k - int downD = D1+D2+D3+D4+D5+D6;
+ Z6 M1 A, F5 ^" X - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);2 u3 Z `/ V' ~5 [! G
- if(downD == 0)! p+ C" e7 E4 n2 [0 `3 R# f
- {3 a/ J4 N3 Y0 c4 @* [$ L
- return 99;' J, \' U) a# _+ o7 r
- }
# G* Z: t1 c* E7 [ - else
( L- ~0 j) S: p, h4 M - {4 u* ]5 q$ i' j
- return upD/downD;- H! ]6 g1 d; D2 v. l- c
- }
: ]9 ]5 B$ O1 z. H) c' n \ - }2 g1 ~$ v- q' {: k
- ! y( c) J5 J) H' F
- void moto(int Speed_L,int Speed_R)$ `" ]. F+ h4 m
- {/ W. U/ c$ A( J
- Encoder_2.setMotorPwm(Speed_L);' ^9 [, ?* }2 {" ]$ Q
- Encoder_1.setMotorPwm(-Speed_R);
0 k' ~- |0 K0 B ^ - }
複製代碼 ' a O2 ]0 D: E4 \
MeLineFollowerArray.cpp/ }! |) S, o6 w, S
- #include "MeLineFollowerArray.h"
8 ^+ R* c( r! o8 Y/ |5 t9 O$ m - l2 w" t/ q; D' O
- #ifdef ME_PORT_DEFINED
8 A/ m7 |4 d; U7 v0 }, B# s - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)- M. ~' B" w3 G' Q
- {
1 Z# P+ C1 L6 G$ o( ]0 P& S9 ` - . N# Z! D5 ?0 B" k( x, w3 \
- }
! X2 {+ v" u' B1 m1 ]. V - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
' H- K) s/ d. }- H5 v" C9 H - {' _7 U% g, A2 K2 z1 j$ K
- _DataPin = mePort[port].s2;
+ O K9 R) U3 o - pinMode(_DataPin, OUTPUT);
- ?, b0 X# Y' \ - digitalWrite(_DataPin, HIGH);
+ V O. Q- B8 \* h$ C7 C - }
* C N: u8 o* r1 V8 e3 O$ E% W& }) \ - #else // ME_PORT_DEFINED& U0 W* M0 |0 N) f7 Q
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)" @8 u j* U& Y
- {
% L. k( W& ]1 y9 F# C - _DataPin = pin;. H# k2 C+ u4 x6 u' O' s. `0 }. c7 o
- pinMode(_DataPin, OUTPUT);
+ X) X- Y [! j7 V - digitalWrite(_DataPin, HIGH);9 o) T/ S2 g" K4 S9 a
- }
( x: b$ S" L2 _7 \' M$ S6 [8 _) _4 o3 s - #endif // ME_PORT_DEFINED
7 V C8 Z2 ^/ U6 _) ]
, a& E$ @, V! n( D( E
$ E, Y2 O; l' a- void MeLineFollowerArray::setpin(uint8_t pin)3 ]6 p1 y/ z e! Q3 w* I6 x
- {8 y4 `% \/ o; m& j
- _DataPin = pin;
# m8 s7 a' w6 r C8 w - pinMode(_DataPin, OUTPUT); 6 q% O) a. u& Q; R2 l% \8 B* a
- digitalWrite(_DataPin, HIGH);
- m7 n" ~/ d! X
4 X' A: \& ^. ?- k8 K2 c- [; ]- #ifdef ME_PORT_DEFINED
5 L* Z- h' }( M/ T5 b - s2 = pin;: e4 r1 b" F9 m) R) v
- #endif, k* d9 M* H R0 s! R$ S, |0 V
- }
* _% j d" n1 o& A) g3 @" s; l - 9 R6 g7 u% I# u. B2 O
- uint8_t MeLineFollowerArray::getValue()
5 ^" F6 s& t# S- H - {! w% D. ^8 i, T! W D5 [* {9 m
- uint32_t LOW_level_read_time;9 Q7 v8 J6 S* g3 x4 K
- uint32_t HIGH_level_read_time;
/ ?2 d) J, q2 w! O - uint32_t time_out_flag;$ y6 L6 z6 r8 u/ p, L) F \8 l
- uint8_t Sensor_Data[3];
" R2 P' S: J. H - static uint8_t old_data = 0xff;
2 H+ v0 D4 h& [2 ?6 h8 F
! O# d' Z. X' A6 t- [! W/ z. p- pinMode(_DataPin, OUTPUT);3 `0 x0 B6 W3 @' _; K
- digitalWrite(_DataPin, LOW);
# j# b4 |5 M6 n3 d E - delayMicroseconds(980);
# g! z% O' v$ D - digitalWrite(_DataPin, HIGH);
% C7 R% E% L4 k4 }8 b
! |& D* S" D; W9 U8 m- Y- pinMode(_DataPin, INPUT_PULLUP);
6 E8 Q. B5 S4 C }9 H' _$ q - delayMicroseconds(10);
, t& [) _, Y; T+ \4 T% g
, x% G9 {' h2 V1 J* C% O# y' x {- time_out_flag = millis();
7 r6 i- U& ^2 d - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );7 r, R9 H0 W U. f0 y$ S$ U& C% a
- 6 ?- Q" `9 h5 s0 W
- LOW_level_read_time = micros();7 u& v* E* p( ]+ D
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out6 w. \* p2 @9 g3 S3 w+ _
- {
* u6 w, l5 l. V - return 0xff;4 X" k; E: J- V, X: K& \& G
- }3 z# A) A2 o; S
0 U( U, S! j1 ]: K0 u2 E- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* Q7 a" n) W* I* x% P4 D5 I
- + u5 R1 |% W$ x" p
- HIGH_level_read_time = micros();( }* E, b9 E4 k" k2 T% u
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level0 q1 f( s" P2 g f- U
0 ]6 S: O' H1 d" K" k- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
/ o/ \$ |# c+ X/ S0 o+ {% k - {5 d2 \4 x8 [( U+ d
- return 0xff;
/ W3 g+ {& X5 |! U0 S$ i$ I- L - }7 ?9 i3 E! w* l9 X7 g2 b
: l1 o" @4 U% G' g( T/ H# |, D- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
0 u3 @8 C& A+ j1 I - {
! b. Z/ T8 V# c0 O2 o - return 0xff;
: q& L; N5 X5 I6 i - }2 { [7 U2 H6 }7 J i! e9 G
! V/ n. a1 z f% C1 S- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 p2 g, k+ K& o: X( J5 _ - LOW_level_read_time = micros();, B9 k. D2 Q# R
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
# j0 m L6 w7 Z8 c) F& x+ j2 V
) q M( m J6 T. N! f0 m* ? b& g* S( I- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out7 `$ k" O Z; \9 X; H
- {: J- R& j( B4 ]
- return 0xff;0 E' C! O0 k. Q: ]% p/ I3 P
- }7 V. Z$ M$ x$ f
0 H0 v3 ^! y( o- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
# C: P% A$ F' J - {9 U. I% {) t/ ], f% ?6 c% H$ e
- return 0xff;
+ m' \0 u% S( E. S/ F- S& E/ Q - }
8 I0 r7 Y% h. {" I" o
: X( {; h& K1 l# E* ]7 T& U- for(uint8_t k=0; k<3; k++)
0 G3 [2 d8 R0 M: i0 u, h1 ^ - {
P! u) H3 t6 @ - Sensor_Data[k] = 0x00;
" M/ K, Q9 x2 E3 a - " ^' ~0 d3 o+ m! l+ Q" W
- for(uint8_t i=0;i<8;i++)
- m1 d6 f$ g! J. `/ E3 [ - {
( a6 D$ Z1 G: Q' x/ s1 n - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
9 x3 M* s- |2 X - HIGH_level_read_time = micros();
6 L8 s; n- M% p& }" n( h8 b1 a - LOW_level_read_time = micros() - LOW_level_read_time;- I& \4 t9 i* F0 t9 G9 B( |# j0 p
- , q' [% ~' p+ b) x Q+ d
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )6 n0 j0 q. R% ~7 s) j8 ^
- {
( L B8 o# T7 q1 s9 Q8 q5 F6 P - return 0xff;
6 ~5 w9 M0 V! @/ z6 Z. z - }) t) x* S* j1 o$ M! C/ L
- ! L G- u4 R( z Z& D @
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 b2 |, f( e4 g& D - LOW_level_read_time = micros();
+ M. C7 G8 T) T0 K - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
5 T& p& c* R2 J9 A
8 h; F! A8 g: J7 H. @- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
9 |7 _7 z, y/ w- S( G: o2 a - {& g3 ~0 X( {+ [ p4 x- o+ G/ ^. N
- Sensor_Data[k] |= (0x80 >> i);) u1 Z8 |& x/ f X* Y/ t+ \
- }
. c) H- P* a! z: ]2 P1 E - else if(HIGH_level_read_time >= 100)+ q! N3 @/ u" B2 o( s W5 L6 Y
- {
% K4 a4 k, W9 ]7 I: e5 n, a - return 0xff;# m3 O7 i ^5 ^# F
- }
0 l* y4 F! ~% U) v( t3 i* n
! ^ I2 C$ H J9 S- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)% A. n( {: s3 {1 ]+ p& J1 N
- {
" w# H7 C N& y7 G* v+ ]( B - return 0xff;5 \) Q" z) S) y( r: h1 D1 O
- }) P6 {; }( ?* U' [& z: Z2 e
- }4 m+ r- t& M- ~0 W$ H5 u6 o) A7 f0 \2 E
- }
- q, `. t/ O9 ^) ^* q& E
( k1 t0 w: _0 J9 q/ v" F, D- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
1 m8 X9 M2 w1 w. v: n B8 X - HIGH_level_read_time = micros();5 c, ]/ |( T2 v* k% r7 W
- LOW_level_read_time = micros() - LOW_level_read_time;) e4 s+ d Y! k
- 6 T8 I' I% N* B# {( u2 F$ k7 z, H
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
2 u0 P i, o* d5 b; U# i, V - {1 G8 s6 }2 G; H2 x- y! ^
- return 0xff;
$ `. ~( E$ T; x) B( X t# f0 { - }1 s6 [; K9 Y ?
3 E, ^# h7 v S+ e4 y& Y. T! E- pinMode(_DataPin, OUTPUT);
% Q1 D- b+ f: I C; {% ^+ D - digitalWrite(_DataPin, HIGH);/ g3 f' V( v+ {% Y- w
- 1 e* t6 e. z+ ]! b H
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
2 ~* N* p/ A8 y3 P1 W8 W0 C' K1 w( Y; t: {5 y - {
! j0 }% {" [/ U4 Y# }. @1 K - old_data = Sensor_Data[0];; X _7 [7 P' h& Y& n7 I
- return Sensor_Data[0];8 [1 L Z' O1 B
- }
" j7 s1 c, `/ ~% d - else
6 ^+ K* @0 }8 x- X - {
/ w( E; T& [2 E" ?; Y! A - return old_data;
- s% Y2 R! n2 B1 a6 l2 ~1 ] - }6 S5 A8 |/ Q% e& C- o
- }
$ ~" m) C4 G$ [# o) R" K4 @! c
複製代碼
- @- y# h7 w6 N! w& h' v- v E, QMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
) h) @7 i% w& Z6 \
9 G1 y* x# b1 p; b
|
|