|
|
% }" x. ~0 f# S/ ]/ U8 f& u- #include <Arduino.h>2 C9 }) D0 J8 e# V
- #include <MeAuriga.h>
7 y5 P. p' I# { - #include "MeLineFollowerArray.h"9 h6 g/ R! m% t" K7 a5 H& ]
& J% C. P+ P) P l- MeEncoderOnBoard Encoder_1(SLOT1);
( ~/ @7 s7 T% e9 N; H( N - MeEncoderOnBoard Encoder_2(SLOT2);
: ?6 H6 h; v5 r7 R - MeLightSensor lightsensor_1(12);
3 E+ |- I5 e. v - MeLightSensor lightsensor_2(11);9 q% a, e% f R! o; d
- MeBuzzer buzzer;! T; m9 w$ U( r- \: L9 w
- MeLineFollowerArray linefollower(PORT_6);! S) D) w7 a- W: H5 [9 y# f
* F {/ I: }% J- #define Error1 1
( i1 a# B1 X! L0 Q$ z. L: `7 y - #define Error2 2
9 @: V6 \8 p. k. T" Y$ f - #define Error3 33 M% M. H9 s/ {/ ~
- ( F9 i% s+ ~) t) R: p3 W6 P
- #define Kp 15
4 L8 ]3 x; I. ? N - #define Ki 0.15, k/ r" J8 y3 Y8 z' V3 B
- #define Kd 0.03
, B0 N0 d: |8 z - 7 l2 |4 [' C. ~8 d! g7 t5 B
- uint8_t sData;
2 e7 m( s) I- L) u8 Q7 A8 G# n - uint8_t D1;, I# f- G6 M5 a8 H
- uint8_t D2;
1 A: v, Z3 I5 r: r0 y7 z# D - uint8_t D3;- \% c; h S3 K; ?
- uint8_t D4;
; c# D6 P% } d5 N - uint8_t D5;4 A$ f# U( v' l N0 c
- uint8_t D6;- r3 m5 E# H |8 K2 a% w% c" ]% E
" D# a- x& f# v. b- float previous_error = 0;
6 q% f! w# E% V - float integral = 0;
6 N( q4 U2 {4 I4 ]0 O9 w - float derivative = 0;% o2 ~ d1 c# s F6 q/ m' M
- int Speed = 160;
& p0 P# z! N" [ - float output;
& x9 Z2 I2 z& p& ]- ]
1 g* j0 D9 ^3 U- byte Left;
( `3 ~! B0 o7 s# y- ] - 2 |3 T8 p+ g5 C' [ j" U
- void setup() 4 i! h* i( v2 h9 |& A
- {+ o* f% _6 {% w- g- }- }
- //Set PWM 8KHz
# _* J) c' F0 u5 ]7 J - TCCR1A = _BV(WGM10);
2 h! a9 z9 o1 B/ V - TCCR1B = _BV(CS11) | _BV(WGM12);1 `! o/ l6 F2 Z
- TCCR2A = _BV(WGM21) | _BV(WGM20);0 O( r4 [6 b* V" O7 M9 ?% S" \
- TCCR2B = _BV(CS21);
: J# o6 Q O/ Z - Serial.begin(9600);
. \7 V( A0 U5 Y# V/ T! m - buzzer.setpin(45);
/ ~8 h3 h; o5 m# y - }
* c1 s3 m* U: k Q5 x
/ l4 ?! L2 G7 T5 ^/ F* ^- void loop()
- U0 g) a; ]" z9 X+ O/ \$ M: K - {9 E' c+ y1 F* J4 p# ]1 h) M
- while(!((lightsensor_1.read()) < (10)));5 d2 e. `$ | X
- buzzer.tone(1047, 500);
' C5 t; f/ D3 R" a - delay(500);( o) @5 a# U' |2 i3 Y
- do# H5 R( u3 |9 L0 Y* p. O! ^
- {- V5 U, N* _9 h' K8 L$ r+ `
- int Err = getErr();
2 t# B+ h, j2 w3 Y l/ r - if(D1 == 1)
6 M/ |3 ~( P: }! n - {
' V% r+ B3 S& b4 B/ V6 C - Left = 1;! M' T, |/ x! X7 U ]
- }8 T( g8 p( D5 I) B3 i# S6 T
- if(D6 == 1)! r6 z% d( I, _8 `! B2 U$ o
- {: f5 R0 q% C% m6 {
- Left = 0;
5 H9 f; H" m- ? - }
2 r. q4 \' |: j' N2 D - if(Err == 99)
5 a# {7 G1 c# Q- i9 L - {" L& G/ B9 o: V, v9 I' x" J# R2 `
- if(Left == 1)0 c! ~, W; b a1 U+ d/ I
- {+ s' X) k2 T* S. N' ^
- Speed -= 5;
! I! a" k' O: k) _& z4 i6 \2 m! a, } - moto(0,Speed);
R7 q+ X9 x m3 Q7 j# `5 H; D0 P - do
% |+ G- U* q2 r; Q& f# A- W - {$ g* @# w6 @0 L t& i$ G
- Err = getErr();
: _( l" ~& m- h0 `# h, } - }while((D1+D6) == 0);
. m* z# X! j) e' v - }& u. }. ^+ T6 w
- else+ L' u9 R/ c- u; r- E2 p
- {
0 h! X; O5 `, A5 l& p$ w - Speed -= 5;$ o9 U$ V5 p0 r( E1 |$ N2 y, M' P, R3 I
- moto(Speed,0); 3 R" D; r% k1 \ ]- C( U; g
- do- S7 I: i, \- ~ u' r. E, Y
- {/ O: g2 ]% p- G! v+ M% [
- Err = getErr();
2 W d4 H% \/ N1 H7 x9 l! V6 e5 I - }while((D1+D6) == 0);' @2 p" o9 k, g: m* c6 b
- }2 w3 @; y1 @8 |* L: r; i
- }
' E2 h) @7 L, r; [5 e* o; @ - else
- \: Z8 C! y3 Z7 q+ a& m _3 w' x - {
, d; T8 b# \- J) ~ - if((Speed < 160) & (Err < 2)) Speed+=1;
1 t1 y5 [% f4 r; |3 D - if((Speed > 100) & (Err > 2)) Speed-=2;
& ^' e! K4 Y7 L* B9 E# |( V& _1 W" U - integral = integral + Err;
& y5 p$ X& H& q C) v; n& C2 h: Q - derivative = Err - previous_error;
7 F7 i3 M2 f0 {$ o" U. @( b# G - output = Kp*Err + Ki*integral + Kd*derivative;( ]% E7 |! a z* R8 E
- moto(int(Speed-output),int(Speed+output));* v+ _+ h6 h' G6 W' V g0 b
- previous_error = Err;
2 k" h9 q5 i$ s2 s# M7 I t" z - }
" x8 O. N5 n. q: k. b0 `& M+ m - }while(!((lightsensor_2.read()) < (10)));+ s l" x: I: N: v$ Q g* h
- moto(0,0);) g: E3 n0 P9 g i
- delay(500);$ Y& s: [& S: {; w
- buzzer.tone(262, 500);
0 w& J( m6 ~; s0 N" m+ O! q - }9 s7 Q1 V5 c, F" j; U
- + a. ^2 q4 x; |6 j
- int getErr()
2 q9 G3 z" }2 P4 \. E8 C8 T - {
9 F7 Z' ]5 y9 d- j( B& ^6 [ - sData = linefollower.getValue();
Q/ a3 d; T! J; [$ b ^7 g7 E - D1 = ~(sData>>0)&1;) g5 u; r2 [' r# |0 S2 W) b
- D2 = ~(sData>>1)&1;
" q5 C$ N( \% W) D - D3 = ~(sData>>2)&1;: l# m+ w' {1 J$ ~3 y D
- D4 = ~(sData>>3)&1;
: |! B; a( _' l7 }) Q - D5 = ~(sData>>4)&1;( K2 x$ m& b v( K4 b) W5 c* r+ t
- D6 = ~(sData>>5)&1;, S, g% y( K3 P) p7 r% u# P! Q
- int downD = D1+D2+D3+D4+D5+D6; ~) h/ ?9 J) |$ s9 B
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);6 g& [0 m0 G3 r5 \) g2 y2 v
- if(downD == 0). U: N; A6 Z4 ^; |
- {% O2 G, _/ G7 ^9 s, r# Z
- return 99;9 i) y* t9 m' Q
- }* ?. n N; w( u6 c' o
- else9 {& M, v0 d- J+ ~9 x
- {7 b( w3 D8 v% N q$ M. N1 D. B( B
- return upD/downD;+ L/ L5 m! a% F
- }
0 i8 B [9 v/ D - }( Z* k! Z1 b4 ^$ U2 e7 [# t, G
4 Z1 |( |7 O3 Z8 k4 r% a- void moto(int Speed_L,int Speed_R)
/ B) W+ C Y8 Y1 z- ]* F6 D" I" C% n - {& S" t" A, c7 q5 U2 B F9 _
- Encoder_2.setMotorPwm(Speed_L);; q* z) S9 D$ h1 N6 c
- Encoder_1.setMotorPwm(-Speed_R);
* J6 ^: V5 v8 y - }
複製代碼 # E2 I: Z1 y% k% E
MeLineFollowerArray.cpp6 ]8 `0 ~' Q& k7 F# [) H
- #include "MeLineFollowerArray.h"% n9 c" O# u# Z- h
- 8 b+ Q) _* p& r8 U! m
- #ifdef ME_PORT_DEFINED
- _6 m& ?3 g1 H$ z, Z - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
Y' ]2 ], r' y. ?9 R0 j0 r0 j - {
& m6 n. r, u- |& G. w' P& k4 |# f
z# ], G" z6 S5 [- T- }
; f0 D$ c/ s& d D2 F - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
; s0 T1 w4 w5 m& t - {$ ^. T8 u2 T' e1 }! ]* G
- _DataPin = mePort[port].s2;
6 A' h3 D- e, s1 \ q" B - pinMode(_DataPin, OUTPUT); - ?* h J8 I+ l) p- _5 h) q& t
- digitalWrite(_DataPin, HIGH);
) v2 w' V' h( ] - }
$ B' P8 i9 l! Z3 V+ p/ R! G C - #else // ME_PORT_DEFINED
0 V& Q# [2 z/ l: f7 b1 m% T - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
1 V, Z# b3 r' o! @" c4 P - {
# N; r7 ?1 v, T0 X% r - _DataPin = pin;3 M7 b' @, V: H! f) X4 n0 m
- pinMode(_DataPin, OUTPUT); . i3 t" [- W6 ]; U
- digitalWrite(_DataPin, HIGH); P$ ?5 g; T/ z( Q3 l0 m
- }! ]: s6 `9 ~; E
- #endif // ME_PORT_DEFINED) c& ~7 `5 B4 l0 x5 t2 t6 s: G
% T5 K& Y1 B! U# H, F- & D& e; R7 f. k% b+ q" a
- void MeLineFollowerArray::setpin(uint8_t pin)1 m; q+ F' [$ y0 w5 r B
- {" d" r' l! s" y# x
- _DataPin = pin;
; U x- j: @& ? @% S$ v; k - pinMode(_DataPin, OUTPUT);
, S5 n# L; G/ v( B) [' ^ - digitalWrite(_DataPin, HIGH);+ h" j" a. u( }" x% H% g
- & \# B8 g! ?& @: s* }! b
- #ifdef ME_PORT_DEFINED8 P" Y$ M3 P2 u/ k' S, s! S3 T
- s2 = pin;
8 t' H% Z; J2 _" N+ Y1 | - #endif
& D; v3 `8 x+ u/ ~5 u4 O - }4 p b; V7 q* ]" U2 x* \
" @. b% o. S* O" P# ?4 x1 r0 q7 z- E- uint8_t MeLineFollowerArray::getValue()
9 h7 |( a8 Y [& E( G9 t- A$ } - {
7 f2 z! J# y9 z6 Q" }+ u - uint32_t LOW_level_read_time;3 ]7 y$ @4 O9 ~
- uint32_t HIGH_level_read_time;
2 ]0 ^7 g/ R' ^8 w S. w - uint32_t time_out_flag;
( ]/ v' i* r4 j - uint8_t Sensor_Data[3];6 P" m2 k& j, R; c7 Q1 _( ^* S
- static uint8_t old_data = 0xff;6 z, `# M" H6 Z* v1 @6 c0 U/ X
- # l* p2 q& l% ]
- pinMode(_DataPin, OUTPUT);6 S7 s2 N7 S l7 [9 G5 x+ s; Z
- digitalWrite(_DataPin, LOW);/ }! M9 @5 H' x0 n9 p
- delayMicroseconds(980);
7 X3 H" q4 m7 e1 Y4 X$ B - digitalWrite(_DataPin, HIGH);4 N# g3 M6 ~9 q+ p& D! J
- J" ^1 y( |1 }% b0 C
- pinMode(_DataPin, INPUT_PULLUP);
- m \0 z4 Z, a; f H' A- a1 l# D2 B - delayMicroseconds(10);
$ ?8 a: Q, F T! s
! ?6 g4 i. x7 f0 @6 s- time_out_flag = millis();2 q9 r7 y1 r% X. V4 ~9 p
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, k% c3 ^8 o5 J6 W
- 1 l# S' W/ b# i: `/ k! f
- LOW_level_read_time = micros();
: w/ O( \7 q/ Z0 u - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
) r6 E. t, q ^' Z2 d - {, |: U- D5 i y! h0 G
- return 0xff;
$ f( u6 c% {* ] - }( f# D' D" X9 k/ e0 j9 _- E; U
7 b6 B5 M& L! a' n3 T- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" w/ l E+ n: R' r, G2 i$ F
; I5 v' o9 E- V7 H. r* H- HIGH_level_read_time = micros();
( T% g/ \' H5 a7 | - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
: d0 d. v, w% p) p( A- o, q
4 W2 z& J# B, j3 J' ?/ ~- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
* i) T9 U) t6 ^+ k3 I - {% A5 k% ^5 `1 q4 D: I
- return 0xff;* i6 p5 d! h8 G1 i
- }
" z1 t* ]3 H* L4 B* w - # H: ]. g9 n1 j5 U/ [3 G
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
7 _3 H6 m& v2 k1 L2 A7 l% Q! B - {( m( S1 k7 x0 K2 N
- return 0xff;$ _' p4 p/ \+ n0 [$ p6 N
- }
/ m2 U0 m# S1 P# z; H - 6 o6 w, O( n2 R/ q, j
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% }5 {8 {, i6 ?; n - LOW_level_read_time = micros();
5 i1 j" q8 [( r6 \9 V - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
- g3 ?# B: \% ~4 t - 3 W' r- k k6 i
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out X4 p3 G5 W) `2 c D* @
- {
2 a! i$ l; U/ @ J/ q - return 0xff;- t3 W2 d9 f6 M' L/ B9 _0 k
- }( q$ L3 I6 A3 v4 Q
g; ^" d. a0 q8 g& W4 I1 X# |1 V- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))8 q' {! ?4 U% H) h f- \# N% f
- {
/ n* Z9 Z1 y' \ - return 0xff;# g2 F* l* n& |: \
- }8 T. N- {% K. R) s+ S1 U
- + t: }7 E) N3 ?8 `8 A6 ^
- for(uint8_t k=0; k<3; k++)
6 B4 n0 ~2 T0 y8 r - { I( E6 c9 W: v x& X2 G
- Sensor_Data[k] = 0x00;( c2 h) H! ^* K
% x& x- e# d; V' B/ M4 t& V- for(uint8_t i=0;i<8;i++)7 B9 s( [2 }' J( J
- {
# L6 f" P1 c" E Z& m$ F - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level" l1 V ^+ r9 B
- HIGH_level_read_time = micros();3 I2 n3 N! Q/ Q" v
- LOW_level_read_time = micros() - LOW_level_read_time;
) s6 i. X8 f S+ D" |7 \3 S
. p5 E9 k7 o6 X+ R' }" W! h7 v- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ): ~* _& [* Y% E* l' o& I
- {
( \2 [$ |* @" U& E* w - return 0xff;
8 ?4 ?# q! I- `2 O - }
* i7 E$ D5 {+ b( q, b/ D - , e a4 ~: \% o8 r' n- L& V4 G
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' x3 x6 d/ Y# q! c7 `4 v, w/ h
- LOW_level_read_time = micros();; \. F9 }- f1 V, Q
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level% G5 n6 W! V' l9 B1 z
- " A5 O. D5 @. t3 k U( Q8 ]* X
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
- n$ c' m9 X& e - {
6 \* X: I% k5 I4 t% J- o) ^. r; G - Sensor_Data[k] |= (0x80 >> i);! j4 U" a: K0 V. G3 E$ n
- }
0 W3 g+ }# O( G6 p" \& n - else if(HIGH_level_read_time >= 100)6 g0 v4 z. G7 H
- {
6 Z2 z) g8 m5 Q5 I0 I1 n - return 0xff;/ z0 S5 q0 G) `
- }8 H& s4 K+ w& P1 P
8 I8 K; M; q+ _; ]8 P- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) R, Y1 P+ \8 |0 V( g% c
- {% K9 y+ ^% o/ m v& K9 @% C6 T
- return 0xff;! S9 O( ^; ^; ?+ S" B n8 L! k* x
- }
/ t# B, N: W- i) a. C - }4 L' f7 ~4 L; |7 a% } h/ Y
- }
( _+ ]4 L3 {; F( j - $ g+ M2 } {1 L
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level, O+ e! p i8 G3 o$ n
- HIGH_level_read_time = micros();
/ ?: t# d" O, z: a* h( @, Q+ E- A - LOW_level_read_time = micros() - LOW_level_read_time;
1 N/ E2 M: C0 v' U
+ L5 l7 A1 B5 G3 Q0 N- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
% _+ C9 a" \" A: f7 f - {
4 _( w3 B. a$ l4 p. g9 O. A - return 0xff;& ^6 e% j4 o6 g0 i+ Z3 q9 O+ q
- }: @, y5 Z5 ^3 U* P
% M. T$ r* z8 b- pinMode(_DataPin, OUTPUT);( R1 c4 F q; p* _: k- J" e8 S, l
- digitalWrite(_DataPin, HIGH);( N4 N6 b& D8 `; ^2 `5 P* {
; ?5 \7 {+ }" f+ E( z' W2 B- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
5 T6 Y; l7 i- H7 m, f - {
W# g: r, h+ s6 O, A4 O8 k* [+ f2 P1 P - old_data = Sensor_Data[0];& `/ {6 [! n! W
- return Sensor_Data[0];
5 k$ A H% b% z4 _ t) d, Q4 n - }
! W7 f& d$ ?9 y% H - else/ d, [+ l" Y; T( i- X" V. M
- {
# E# v5 s( W5 y - return old_data;6 ?' H2 W2 l* s& H0 l, P
- }
1 w9 H/ `) t4 w, k2 E+ U K. T. h - }6 [& H* m: e' I, V: E
複製代碼 # e2 w7 _1 m. |3 {# m2 H6 E
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
/ O" c; h5 v0 _# g: y9 `: J* w& b
. Y7 e5 I3 }, J |
|