|
2 N7 x" p( ?% c5 v4 h5 h% R; a- #include <Arduino.h>
W, e1 S) O# P( z# s/ ?8 d' u - #include <MeAuriga.h>% e$ Q$ I) h9 o
- #include "MeLineFollowerArray.h"
* Q3 p. f: n; d6 w0 v - / m+ Y. Q; N9 A; T2 u
- MeEncoderOnBoard Encoder_1(SLOT1);2 Z( a- J3 i+ w6 _$ q5 g
- MeEncoderOnBoard Encoder_2(SLOT2);
* i6 ^6 k: y$ w - MeLightSensor lightsensor_1(12); q1 d, s. C2 C! m k# i7 Z! v
- MeLightSensor lightsensor_2(11);
& E0 t6 b% V V- z' ?; a - MeBuzzer buzzer;: h$ A( M3 i: z6 ~" P) Z$ g" x% {
- MeLineFollowerArray linefollower(PORT_6); h! ~7 H) o7 J' S G+ ~6 k, Y
; {/ i, ?3 r) y# s7 h- #define Error1 1$ H# b$ V" O- M" ~; R
- #define Error2 2
3 a3 e( K% Q) ? - #define Error3 35 {+ O1 O- k' W
- , d7 k' d i6 X0 _5 F
- #define Kp 157 S5 W0 ^3 q# s* Y+ z$ M+ j% O
- #define Ki 0.15
* m( N8 _2 j' _8 x4 B$ k* e/ X1 N - #define Kd 0.03
$ w& o6 n4 `: h5 i" W; v - 0 Y+ T8 a' I' E' J+ e1 w$ J
- uint8_t sData;
$ E0 n) E# }& E! R' h - uint8_t D1;% C0 y6 ]( s1 u0 l! b9 B7 f
- uint8_t D2;
' O2 h* @2 f9 r" l/ d$ f# g - uint8_t D3;
4 Q: o, X0 L* I* y9 b- U - uint8_t D4;4 ~) D" b$ M" C7 C
- uint8_t D5;# O( U; k. {6 A% T
- uint8_t D6;! S2 E" s1 f0 X5 o
. S$ f: ]& D. a& B- float previous_error = 0;
5 J4 i1 X8 |- t+ j$ [5 H( Y - float integral = 0;2 A" y7 C9 ~+ E- c
- float derivative = 0;6 q7 T; X% q3 Q6 G4 y/ N
- int Speed = 160;
/ Q3 x9 q! |( j& c) ]" j - float output;
1 q0 t( J* S! H3 k$ m
9 \3 u( ^4 {* |/ L$ n- byte Left;% G6 R* K3 m' x2 l& H
( r& t, b6 I# ]% l( U, S- void setup() - A7 Z, ?' m2 A2 D! W M
- {
$ P/ W0 _: J$ Z3 [ - //Set PWM 8KHz
: W2 E8 O+ L+ q7 S1 [, z6 Y( ~ - TCCR1A = _BV(WGM10);
# C' [: t1 a4 B; V - TCCR1B = _BV(CS11) | _BV(WGM12);
; W6 z6 E9 B y; [+ c; Y) ] - TCCR2A = _BV(WGM21) | _BV(WGM20);* b) e, n8 ?) p/ H4 M1 j
- TCCR2B = _BV(CS21);" E) V0 W, U/ I& G2 a/ K% |! E
- Serial.begin(9600);
! L7 L5 u) o" b; d4 Y5 L" e' m - buzzer.setpin(45);) k! y. G1 t* [
- }
9 r0 O( q* B3 M0 Y- r
7 W/ o& w' b% \6 o- void loop()
2 V" B- a: o$ a% s - {
3 J. t' z! q3 [2 `) A2 \ - while(!((lightsensor_1.read()) < (10)));7 g. h; X; h8 V. L
- buzzer.tone(1047, 500);
5 ~' U b* m& m9 X - delay(500);
/ ]# E7 e4 H" D - do3 N: B& ]2 x( D
- {
" \7 b+ k3 K' ] - int Err = getErr();
& I7 b# G8 p/ T( o/ g - if(D1 == 1)
! S) l: p* n1 q* k8 [' z2 s3 n - {$ A- { t$ W$ u$ W
- Left = 1;
* \9 `6 f: k4 K- g8 q- } - }
+ x3 R8 {, Z7 [* w/ L- y# O - if(D6 == 1)
1 h9 K& @% k s/ c2 e6 x( u - {3 F4 J: W) L1 {* h% ?- g4 i
- Left = 0;
' Y$ `/ n8 ^ H& J O - }
( j; |$ Q$ h; J+ a \ - if(Err == 99)
$ B- q$ z6 g: W' t# c - {
. A& W$ \; w8 [! J N y, { - if(Left == 1)
( O; T( s6 O; O5 t, J8 x' B+ {5 G - {
' s4 H5 m3 O6 V- R( i, j* F - Speed -= 5;
5 [0 }8 J5 ?, H P - moto(0,Speed);/ v/ s6 R+ N4 h+ ^$ A3 ^
- do
3 m! q" w& f5 U5 `3 E- d0 T: z, C - {- B# v+ I6 y: m& R
- Err = getErr();( H- n. ]: u3 ~( Q8 [# s5 ~7 J4 z' i
- }while((D1+D6) == 0);% U; z8 w9 U/ `. d6 x
- }1 f/ X4 q- L! C! [
- else& X6 C; r; k2 ~- \1 d% N! B% ?
- {- D. t: B, n% a7 d+ M& R
- Speed -= 5;9 I; P$ L* D/ `9 E% y8 F
- moto(Speed,0);
. g$ m1 t1 x0 }7 m4 r - do
' W3 k( ]5 G6 t+ I8 _ - {# D/ u4 ^6 Y% \3 U
- Err = getErr();2 W: Y6 A# i4 b
- }while((D1+D6) == 0);% C0 g- O1 c4 k
- }
3 r6 r1 o0 A2 f- m/ Y - }
. O) p8 F+ U, ~1 ~# h - else
$ O' O: H8 f. b8 @- n - {
3 p) A2 P* J5 @ - if((Speed < 160) & (Err < 2)) Speed+=1;
! s: i% |0 E: V - if((Speed > 100) & (Err > 2)) Speed-=2;
# Q( `& n4 {0 Q3 Z- E' P - integral = integral + Err;
) Z3 i9 I5 @# A9 D - derivative = Err - previous_error;
3 S0 t$ L+ h% W. V8 {# K* O$ L - output = Kp*Err + Ki*integral + Kd*derivative;- g7 L0 d& L4 v5 {& Y
- moto(int(Speed-output),int(Speed+output));$ \ t, c8 v: g/ K0 b
- previous_error = Err;5 K/ D* g. Y5 L( M
- }
% D, M/ S1 ~* d* z9 |6 f Q# X9 n - }while(!((lightsensor_2.read()) < (10)));2 r" S+ o9 D% w
- moto(0,0);9 h% s/ j4 a! Q: q \5 F9 K
- delay(500);, G4 y# T, C! y
- buzzer.tone(262, 500);
$ ~9 L! a0 _+ z& b( |! ] - }2 m0 a! P$ b+ ?' m4 p8 T
- 4 [5 g4 k% ~( s- m1 j& d4 C% L0 N
- int getErr()% Q9 b8 B9 Z; L9 q4 e, i
- {
: I- v o+ T8 j+ J0 D2 C3 y - sData = linefollower.getValue();6 ^+ {/ l" d. K0 B0 H
- D1 = ~(sData>>0)&1;
: M- P- [$ _& N* Y - D2 = ~(sData>>1)&1;' V" k( g! `. L1 d& l! M3 K5 l7 s
- D3 = ~(sData>>2)&1;
. D! F' {# _" ]6 h5 d3 i; _# M. h - D4 = ~(sData>>3)&1;0 S8 M" P7 e8 t: V3 U7 N. @8 S
- D5 = ~(sData>>4)&1;
4 U4 ] Y( ^7 ~7 V - D6 = ~(sData>>5)&1;
7 u1 I% k6 g; L8 C - int downD = D1+D2+D3+D4+D5+D6;
. C7 H- O* B- Z7 c+ i - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
0 w! d F0 ~3 I" N4 p. |2 c - if(downD == 0)7 x2 l- A+ S, O: n: W
- {
+ i ^( A8 D. I" j( R. }, z - return 99;
6 V% U5 \3 ], N+ b& D# m - }
* [ f$ U* K+ _" _; E - else. `) x: L" `+ N j* y G2 z
- {- E# g* f' k1 f/ Y% X/ K
- return upD/downD; c2 j9 d) B( k5 c
- }
, x9 z7 l, v% ]4 ~4 B - }
( \3 s# V" g& [' R' h - 2 U; O3 t [+ O! L# L, G
- void moto(int Speed_L,int Speed_R)( Y. }5 i5 C/ X- L5 |0 P! n
- {
2 [" Q) n+ S* J! f8 q: }! V( B - Encoder_2.setMotorPwm(Speed_L);# \ ]2 [. b% |4 D1 u" U( p
- Encoder_1.setMotorPwm(-Speed_R);. r$ L: |3 J+ _
- }
複製代碼
* q0 W6 @1 B" hMeLineFollowerArray.cpp2 u8 b% O8 I7 y, |! J: l7 P/ }0 N8 M
- #include "MeLineFollowerArray.h"4 `: e: u5 C' Y1 @% S7 s
5 ~) V0 Q$ t' v! a- #ifdef ME_PORT_DEFINED4 _1 ]! e/ S4 i- R9 L5 W
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
& ~0 D- f d! |; u6 c* U - {
6 Z; e- H5 r" z3 \$ x' J - , K V2 r6 v6 ~5 e
- }
- d' p2 V. @) Y% S# L/ ]: i - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
$ q2 n) A. z( O3 E - {7 a; v9 W+ T& J% J: @# j
- _DataPin = mePort[port].s2;! |& R8 E+ b) G' j
- pinMode(_DataPin, OUTPUT);
" K+ a$ p6 c1 u- s7 i6 h5 \ - digitalWrite(_DataPin, HIGH);
' @* k6 d3 F2 E" n; b) h N% N - }
& p6 a! U0 n3 G# Z/ D- P# S5 s7 X - #else // ME_PORT_DEFINED
3 e( o: z6 B9 a0 ^0 a' ] - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)1 E* Q& l5 V& ~* R1 g! k) [9 M
- {. x( V4 p* v7 c; i8 _& S
- _DataPin = pin;% N0 ]# Y4 `, l4 m
- pinMode(_DataPin, OUTPUT); 6 G6 u+ I1 K b
- digitalWrite(_DataPin, HIGH);/ ?3 H1 }) U8 S0 I/ j
- }
7 m6 V3 T9 T' a+ W - #endif // ME_PORT_DEFINED
: B* W) n' W# \3 o8 m5 T) x - 6 c7 a) r ~! [4 V( ^# Q
- + X* |* F3 N" ^, V. R, a. H
- void MeLineFollowerArray::setpin(uint8_t pin)% e% i& c4 ]. l
- {- u' s. n+ I4 X h; c6 {
- _DataPin = pin;
$ r2 Z0 q. k6 g - pinMode(_DataPin, OUTPUT);
( _9 z! ?! A. ~, s5 @ - digitalWrite(_DataPin, HIGH);0 M% a/ Y) b2 K/ S2 l& \8 b4 D
: e' I$ Q& `* c/ x7 R+ h7 G' e- #ifdef ME_PORT_DEFINED
5 K" Q6 _0 J3 o+ D( f! F6 I( @; W - s2 = pin; d. ]4 P! u$ `. |" V- G0 G: M, Q
- #endif$ `' f0 C9 {8 \3 [
- }
7 M1 X! H7 J3 z) X! V7 k
& l/ m9 U; R; e, U% Y1 ? K8 q, k8 [: G- uint8_t MeLineFollowerArray::getValue()8 Z1 y1 U! M9 p: g+ |
- {! k- d! i6 l" h* M; ]! o
- uint32_t LOW_level_read_time;
# R* ]1 X0 G: O" W/ M+ S - uint32_t HIGH_level_read_time;$ T2 t5 h% a" H8 r
- uint32_t time_out_flag;' Y+ c" }# _2 A5 q) \1 o" z3 a
- uint8_t Sensor_Data[3];2 G+ c* _! X. D }& ]- l( Y
- static uint8_t old_data = 0xff;: l) X; Z" v5 j Y r) g: f0 `
- 6 L) d. {# t% H$ H( l; Z
- pinMode(_DataPin, OUTPUT);
: m' ^6 v3 Y. K - digitalWrite(_DataPin, LOW);9 n& o9 T5 N/ k8 A/ K
- delayMicroseconds(980);
6 L- H. q& L) \# e4 [& @ - digitalWrite(_DataPin, HIGH);
7 h4 m2 ^4 ~2 U+ f) U0 N# [
& |6 G( p3 k5 \0 e& A- pinMode(_DataPin, INPUT_PULLUP);2 o; {" p8 u# |( b4 `6 q9 ^
- delayMicroseconds(10);
0 p Q% |5 J- t) V' `- ]1 o
" Z% ]5 W+ V! E4 N2 r, H( l- time_out_flag = millis();; f9 X# e: Y% Y4 B7 m& t
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );& q) t* \2 Z' j0 l/ ?, Q
1 u) e7 j: r5 e+ k- LOW_level_read_time = micros();0 q& C, U T y, e% Z. R8 X
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out1 ]& a6 s! }. S4 Z f- G
- {
9 {5 ^8 w+ t2 C9 ? - return 0xff;
: g1 \: V$ w( W' X: B; o - }
0 c2 c. g Y# D/ B1 u$ h& g- | - 7 s0 L# r% a% n$ O* D
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );2 Y$ v1 d% M% s; @+ a: X# E) e
- " s7 @' P& J J
- HIGH_level_read_time = micros();8 u# F/ w" L5 _
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
2 b6 v& S" T$ f/ k
5 m! g+ W* _& w8 [" l/ ~8 D- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
{2 b0 V9 `( H6 b+ a - {7 ~" R( M5 E C( G0 b" c
- return 0xff;! H* S9 k) P2 K7 c4 s5 d
- }
/ ?1 z- d- B! a' @) a - * ^- l- ? |7 w1 I; w* x2 Z8 E
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
" I1 P2 q1 R R# Z8 a( A - {; A) H% M* d I8 a/ E/ X
- return 0xff;
0 o6 d9 V. C% @4 p* T6 J - }
7 l0 L5 U; i$ K: v3 d
5 F1 p' r' w% r7 |7 }0 B F2 U) [- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( V, }: u! |! t' i
- LOW_level_read_time = micros();
. s0 {2 k$ M) [& t& ~5 T2 ?% W - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level8 b- R8 v' A# l! Q
& u" L" t, a' a4 M, K U& m8 b- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out7 W: a9 Q' G* Y2 e' L
- {$ y/ d' m6 ]# [5 F
- return 0xff;
' w$ e6 D S4 J6 J6 N1 ]( K1 h - } ^- M# G; p& ^5 a, Z* D {
" R% T5 X; v. N! [& g4 c9 x- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))+ }/ D# E% j/ n' b
- {
0 p8 [( ]( [# l; m# C - return 0xff;
, i9 J& q* H9 o. f$ e: ` - }% K8 s- m& @* d, A4 k
" j( b" \$ u* ~0 M$ u/ s- for(uint8_t k=0; k<3; k++)
8 D7 r3 d' M) f: e$ ?2 O" _* W* h - {
& w- @6 L( {. D. {/ D) h - Sensor_Data[k] = 0x00;) u8 d: m M8 }( D9 n4 P
- 1 y0 y0 M# K' G6 }, `' G, h! k
- for(uint8_t i=0;i<8;i++)7 e5 |; ^+ s+ i8 E9 }
- {+ }+ o+ Z6 R1 Z3 @
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level6 m; h& J& f- ]
- HIGH_level_read_time = micros();% M! h' r( K' Q( E
- LOW_level_read_time = micros() - LOW_level_read_time;
2 _) J) o* Z& C. }$ ~: a9 Q9 o - ! Z5 Z2 W4 v: w' n
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
/ ]6 y9 ?+ v: S3 L; S - {
2 [- n6 V# ^5 W+ D8 z - return 0xff;
' g! T/ }6 h j( w3 M - }
! l5 o/ L4 V# ]' o4 _ - - {0 c, `( c5 e& B4 ^- _& \. k
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 G6 H* a) J, n3 q! C1 S7 U$ N
- LOW_level_read_time = micros();
' m1 Q8 \, C$ Y - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
" e& I$ ?9 x& V/ g$ k* o5 z
K; [4 a1 f7 A5 i. z' F- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
7 z1 X6 C+ L) o2 F' d* I4 E - {7 c: z' Q/ A/ ~& h3 L/ ?
- Sensor_Data[k] |= (0x80 >> i);3 h3 @4 k, m5 C9 E. g
- }
. X# \/ G9 V6 n& @9 l8 {% v( ^ - else if(HIGH_level_read_time >= 100)" r: {+ [( _- C' t% X @
- {% u1 C! U3 I% `8 T4 z
- return 0xff;3 S' W7 v( @: R; S; C
- }
4 ^! B" }3 g4 {: Y3 N - & O6 M3 N0 {* f3 X
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)& }; u4 W% K) z2 j8 Q
- {
! D/ g) g" k! l) B) P7 b - return 0xff;
! g$ }0 i1 B2 |$ m) f - }" I# \1 J+ c2 w! w) q# W, A1 I
- }0 D. u" I- } l9 b3 ~0 X/ X
- }7 b% m: V; f2 _; z0 \+ M
( o- |7 V% ~" w6 A: i) z9 ~- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
( t6 {* Y5 [% E5 ^ - HIGH_level_read_time = micros();
G3 r7 @" X V5 c q! K0 L; K' t - LOW_level_read_time = micros() - LOW_level_read_time;
4 [6 u) W! Q4 _ O& V' Z
- W" Y/ G1 Y+ d& O/ ~/ _- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) j. y! Q* M6 K% S
- {; m2 S$ j+ ]9 l% ?3 K( b; z ]
- return 0xff;2 C- }' r$ I' }( {2 u
- }
; C- j* f2 a* O/ o
; Z3 t$ C! o6 `. x. J, b- pinMode(_DataPin, OUTPUT);" P% V5 }2 G- ^. A7 E4 c8 |, s
- digitalWrite(_DataPin, HIGH);
+ Y; a1 ~! i: v8 s ~+ g: h - ; P! o+ y2 l7 i& z2 u
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
' L, {$ M" @2 l: y7 Q3 M% _+ a - {. h" h8 J$ K( ?1 C
- old_data = Sensor_Data[0];- T5 c8 L0 n% y4 x
- return Sensor_Data[0];
, v. ~2 [; [; o7 s/ P8 I( H; s - }
% v. h5 _8 S% F& |. p - else8 U0 A/ I' B' f) M% v
- {
4 c: I% f$ P# i. S | - return old_data;
9 V/ n- ^& j7 c0 g8 S7 ?7 P - }
2 n3 S+ z4 I+ ] - }
& }' e( W' @& B8 U
複製代碼 4 w' O9 `6 S+ [/ |" ]7 h
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
) A: U4 B, _# K y2 e
8 `3 r9 y/ y& |9 p |
|