|
|
1 h% d, ?4 M; M! c
- #include <Arduino.h>
+ z; o- Q& R( [+ E2 I - #include <MeAuriga.h> P% X/ B6 v6 A0 ?
- #include "MeLineFollowerArray.h"
* r2 ^: e: }# O" a' {8 {" d1 u - + r$ j: ]; W) j+ ^+ [& O
- MeEncoderOnBoard Encoder_1(SLOT1);
( {3 {1 O; r" |0 t! U2 G' j - MeEncoderOnBoard Encoder_2(SLOT2);
* y8 I- i! T. F# C$ y - MeLightSensor lightsensor_1(12);! S8 N% M0 C. |8 P
- MeLightSensor lightsensor_2(11);
+ x" w- l% |4 V, [+ Z% C - MeBuzzer buzzer;
& |0 }1 q2 L- i. f! X - MeLineFollowerArray linefollower(PORT_6);
) b7 i) V1 R: h7 p7 y4 ] - 0 ^$ a: ?9 h9 W
- #define Error1 1
- j! s; A# C( g5 M - #define Error2 2
8 Z6 `) K9 i# H) J: T* X( O - #define Error3 3: h) ? q0 R3 E8 S$ v% K
5 n6 U" v0 a7 o# n% \* ^8 O$ K- #define Kp 15, s" l: \: p3 x3 i. L: U7 S5 a
- #define Ki 0.15$ z' m$ P2 F: l @
- #define Kd 0.03
- f/ x3 s+ u% m9 ^' U- E8 E. S& ^ - R* s9 n z3 R4 o! O* ?2 E
- uint8_t sData;6 g& P& ]$ y2 S0 t# W6 r8 n n
- uint8_t D1;
0 d, [0 f; i' e1 Q# b1 Z5 ] - uint8_t D2;6 r0 d& j2 t7 _% r$ S* @) z, Z
- uint8_t D3;* f. f0 `9 ^9 A, V6 t) m
- uint8_t D4;
3 k0 r- x& g/ l. {2 u. H( R - uint8_t D5;
/ J9 |- T0 h% E - uint8_t D6;/ M0 x2 ]/ \( @2 j2 j% K0 ^( m. T5 {
- $ ]$ s+ ]! b8 X, E8 g6 h l
- float previous_error = 0;1 h1 a0 T" N. }7 H$ F! u8 P
- float integral = 0;3 n! F {5 S1 s
- float derivative = 0;
; a# g* d. k9 H# F7 v! x - int Speed = 160;
\2 Q) V- j6 A [2 M/ k - float output;
% z: i, U) Z& x x9 p. Y - # f6 H1 o1 ~$ N/ R5 m
- byte Left;
5 T- r, O% u: I! L8 n - $ ^. F. U, b7 A" W+ y2 ^3 M
- void setup() , p5 R F/ {: w% p2 j# n+ E% u
- {3 A. C+ K: [5 W
- //Set PWM 8KHz: R9 V8 H H7 ~! m5 b6 F& ~
- TCCR1A = _BV(WGM10);
% A" v) |$ e: y) N; S - TCCR1B = _BV(CS11) | _BV(WGM12);# B5 h4 s: m9 w; V3 k i8 c7 |; j
- TCCR2A = _BV(WGM21) | _BV(WGM20);' f" ? s% c: V$ t. C- G0 o
- TCCR2B = _BV(CS21);5 P; L+ G$ _$ W+ s I
- Serial.begin(9600);0 o& L) c7 V- W6 w
- buzzer.setpin(45);: f) Q' Y( F& p6 ^
- }5 e% D1 V1 Q! ]9 Z% g: P0 S5 |9 y
- T/ s0 F6 V- a- void loop()
# W* A% H6 ]2 O - {' E6 T" p7 ^& |5 B/ e
- while(!((lightsensor_1.read()) < (10)));
. n) j, U, `8 P9 F2 N - buzzer.tone(1047, 500);1 g5 i0 {) J8 r( l$ F3 D
- delay(500);4 d( [# P: T5 {( l
- do$ y# E) A: H6 {0 f
- {
3 ]3 g$ P: @5 ? - int Err = getErr();
6 e' C2 ?/ ~$ L( ?7 E5 U2 Z - if(D1 == 1)% {9 `- [' E: ~5 X4 F
- {; l, v8 W4 M. [
- Left = 1;
) q8 [% n! p+ h! B - }
: L$ P& P, x- M* g - if(D6 == 1)9 Y* g: q/ K& Q. Z6 w4 M. |
- {
% T" R+ t! F" k - Left = 0;
% M' P9 O* h+ m+ R& f- R* | - }5 Z% v* v: V. ?
- if(Err == 99)1 e/ S2 R- O/ ^* Y, o
- {
2 {3 H7 U: c$ _" A& a% y - if(Left == 1)3 Z" t8 V; h8 k; u7 R
- {
- F! r+ l3 }; m: B3 d - Speed -= 5;/ L# E, c; N1 d
- moto(0,Speed);8 j+ W8 y% m" f, H4 _
- do
* [! L% e' T) `+ a) s - {
7 g h1 S: }, i' H0 y; z - Err = getErr();# o# a( C, Q% i
- }while((D1+D6) == 0);
/ \6 m; D$ ~6 ]9 U' n, D1 M: o5 ~ - }% Y2 r# V' u- j( G) ^- e
- else
8 N7 R4 o3 X9 m; x! R6 ~* U - {
5 r; d' V: W5 D- b3 E - Speed -= 5;" [+ J1 X1 g: ?$ @: L0 h! ~" }
- moto(Speed,0);
. M! `$ h& U1 |5 j E- i( V - do
) M& o# f! k Y+ J( q - {
5 C0 k+ o& T6 I7 D! i# d - Err = getErr();
5 B: u2 T, w# c/ ` - }while((D1+D6) == 0);
8 q2 g1 e3 K J1 { - }
$ D$ T6 ]2 _: f2 L& c3 f - }# v% k0 n. [% v9 Z. S2 C7 T/ K
- else3 Z2 P j+ Y2 i7 A# c1 ~. n2 N
- {/ B. r+ h, {' W- ]; A
- if((Speed < 160) & (Err < 2)) Speed+=1; * t8 b; W/ G3 e$ N/ z2 d9 i
- if((Speed > 100) & (Err > 2)) Speed-=2; , Q$ `4 W+ I0 {2 Q3 w) G/ V. _
- integral = integral + Err;( Z h/ X ?/ N$ M4 O
- derivative = Err - previous_error;$ q; X- h% p* ]6 w: x6 } ?/ U
- output = Kp*Err + Ki*integral + Kd*derivative;4 b& j9 M7 r/ Y5 `
- moto(int(Speed-output),int(Speed+output));
3 G/ ]( B4 M- r1 m9 y7 F; p - previous_error = Err;4 t6 d1 E$ X: P' m2 b
- }1 R$ `- y! v7 `! F
- }while(!((lightsensor_2.read()) < (10)));) ?& F4 h7 @* x; Y( k
- moto(0,0);
' Z5 |! U0 _" \/ n - delay(500);2 C1 l; M" F3 f. d: I% }4 C. ]. v4 C
- buzzer.tone(262, 500);
1 r" {. k) L- C e9 s8 l - }/ {+ V8 |! U" |$ }4 B5 M `- F* l, i, o& t
8 s7 S- [7 S6 q3 a) E2 {- int getErr()
2 }- n. _" d6 v0 c6 N4 n - {
+ {! Y% ?7 z9 E - sData = linefollower.getValue();
; }. h4 y8 M: F; A' S- J3 ? - D1 = ~(sData>>0)&1;' K% c; f. X! a( T. p$ S/ a$ p
- D2 = ~(sData>>1)&1;
. p; s) I" L6 U9 ?, i+ g5 H - D3 = ~(sData>>2)&1;
. Y+ ?, |; Q4 X( E2 G. c - D4 = ~(sData>>3)&1; |& W- Y6 M- L: s& v$ }
- D5 = ~(sData>>4)&1;
. F; @+ P V9 t' g( D: ] - D6 = ~(sData>>5)&1;+ u3 u' Q- y: X9 p4 R
- int downD = D1+D2+D3+D4+D5+D6;
% {7 p& S5 f0 `2 D, L! R - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);7 m1 M; D! a! \) S# J8 R; m% v7 _
- if(downD == 0)% }6 R: _5 b, N0 u
- {% [5 K$ B' N' w; v
- return 99;
0 ?! S. b" V9 X) O! { - }
2 G6 z& w5 _5 T - else& j( F/ ]% t3 E* M5 Z- W3 F& E3 M
- {
3 e! M; x- z" J9 [" V1 B, D - return upD/downD;
/ F: S6 A# V& b1 G! {9 ~ - }
/ ?$ j9 F% j1 B9 A; N - }# t% P, }# ^. f5 I
% A7 \5 P7 X1 W8 \1 j( u- void moto(int Speed_L,int Speed_R)
- j8 f- ]5 a1 J/ I+ }2 e - {
3 n) j* J3 S$ W* N6 K! Z6 U - Encoder_2.setMotorPwm(Speed_L);9 b) N- d2 ]9 }
- Encoder_1.setMotorPwm(-Speed_R);
3 ?9 \; R8 A+ x" B - }
複製代碼 + \+ }5 W. l) }& W2 m, W
MeLineFollowerArray.cpp# L& y# e6 j7 o
- #include "MeLineFollowerArray.h"
4 ]+ X" m: d; S: N - ' G# V6 H, `! U0 R: a" v2 e
- #ifdef ME_PORT_DEFINED
+ v1 g' f p& n6 L; Y$ P - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)8 Z, a; f( Y/ @ _
- {6 @$ x3 m( Y# p2 D* Q2 y6 D; q
, t2 W' m e" v* i/ ~. g9 M% B' [ S- }
- ?7 E$ k. j8 b" h- o. W4 u - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)) `! o b# L+ b5 ]% D
- {
' _6 ~6 D% y! P' N: f - _DataPin = mePort[port].s2;& V, t5 |! | V6 k7 B* @. ]. p
- pinMode(_DataPin, OUTPUT); . c, A: }& Q- h8 r; L% v$ z* o
- digitalWrite(_DataPin, HIGH);3 U2 @ K( s7 e Z
- }
7 i3 |# P, I9 }, J I9 ]/ x - #else // ME_PORT_DEFINED& h. y- ]. ?$ K$ P
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
$ v; P! n& v8 f - {
5 z( J5 J, c1 c, t - _DataPin = pin;+ J* L0 \# r5 D, i' }. ~
- pinMode(_DataPin, OUTPUT); 7 ~+ D% g; J, g' h' M3 W
- digitalWrite(_DataPin, HIGH);
" A" G; Z& `3 i# _$ a6 V8 n - }( n% r. v7 [: q" i
- #endif // ME_PORT_DEFINED; k2 v9 W$ d# c0 f
0 m# }; ?- Y9 s# o+ M% i1 N# T, O* {& B- + K$ C4 T1 H# r4 L/ C z
- void MeLineFollowerArray::setpin(uint8_t pin)
7 ^, L2 [+ j3 @, v% h. |0 n - {
* r7 ]) l; Z5 m - _DataPin = pin;
2 c9 J0 ~3 b" {: a: { - pinMode(_DataPin, OUTPUT);
; J* H$ b2 H" |6 y: k) F! Q - digitalWrite(_DataPin, HIGH);
$ S: n6 P5 I# F# Z2 t9 o* |: B/ U - ' X1 G1 k' ^" z/ @
- #ifdef ME_PORT_DEFINED. I ?9 V/ u2 O8 y
- s2 = pin;& `: ] s; X% d/ v
- #endif
6 e5 A7 |- x% f$ ^& }# w/ i S: X - }$ B0 s* Z2 k3 I' s
- . H1 l# l4 I# l3 f
- uint8_t MeLineFollowerArray::getValue() b' Y5 ?4 c% U- V2 F
- {
+ r' z' _5 G* A. m/ B - uint32_t LOW_level_read_time;
) f7 U; q2 B6 @3 X - uint32_t HIGH_level_read_time;2 O" j u5 z$ i. O o: `
- uint32_t time_out_flag;8 O: h+ V5 i/ B+ t1 @
- uint8_t Sensor_Data[3];# T6 c2 P* H2 s. L) p* |
- static uint8_t old_data = 0xff;) D& N9 h- f7 T8 \. W2 B5 x
7 r. k1 q$ \: U6 M$ B2 I/ t- pinMode(_DataPin, OUTPUT);
! Q8 \6 }6 `9 h' I - digitalWrite(_DataPin, LOW);% j0 f+ d# Y/ U7 }% z
- delayMicroseconds(980);
' R, _/ t1 T% d; J3 d4 t/ r+ X0 |9 ? - digitalWrite(_DataPin, HIGH);0 l5 Z) f+ e6 F3 d$ ~8 u2 k
+ |0 P5 @+ d+ [' ^+ O7 C- pinMode(_DataPin, INPUT_PULLUP);
" [& _1 N4 G+ O$ x4 Z# v! n - delayMicroseconds(10);& h( ~( Y+ F/ |3 O: X% R% A
- ' ^ H- Q. }3 v/ K' O1 i1 ~
- time_out_flag = millis(); g& }5 Q) B1 T h8 n7 R
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# l) R4 P- @5 r5 [2 f
3 m3 s* V8 [0 \/ G/ C- LOW_level_read_time = micros();/ w5 s" z) v6 \( m2 p, W0 E. r
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
, X) M! X, R/ p: c - {
& g/ G0 l1 |6 b6 O4 p, K/ ~2 [ - return 0xff;! t( b/ ~' h2 |% A& _" P, b* }
- }
9 r7 H7 z( ^3 h - 6 _+ b7 C( I/ r Y3 O |
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );4 e, }- }3 I* \9 O" h8 B6 d7 {
( A; p( ~, q: H# ~' D- HIGH_level_read_time = micros();' v& h" U7 Q( E/ u, S, ]8 R
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
$ L6 S; {, E: i" Y$ z; B# B - & w4 W' c2 l6 t' Z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
8 C. ?# }4 F! T - {3 ^) k0 }2 F/ H% `, p; }8 j
- return 0xff;
9 f' R: p3 A' J' F0 d - }( C9 M0 y$ q+ g L
2 V: C2 o4 K: I9 n* r- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))/ r0 Q- V3 f1 V, }; I
- {
( w+ c/ Y2 y$ K+ I2 H - return 0xff;* I4 T% z1 N0 P
- }
$ e- e M3 E) g) e# { - ( |7 j! h/ w: h
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* \6 ]! o, {) B
- LOW_level_read_time = micros();1 W8 a: ?6 D( A; Y- ?: d
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level0 a. A6 n8 G" w# r* ]
' K, v5 K R% u- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
P7 s7 o2 q! t$ \8 i - {. I$ k) Z) u* h) h. W1 a& P" u4 A
- return 0xff;
; M3 \9 g0 R6 m0 C* A/ g6 u0 f/ Q - }
! r4 `# a' v# N' X+ @
) }: E$ J( ], h) T* G) Z6 l- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))/ d! F2 D1 K+ a% z! V/ y0 T6 H
- {
" Z* Y T+ d! }% D( i. F - return 0xff;% B5 R# Z/ m' n, K+ }2 T/ Q; ? F1 ?
- }
7 l1 `# ^" }* l7 r% B - * {3 N/ @% }8 S1 t0 B0 F
- for(uint8_t k=0; k<3; k++)% a9 F9 @0 Z u
- {. J+ @4 `" o6 k
- Sensor_Data[k] = 0x00;5 V% {4 @- t0 P& c& t( X# R( r1 B
- / p$ r! U4 t/ r9 z) i& O
- for(uint8_t i=0;i<8;i++)
0 O+ _' f9 C0 Z. ~ - {6 z- ~, R k$ m# X3 u6 `: Y1 k
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
7 S2 [$ ^1 S4 D% q - HIGH_level_read_time = micros();
0 e5 T1 y( L# t) k+ q# ` - LOW_level_read_time = micros() - LOW_level_read_time;
; r' ?9 R$ F* }5 K. {
0 x* s5 d4 `8 x! |- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
3 n+ F5 w, I4 Z y - {
z' H- }+ J7 `$ O; S, E5 X- V - return 0xff;2 \& p7 p" R) C4 u
- }
( g' b. A" {/ t2 y+ t3 B - ' Q! @7 I" @- {# N- ~. Z
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 O& v2 x/ o% f% ]9 \) |3 | - LOW_level_read_time = micros();
& T9 S* G& M! X1 ^5 u) J2 J - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
5 \& |4 y4 J1 o
; |5 D$ p1 S/ k( F- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 19 z! Z* _+ H! O
- {& C3 t; r2 O, o& y0 A
- Sensor_Data[k] |= (0x80 >> i);. G/ k/ K3 b; l5 X- O5 ], V- v6 @
- }3 B% n8 G9 M" b6 y
- else if(HIGH_level_read_time >= 100)* r, H1 O; t' }
- {
3 ~5 B% e1 I( a2 x" [ - return 0xff;7 X t# m( h1 i6 ?5 y8 @* R
- }
$ K2 h2 a% }6 m9 o' a( _$ C* j - " _. h1 B1 x9 `
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)6 Q: w! z# X: L1 k' b1 a
- {6 j: u# @% X3 d" N8 W4 P% B
- return 0xff;5 O5 s0 B7 `( N$ M q; I) Z3 r
- }9 Q+ k5 W8 I' c5 U9 f- H
- }
8 A& f# J6 ~$ H% L - }# t- X8 Z3 J* O6 S; J
- Q& j! \- Z! A- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
' Y5 @6 h+ u: D8 d$ Q - HIGH_level_read_time = micros();6 \4 w" y3 e$ F8 w) k0 i1 e! F
- LOW_level_read_time = micros() - LOW_level_read_time;
% O3 z2 l8 H- v# Y+ R4 ~, {' `* d
, O; ^) `( {( T1 V% m) H) N# y- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ); }2 j7 ^# Z, W$ y" Y8 n
- {/ g% ?2 p- J9 p4 V+ I
- return 0xff;8 h6 b+ }. j \, s3 U; r
- }( S+ c7 s. I* v+ N+ F3 N
- : o) K5 n( ]- T. u0 f$ `
- pinMode(_DataPin, OUTPUT);- b d6 W: y6 o
- digitalWrite(_DataPin, HIGH);
; ~# A6 W' P! }' g5 s' o b
8 `' A; C% j' Q+ ^0 v- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
7 \& c8 m. ~& Q9 D6 i" {# l - {
, q7 e: v% C, H8 l. }2 b* Y - old_data = Sensor_Data[0];" E: s- Y! S0 V6 V' D- |9 N
- return Sensor_Data[0];/ S7 w, i: a6 e* [* Q
- }* E0 M1 c$ e( b2 K4 U
- else8 R( ~5 A% d+ o. h- t9 V# Q
- {
6 V3 B% B$ k2 X! n1 b( ` - return old_data;
, _+ n& `1 I2 r) M% L# Z* d+ e4 Y5 e/ } - }; G! h9 I+ \0 ~4 V
- }( |% r, q6 S9 E8 B# q& M
複製代碼 + |) P; K; R o) T+ e9 i' ~
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
" p& q% F8 B4 Q/ {3 c/ Y; E
" j0 w; u- X# U0 r9 K$ N4 P |
|