|
( x5 K6 B0 [5 I
- #include <Arduino.h>
3 C- K+ |1 i6 L/ L7 h& i - #include <MeAuriga.h>
7 E6 \0 G* I( k6 `( @) k. R1 f - #include "MeLineFollowerArray.h"4 u) t& R8 r8 w7 ^
- 8 p3 V* I+ Y' w7 K* M: F5 }- k, V% {
- MeEncoderOnBoard Encoder_1(SLOT1);$ ]6 m9 `2 ?& D O& |: p" Y
- MeEncoderOnBoard Encoder_2(SLOT2);) m$ B5 a. c+ n! [; q
- MeLightSensor lightsensor_1(12);& E. h' f5 Y7 @1 `( C
- MeLightSensor lightsensor_2(11);- x ]2 i+ z4 R8 ~' F; B
- MeBuzzer buzzer;& J; [: K1 p: |: Y0 X
- MeLineFollowerArray linefollower(PORT_6);
& Z5 Y% ^" j( v- i# q6 X8 T - & E1 b9 c5 u/ G# ?. Y7 w- r
- #define Error1 11 A; |: V, i% M' p. Y
- #define Error2 2
7 i6 R( M! A7 h8 z* E% L - #define Error3 3
% O3 y2 a8 A3 X! n' N- [6 B
, \3 }. [( S* ?$ j; K. F6 O- #define Kp 15) x: I, I& {& a9 Q4 W# e4 N
- #define Ki 0.15
4 r1 S# C4 E: Q! H - #define Kd 0.034 l3 [& f+ f! c+ w$ _ m p# a
) Y) e( D- {- z9 E# P0 X0 |- uint8_t sData;( V0 }0 f. L R) W8 ^' K! @
- uint8_t D1;' l I& c- R. p2 i. ~
- uint8_t D2;: @( m' `& C @6 i% o5 H1 h8 D' ]* x6 K
- uint8_t D3;
( t% y4 U# g' ^: T( [9 H0 E& k - uint8_t D4;
& G" b2 \# t' U0 W( h; n - uint8_t D5;
" Q; n5 b( z, O$ P3 @ - uint8_t D6;+ T( T8 h' n7 Q# Q: ^. d
8 k! v3 u. J; \) P+ d- float previous_error = 0;
! o5 |& _- ?8 J' O+ r2 O9 B - float integral = 0;
# o C( w+ J5 u* h - float derivative = 0;' i9 {+ Y, J; h' V
- int Speed = 160;
: v4 e6 `# a! _ - float output;, Y9 A% M8 I: `
- ' Y* H: S0 C+ F, C: }
- byte Left;
2 r/ [) ^/ ?' ^( R& m
+ Q) F3 \) U9 k- void setup()
( b2 B# r1 X) [) v& [4 H5 e - {
6 D8 d% C7 [2 h; |0 O - //Set PWM 8KHz
3 G. u% }! ?# ~1 Q- [) B4 @ - TCCR1A = _BV(WGM10);
, i' t- `5 H* F/ K; Z: V1 _ - TCCR1B = _BV(CS11) | _BV(WGM12);
- ?) \/ _* q+ R* i, }+ v2 K# d* x1 d - TCCR2A = _BV(WGM21) | _BV(WGM20);
6 v6 u+ x2 G5 m' p" d- L! G- \ - TCCR2B = _BV(CS21);
8 H# L9 J. L/ `# v! o+ z a) O( W - Serial.begin(9600);. |; R$ U7 U; I! J# I
- buzzer.setpin(45);
! ~! R1 N# O" T* b - }
8 U0 P0 D E7 R# x - & |- _+ K* \5 A) \4 H7 u5 X" {
- void loop()
3 O& p2 a+ m5 _3 L# ?1 b9 _ - {+ t, m2 ^9 L- J# t5 x; a2 c" [
- while(!((lightsensor_1.read()) < (10)));1 `3 s1 s0 h" l8 V
- buzzer.tone(1047, 500);
# H- w3 P- n8 N& Z - delay(500);
$ V& I+ L( n' g3 o( x2 ^) j+ S& s - do# b. G* Y9 }$ u
- {
q1 S3 z& \( i, l5 j - int Err = getErr();
$ Y. D) k$ A. \2 g - if(D1 == 1)/ l. X! K! R. P/ x0 Y$ X
- {' Y0 a" M, Q) V0 E* O& Z9 q9 S
- Left = 1;
8 N: S T- _/ @) l5 z1 a - }: H7 p" f2 N' c( a% R$ @ N
- if(D6 == 1)
t( `9 }3 Q- r n/ E - {4 f: J! U; F3 w+ J) Y
- Left = 0;
7 U; x& ^) K) n4 Z - }' u4 c, S0 g. S) q& C. o# t5 Y
- if(Err == 99)2 s4 x& T. a0 \, S; `& s
- {
" Y9 \- H) e/ v - if(Left == 1)5 `( O, Z* d, d/ y
- {
$ ?2 m) |0 ~: D- M6 ]- G - Speed -= 5;% s, d8 d. x1 t5 s$ a0 W! |. @
- moto(0,Speed);
; I0 S) ?3 e1 N0 D8 F C - do
4 w( [' |7 U% o& M3 A% M" L - {" H+ g5 W6 p( h4 M
- Err = getErr();
" [! o) R1 @4 i. d& X - }while((D1+D6) == 0);) ~' b0 `# K/ m& n( {+ y
- }
1 i3 n9 r% p& K- ?8 s, \ - else! f' c I5 K1 p0 l* `
- {5 h3 d: t: Q0 j: \, I
- Speed -= 5;1 ?" t# c3 F& m" _; S
- moto(Speed,0);
' `+ f* l. m9 X5 c; H3 L - do" W. Q2 q/ S7 W8 o v
- {
8 s/ [/ p" f" J6 _# m/ z - Err = getErr();
. k( Y- C3 W6 k( i# Z3 m# F6 f - }while((D1+D6) == 0);
/ J& k9 b: k) J - }0 b r Q( ^) l* q' ]
- }, J' `% |0 d" P$ o, U8 H
- else6 c9 {" v, s' S- p) D; o8 L, Q
- {1 J; I2 b @+ E+ g
- if((Speed < 160) & (Err < 2)) Speed+=1;
/ `" ?; ^+ q9 r1 X5 j/ p# A - if((Speed > 100) & (Err > 2)) Speed-=2; . U( I7 |6 L1 @
- integral = integral + Err;7 |0 V$ t4 ]9 q& W4 l$ p1 G
- derivative = Err - previous_error;7 _: E0 w3 |" Z
- output = Kp*Err + Ki*integral + Kd*derivative;
' B" I5 v8 i" e) j$ h# e - moto(int(Speed-output),int(Speed+output));
/ t) G5 X4 G0 u" H s+ `" p; q - previous_error = Err;% w/ F/ J8 C; `0 l6 S" ?8 T. d
- }; `/ Y/ U3 `- W' X
- }while(!((lightsensor_2.read()) < (10)));
5 s# `5 I W+ \ - moto(0,0);3 z$ e5 A8 O2 b
- delay(500);! d- B+ f5 }$ v2 z/ ~
- buzzer.tone(262, 500);
f5 a: l3 u4 b - }! a+ R& j5 {9 p" \* x
. H6 ^( `& {# V6 e1 U- int getErr()
& ?6 m: |( E' A# H( n - {
& F9 \* o# E+ x, B, @- q" P - sData = linefollower.getValue();
2 j" }1 y6 Y4 m; j' c. b- q - D1 = ~(sData>>0)&1;6 ]% e1 W( E4 K0 _
- D2 = ~(sData>>1)&1;
2 D9 x& p3 r0 Q# T - D3 = ~(sData>>2)&1;8 `) z! ?$ e2 ?2 o# w% ?
- D4 = ~(sData>>3)&1;
: G+ R2 G5 C& ?) B - D5 = ~(sData>>4)&1;! W0 G# k" Q) Y6 g
- D6 = ~(sData>>5)&1;4 _. |7 p: {) |/ t3 X
- int downD = D1+D2+D3+D4+D5+D6;9 m: B+ N- S; O1 @/ `" B
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);+ J; q5 O5 [& ~
- if(downD == 0)
) J' R( H6 {4 s - {( n# O- M! H- P. ?0 }" z. h
- return 99;
0 Z* j1 ~3 m) T& p o% u2 Q, [ - }
$ p" ]8 F5 a+ u* S; M! K - else# E% }3 h5 _! U0 t, k1 L+ w! H3 c
- {2 }# C9 L5 }6 I9 C3 ^! U' y" j# g& n
- return upD/downD;
7 E& E5 G& H6 W- x - }' b& q$ j/ ]( q
- }
& ~* a8 L8 k+ v4 o5 { c( |% B
, y7 b: N/ M" l& N3 W: {9 l6 s5 T: F- void moto(int Speed_L,int Speed_R)
, _) m8 }; k1 m; U- a' w" w8 p: f - {
/ v. V5 n5 X7 a9 |. S& K1 t9 H - Encoder_2.setMotorPwm(Speed_L);
# O0 \% ~$ l3 P' B! f$ w - Encoder_1.setMotorPwm(-Speed_R);! }# z" v) c% E. v# s/ P8 E' y
- }
複製代碼 0 o2 R: Q1 n( M" K
MeLineFollowerArray.cpp& z' n" P9 f ^( E
- #include "MeLineFollowerArray.h"
' i( N* r3 b- w' {
, k, k- q3 k7 d- #ifdef ME_PORT_DEFINED7 W+ J: X8 ]' ^7 o1 C: ~2 ]
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
/ m: K+ Q. |9 V9 g, u& `. ?2 { - {
) p) I, ^ {) j6 b - . P! h0 {/ w" r
- }
6 Y& M, ~( G Z. w - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)! @ }7 E6 ^+ a9 v
- {6 {3 e1 T Z: @0 M! i; o
- _DataPin = mePort[port].s2;
6 j6 ^3 b; Y/ Q9 P8 t$ V0 N - pinMode(_DataPin, OUTPUT);
2 I3 M* }9 D6 l) u$ B L$ [, S - digitalWrite(_DataPin, HIGH);1 m5 w9 y6 `, n( A$ D6 t
- }
, d0 N" N0 s9 }. F r" N - #else // ME_PORT_DEFINED
Y2 w8 G- F, @/ b - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
2 b1 q/ X0 s1 V0 O3 z - {
. i' m6 A. q9 x - _DataPin = pin;
& U. p% u! q7 N! T) n$ q - pinMode(_DataPin, OUTPUT);
9 y0 Y( d: a. b: U/ M* e - digitalWrite(_DataPin, HIGH);
6 v8 R1 _1 h/ t# q" H% @* R: | - }. h* ~4 u/ x( V, u
- #endif // ME_PORT_DEFINED6 F$ t. j* v$ H7 l* d
8 s) r, g e% {6 D5 x7 @- : P7 M& u& M3 P/ h2 o3 [
- void MeLineFollowerArray::setpin(uint8_t pin)# Y+ @* T' M3 {9 }. b L
- {% M7 ?- S4 G) o' A
- _DataPin = pin;" X; G- ~7 Z; f0 c; m
- pinMode(_DataPin, OUTPUT); 5 B! Q& x5 h8 n, _
- digitalWrite(_DataPin, HIGH);
+ N' e- `; b) R9 i; _. |0 X
3 {, q, F9 h) X/ a' U- #ifdef ME_PORT_DEFINED
( V1 o/ u \' O; ?& ] - s2 = pin;
# D9 y y; h6 K. G - #endif
6 m& I. A7 w% U* l6 E# n - }7 }% A3 ~, T' X6 z+ R
- $ f! e, w" ^* A" ]
- uint8_t MeLineFollowerArray::getValue()
& C; R# z9 M" [0 f7 j* ^ - {1 _3 v( U+ X) R. @
- uint32_t LOW_level_read_time;
. V5 Z' n7 g/ Z2 m: e8 F' I - uint32_t HIGH_level_read_time;0 O. |0 D! C; Z4 ]
- uint32_t time_out_flag;
- o) i1 F6 U: W& [ - uint8_t Sensor_Data[3];
# M1 ^6 k' ~& l - static uint8_t old_data = 0xff;
. W8 o! |, C* g# \! \4 V" f; z
# ?0 g9 j2 W* T% L) k$ J- pinMode(_DataPin, OUTPUT);: P5 @# w, I& V( y6 R
- digitalWrite(_DataPin, LOW);7 [8 X! j( _ E3 b9 {: m
- delayMicroseconds(980);7 t, H- P7 g6 ]+ r
- digitalWrite(_DataPin, HIGH);
+ {" F# n0 M9 N
9 [: L5 N) y7 y, w( t- pinMode(_DataPin, INPUT_PULLUP);
- I5 S7 g3 q: V( p' ~& n0 B/ K - delayMicroseconds(10);7 u) N1 ?+ e# C- J5 |% J
- " G& f1 k5 R, ]- o/ B
- time_out_flag = millis();% n' K& s. f' k2 d% S3 t0 {/ u" \
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: S( ~" n6 E* `/ s - - B b% k9 W/ Z% j( D
- LOW_level_read_time = micros();
% S7 h4 g2 ^" _, _# e - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
% e# z9 K0 q1 m: G: _ - {
% X/ `4 C- {: s- F& ^; e/ O& c - return 0xff;
) d% t1 P! i8 ?! n- ? - }3 x6 Q4 G2 @: }; @2 N' ^
. E; m% ^5 U1 j2 ]- E# n- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
/ U; j) f. q/ h% B# I
; U: M W$ d0 t/ X& e- HIGH_level_read_time = micros();
3 h& Q1 u" N- ~( I& \1 I - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
: N& G# R6 M; _" P
1 \7 b( n7 k$ H o- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
( h# a* r9 n( _+ U2 A - {
J5 n* y2 O# V* E+ {7 g2 E: K - return 0xff;
* F& v1 C1 `8 V" c& X - }3 C/ ~+ T9 \" u: y7 x ~
- 9 J8 n& B2 i! n0 J) R1 d; p# g
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
1 v/ q! U2 N: `) d, J8 \ - {
0 S* b* a. k7 J - return 0xff;7 E# n9 M6 d! v, ^7 V
- }
% P* m& K" U) i
3 L& @2 i/ J* D- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: X4 e5 R% O7 V3 r) n
- LOW_level_read_time = micros();
a9 M& ^" h ?; y1 G6 g7 j - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
2 t& @. a4 f2 Z: D7 M
$ l8 U- d- o/ P/ m. C8 l& n) M x- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 M. x/ L& [9 @ `; r. u - {
. [% N) ^- R) z; [) Y( z7 w - return 0xff;7 M5 w: m7 c) l$ Y j/ ?0 I; ^ V
- }( v0 ^7 {" V+ [- @ r$ c$ G
- " Y3 o% d( c/ ^9 w: d* }
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
; h9 y. [' L; G - {
r6 Z- N; A7 M$ D' Q - return 0xff;( Y5 {& W! c- a' B8 K
- }) P" s, B7 H2 |
- 9 y! D% N* u. F* h7 s3 d$ s
- for(uint8_t k=0; k<3; k++): D' K4 J8 U1 F
- {- ~. k5 B U$ |' K5 L
- Sensor_Data[k] = 0x00;4 I8 m/ V7 p" o- G: g+ T3 S" S! \
- 1 p; f& _3 B8 M& g0 i
- for(uint8_t i=0;i<8;i++)
7 ~# [$ |; H( W, N- G9 ?4 c. `0 J& D - {
: [, f3 l" K3 x3 }1 J- \/ r; [) a - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
9 C/ N3 y. ~, ` - HIGH_level_read_time = micros();: _3 N& y2 A5 A0 \6 Z+ J5 a
- LOW_level_read_time = micros() - LOW_level_read_time;
) V4 y( S; ^. E# q
2 |; E m u2 Z/ p$ g- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) d- z" t; O! Q$ F+ ]
- {
8 Z# z) @; w3 M - return 0xff;
& g" n, T! h4 S) U - }/ X3 m9 y( x8 m5 r
- 0 x7 J$ m: I) _, Q: r
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: O3 o, @- A" v5 M- w3 q) Y" Y
- LOW_level_read_time = micros();- D, d/ I2 @0 \/ k
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
# _) `3 W* ? q" }+ m) @ - 7 q& V6 Y% M. Y5 R+ n. b
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
& j; J4 j& A, y- I - {# z. o. a3 j( Q
- Sensor_Data[k] |= (0x80 >> i);
; R; K# w' I0 a* T/ x( Q5 F - }
! n, q" c2 G6 `' g7 p3 s8 {, ], t - else if(HIGH_level_read_time >= 100)7 c# ^! I6 R7 a3 V( }5 @6 J) O
- {
' a Z7 Q# y* R$ z" y$ `, v3 i - return 0xff;8 G( V/ i9 r! }5 r3 m8 W
- }
* L } {1 t8 [
# Q: F1 p5 L9 N. ` [# ?- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)+ W- _% B4 }# \
- {0 M' Q0 \+ S. s9 ?2 g
- return 0xff;; {6 p: a, N, E1 K! k, o" k
- }& a, o2 r2 x/ r) p/ n8 B# |# g+ x
- }# I5 V+ @8 P0 y v/ E
- }
8 U3 N- ?4 A0 K: C - K" z: H$ R3 V( V m" a
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
: H9 Y. X4 J7 n; T4 }& U: W - HIGH_level_read_time = micros();( m( T" [' k6 ?
- LOW_level_read_time = micros() - LOW_level_read_time;
# [; I* j- n% _8 X: ^
2 p( P4 e4 \3 }$ j8 }- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
& b' G: H9 }! p1 B - { @: C m$ L4 V- G2 n& q0 G9 r: E
- return 0xff;
' t! ^9 e; G F5 z8 ` - }/ A# ]- ^$ X1 T+ Y$ z
- % i6 r; f" T" |9 S% k" v
- pinMode(_DataPin, OUTPUT);% ~8 v- Q0 b; j3 }- o
- digitalWrite(_DataPin, HIGH);; T0 A8 J! D5 w
- . r0 J4 c- t4 ~1 {. a
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
# Z( u M# k) q/ I) [0 I9 t - {
" S8 U8 i* [! I, Y" Z( O3 @9 T7 L6 Q - old_data = Sensor_Data[0];: [$ P2 Z5 U9 x# q( q3 p; v0 B" J
- return Sensor_Data[0];2 O- p1 b, r ]7 z* j4 L9 ^- Z
- }
) w9 K2 W' o8 Y! b" i - else/ u" |' h7 y' z& w
- {
5 f. s2 y, @+ e# t - return old_data;
2 R% g8 b, T( f- X# i- ? - }
9 h/ E$ u( R1 b% f - }: w3 q2 e( b& M, ~; n" b+ B5 r {; A4 n
複製代碼 " @. j) j" f, w
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
$ E' G# S! o9 J8 a/ O0 ~2 H, g6 E: p" W% c+ I
|
|