|
|
8 j5 Y1 \5 H. f' F* {; C- #include <Arduino.h>
. C7 z, ^' f+ W9 z4 H5 H: C - #include <MeAuriga.h>% @8 L q1 x6 Z( y9 z& X
- #include "MeLineFollowerArray.h"
$ A4 C5 z6 w# x - , v) k- m% i; ]4 q/ { f
- MeEncoderOnBoard Encoder_1(SLOT1); x- q5 \& B3 f* L" z' j5 j
- MeEncoderOnBoard Encoder_2(SLOT2);
8 V4 L0 Y6 z5 N3 |6 p7 T - MeLightSensor lightsensor_1(12);
$ M, _( Q8 @4 L& h# { - MeLightSensor lightsensor_2(11);
' F- F9 x0 b9 A - MeBuzzer buzzer;# }0 _+ p3 T1 s* O. r
- MeLineFollowerArray linefollower(PORT_6);( l* g0 y8 Y0 o. s, s
' W* C1 W" L3 V# y1 F- #define Error1 17 b; _9 A E( M' u y) \+ W
- #define Error2 2
4 D6 b) s7 Z* r6 B, m - #define Error3 33 Y" B9 p/ h# K5 E9 J4 g- v
- : J h7 I5 {9 q0 @8 p. {
- #define Kp 15
+ [ W* X( J4 p9 { - #define Ki 0.15
0 n- m" E$ F# f' R4 _( @; r! J* Z - #define Kd 0.03: i6 j0 P, E& g& K
- ' a2 A& D5 `, V) `* q) X6 C
- uint8_t sData;, t/ D5 [8 S W7 V, R
- uint8_t D1;+ O6 l- R" w0 f8 X. }$ d( U
- uint8_t D2;
3 d( p. Y2 m7 o' x - uint8_t D3;
+ m" W B' n% T4 Q c - uint8_t D4;6 ~, z9 @# P/ Y" q. [
- uint8_t D5;8 Y; s6 j- M k1 v
- uint8_t D6;3 {( f# K$ F3 r, u
- % L% o, G4 s' |. G% v# ]2 R
- float previous_error = 0;: d1 v( F- l3 {+ O7 l
- float integral = 0;
9 [( |2 {) r3 e* M- r6 E - float derivative = 0;) d. y& D$ q5 m1 s( F- w
- int Speed = 160;( p' y6 S2 e h" T
- float output;
, q* t7 n! z2 P
# u. E5 p% R& J- byte Left;
" u7 t$ a$ ^2 x( D
: G, ]7 p1 o/ ^3 W# d" o4 ]- void setup()
2 P6 [5 m5 N5 p- C) M - {* b& V- l2 J9 D& i& E& C d! k
- //Set PWM 8KHz$ O) E! z) Q6 s7 K
- TCCR1A = _BV(WGM10);0 @( @4 q( Q+ k& ?) g* ?: n. s
- TCCR1B = _BV(CS11) | _BV(WGM12);. t+ t' [$ _8 I- b6 A3 h
- TCCR2A = _BV(WGM21) | _BV(WGM20);$ g1 t! S9 }2 k* @. \9 M$ q
- TCCR2B = _BV(CS21);
& H& G; a/ h) m/ h# y, j8 K - Serial.begin(9600);/ C; [# Z3 k7 q" y; b
- buzzer.setpin(45);: U* M2 @. f2 o" C! ~$ b
- }
! o7 s1 S" `( @% ` - h# D% J$ ~; Z; n/ a
- void loop()2 L" t! {" R( W& ]
- {7 }5 D1 o. O, a) V% G* d7 {" f( E. \
- while(!((lightsensor_1.read()) < (10)));
0 I! \+ Q& m3 n0 v9 s - buzzer.tone(1047, 500);
4 ~) }2 ~& J1 G' { O - delay(500);) N* A* _! K: s+ h2 O1 _
- do
0 \" _+ G2 N1 T6 s - {0 `+ a0 Y# A% G0 @6 O
- int Err = getErr();
. \2 ]- O8 Y) t! I; _ - if(D1 == 1)
* D1 ~- o" N# V: q' A% U- ? - {
) s: i* P/ ^, ^/ K4 @* _ - Left = 1;6 I( P) Q4 L8 ?, G# r4 G- y
- }# N* y) L5 b) e( r7 ~7 C: |' Z3 }2 t4 S
- if(D6 == 1)" T- H2 d. U/ f t$ a) V$ a7 `+ O
- {
! g, l4 V/ s/ {" ` - Left = 0;6 W4 s3 W) g; r6 o! z" P& V
- }
2 @: s* c. h# q; D8 h* d - if(Err == 99)
! b; e" K6 ] F - {4 @ [# z8 a& z' y* L; g' n
- if(Left == 1). m, A% V. X; u C, U1 p
- {
5 q! r! @3 ^) X8 { - Speed -= 5;7 a4 M' h: B# ?2 R8 G: z6 P
- moto(0,Speed);
( V8 j" [" X8 G, Q4 b9 X2 h9 ]) O - do
) q. Z1 W8 Z6 F( D: O4 _ - {7 B1 a: `8 \! v, \% T
- Err = getErr();. G- ?. B$ T( Q& j- D
- }while((D1+D6) == 0);* F) L$ m$ G6 M: Q; U. q) S
- }' a% E$ y2 E b& ]9 o
- else
' f, f. ? n4 S! N2 F' _ - {2 X ?4 z; Q6 s8 R7 O) X
- Speed -= 5;
2 W; L: S( a! x5 }2 ?% j: q9 e - moto(Speed,0); / u4 X. m1 x' L/ M% H
- do
u) h) W( T8 _9 N - {
' A: e7 a+ q) d/ u. J* E5 S - Err = getErr();
' V6 Y7 j) W) ^, Z4 B/ q2 c - }while((D1+D6) == 0);8 Q$ O5 o0 H, X. W& G
- }8 y `% }0 H2 u
- }
; f2 c& W9 f4 x$ |3 O' ]% W - else; G: |5 u5 G$ ?5 u
- {) i- J: `% Z, z, ~/ {2 j3 r6 R9 a* l# y
- if((Speed < 160) & (Err < 2)) Speed+=1;
. g; y$ X! T% g- r4 \$ D5 t - if((Speed > 100) & (Err > 2)) Speed-=2;
& H$ T% i' P1 R - integral = integral + Err;0 m( t Z) z$ H" b3 U
- derivative = Err - previous_error;
' g2 @+ M8 R9 ?6 Z - output = Kp*Err + Ki*integral + Kd*derivative;8 r+ |% G' @- e3 D. c
- moto(int(Speed-output),int(Speed+output));
0 h8 q7 D3 R3 c: I0 O9 v% k - previous_error = Err;
& {7 H# q- W. F' E; s - }
L! g! a) l& H! ], W* I - }while(!((lightsensor_2.read()) < (10)));
G1 G- ?. _5 Y/ `* I - moto(0,0);* E A7 M5 U6 V4 K
- delay(500);' }4 f1 x3 R! `* c% W) h+ b
- buzzer.tone(262, 500);
. q: c" V' Y j* d' R4 F# C - }% v% z7 T( ~& @2 z7 y4 _# Q
- $ I; E: L& X. N+ B2 p2 g; ?( `" w
- int getErr()/ Y% @4 r" Y' n- t; A i- ]# v' f: ]
- { + e4 x. n! S; l6 S3 a+ o; O
- sData = linefollower.getValue();1 ^+ _' r }" l7 w" F
- D1 = ~(sData>>0)&1;5 X: ~9 N3 F7 i+ H/ W7 s% N; A
- D2 = ~(sData>>1)&1;
& I; O8 h" a9 d5 ^ - D3 = ~(sData>>2)&1;' p! W* s, i. G, \; ]+ ~- @
- D4 = ~(sData>>3)&1;- Y8 T0 c- A+ Z2 ]5 r- i6 c p
- D5 = ~(sData>>4)&1;* ]! n% x. \2 S
- D6 = ~(sData>>5)&1;. t2 C% i$ ]: K. R9 y$ k6 f
- int downD = D1+D2+D3+D4+D5+D6;
% i A5 w% J* [+ m, H/ d) y - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
: T, H; V% }- P! O7 o - if(downD == 0)' ]! w! E0 V3 n0 X
- {: N- |# c2 c, n; a$ }) ^" M) M2 h
- return 99;
+ y! P* [( e" I - }
* c$ O' u# O) [& v5 m$ c k - else# W2 z& t% ^) ?2 }8 P
- {- ]. Y2 i) T7 ?' x' f: R4 ]# R2 l+ ]
- return upD/downD;
. M( n2 j. F: L( e K# e5 j5 N M7 E - }& W/ C6 j/ f* [" Z$ j( \" j1 c
- }
2 T- Z6 q ~! H% R6 E! z! V/ p( j - ) C% d2 i+ W) o; P6 J
- void moto(int Speed_L,int Speed_R)- z' D, w [8 k1 Q% z5 b
- {4 X L- C& v, W9 ^# L$ `- F3 y
- Encoder_2.setMotorPwm(Speed_L);
' m P$ N5 Q6 g* ^# V$ Y - Encoder_1.setMotorPwm(-Speed_R);, R+ C5 E; F2 {: s8 e
- }
複製代碼 0 ]6 P( h5 R/ U1 C
MeLineFollowerArray.cpp/ h& R9 v6 E0 ]8 g! ^
- #include "MeLineFollowerArray.h"5 u" a6 `* H+ n4 E5 a. }
- 7 O6 T; M9 S% z6 i
- #ifdef ME_PORT_DEFINED/ I3 a$ M+ [& X O$ f- k9 l/ Z
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
7 K; a2 p4 F/ c - {/ ^; z- A1 T; \7 b; j
- 4 f6 d* Q- [' C5 D: K2 G, p w4 S
- }9 Z1 O- A/ |/ q4 @! F/ m+ f( G
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
% J8 m& m g) d; Z0 S+ T - {
& w7 B& q& I2 X ^7 p" w! D( J2 O - _DataPin = mePort[port].s2;$ A; E7 {0 c# E4 }: s. `! @
- pinMode(_DataPin, OUTPUT);
9 v9 `. X( {' ]8 y2 ] - digitalWrite(_DataPin, HIGH);6 K0 E( L- @; p9 Z) J$ a
- }: Q# p F! U L. r- Y4 g& Z1 q& u4 _
- #else // ME_PORT_DEFINED
; I' U, a1 S+ ~$ o+ X - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)5 t5 l& W, e# Z. J( Q
- {* A1 t3 a; p) b, W% w, ?$ m `# }7 i
- _DataPin = pin;
9 O/ J! v4 a+ F - pinMode(_DataPin, OUTPUT); & `! {% J& @; y7 `# [# _, f
- digitalWrite(_DataPin, HIGH);
# E4 {/ F9 o% {* \# \: g+ o/ ` - }
+ q" @0 ^ ?4 N y - #endif // ME_PORT_DEFINED5 f# y: `+ s" V; V5 P5 e* L
- 7 E# J' }* A8 A5 d! w
- 5 H- N: V2 q$ L* k/ J E7 h" v3 _: A
- void MeLineFollowerArray::setpin(uint8_t pin)
; o$ }& W' W" \, t - {
& J, O' { Z! H6 B! a9 t - _DataPin = pin;9 P& l+ c' e; r+ b
- pinMode(_DataPin, OUTPUT);
+ I4 F4 v& Z B1 F - digitalWrite(_DataPin, HIGH);9 T5 d. s- \; J! N7 [2 i) k) ^
( s1 |( _/ ]$ b5 I- #ifdef ME_PORT_DEFINED7 D I0 {4 D( L! O% L7 w, I
- s2 = pin;
8 V) X* k( {. F# {& k! E7 S - #endif
/ V: V, o! E: f+ Z8 Y3 K - }
7 {% V: W4 x5 q& Y' x S - / K8 C/ B L8 ]; \3 v4 G
- uint8_t MeLineFollowerArray::getValue()
- m( x/ ^6 U2 r/ \) i' P3 ~ - {
: Y: ]$ F4 `( S7 l0 W - uint32_t LOW_level_read_time;" W; H4 [ q+ ^4 l7 Y0 [& R; F
- uint32_t HIGH_level_read_time;
' E/ q& V. ]' Q4 i0 o( J; } - uint32_t time_out_flag;
1 J; h: G! G6 O. R w - uint8_t Sensor_Data[3];
- Q% B. g& n" K" Z5 p& V' v - static uint8_t old_data = 0xff;
; K; e- V% N* @2 z3 R4 c - % C! f8 {+ Y4 b( ? H" w- v4 M! l
- pinMode(_DataPin, OUTPUT);3 S/ J$ j& K0 I; u
- digitalWrite(_DataPin, LOW);
+ | S( ? a- f - delayMicroseconds(980);
0 L# J; \! D! [9 {' k - digitalWrite(_DataPin, HIGH);
4 V* M' s \2 Q+ ?# }7 s
* t0 L; d& S# V' Z- c- pinMode(_DataPin, INPUT_PULLUP);+ Y! V9 [$ Z8 a" G) W0 Q1 m$ L
- delayMicroseconds(10);, e& L/ N* r3 r" b Z* Z) m# G- m
/ V& O- r# X4 l! N P- time_out_flag = millis();1 T/ M }" i s5 V3 r6 f, Y
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 j; N- C' ~0 i7 a5 z3 o' L$ e
" f6 _1 J# @ u# s& k! ]6 a& J- LOW_level_read_time = micros();
1 X+ e& `! G0 t - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
# N% U& u" S* g3 q3 f7 f) k2 f - {2 k# j1 W$ M. v" e- [. F
- return 0xff;: v) |# Z. ]4 Y0 ?3 I5 [9 j
- }( q5 z# H% K+ @* p4 {0 \- c3 @+ \7 h
- # d% _ a7 W/ _ s3 {: ^9 ?) s! X
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, K2 R( ?' u" K7 l7 r3 X6 X
- 9 y, |, G2 W, l4 Q' l$ Z6 }; P
- HIGH_level_read_time = micros();
$ j3 @5 \, G3 G. k9 v7 T - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level, i1 { I. j! |3 L
9 U r* n8 t$ e: ~! @* y o* k7 j- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 ]+ t' T$ A* ?1 B* o- H) I: ] - {
- L/ R& z ]; V7 ` m4 u- W7 e - return 0xff;. z1 y' _5 e; F! X, O7 x
- }3 O; O. Y3 t1 i$ t) x& f0 d( h
- $ s% ^& M" b% j' D3 ?0 `
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))8 Y2 O& Z P4 p7 z2 C4 R% R
- {
# V# B, S6 f ] B k1 r7 F/ ?! C% X | - return 0xff;
' C& Z$ i1 U, Y& U - }
) r' q6 M+ S% M, l; b
0 I. T0 d5 w3 Z F- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );/ I( ?$ B6 b* q" X) A
- LOW_level_read_time = micros();
- M ~& x5 R4 w% U$ g& C - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level. Y" ?5 F) K- f4 Y- j9 Q2 u
- ; H% P. _ C! w2 ]2 Y3 R0 {
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
9 v' v4 n, u" n8 o4 v/ i - {6 z; R/ { ?; u: M0 ~7 U
- return 0xff;
( s4 b0 B* d& T ~ - }
" i# q( N2 a# U# z; ]$ i% X W
* ~) A* Y( z9 U% C8 Q/ V- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
! w# t2 b; B" ]5 _+ Z7 O' B, p - {
. A* g7 J* J: U9 J- y2 E - return 0xff;
Y& L" N/ a3 x! L! [ - }
, x# f& ?" J' K. }) R2 i - 4 l* K& a+ J7 s9 k: ~ ^+ f
- for(uint8_t k=0; k<3; k++), s9 x: ~ l( P
- {
4 v1 g2 ~1 ~6 e" q9 ? - Sensor_Data[k] = 0x00;! B. a1 B6 V( _
3 y4 ?- J1 I8 g; J( j, g1 i- for(uint8_t i=0;i<8;i++)
: t/ L, ~: ^: @/ g; b: m - {' l u6 T e) x7 }
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level' Y: s5 X9 j4 B
- HIGH_level_read_time = micros();5 c2 ?/ p* k4 P6 f
- LOW_level_read_time = micros() - LOW_level_read_time;
% m0 z" o" f5 [) j. L6 r/ P; O
7 i$ P6 A% f2 ?" {- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )' N; [6 d ^0 D! c) ~9 ]& G+ b
- {
7 Y3 O& D9 ~8 e/ c: G - return 0xff;
: K! r2 e( @2 V' u - }! @+ V+ G6 \( I) m3 l' {( ~
- ; V# q/ T2 c( A
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 l$ _$ k. A K8 ] - LOW_level_read_time = micros();5 r/ e( R" C% F$ x6 x! G" W- M
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
2 o' [& x, J/ a" x, F# X9 u
9 l1 d0 G. B8 j4 [- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
, ^8 H( ]3 o' T3 C, t3 K! v2 y - {4 ^ z1 N g. S& V& K
- Sensor_Data[k] |= (0x80 >> i);8 H) O3 @# [- O. s1 B
- }' s# `7 C: C$ @7 v, G/ L+ x; s
- else if(HIGH_level_read_time >= 100)
6 b8 l1 C% I( m7 Z' ?1 o - {
9 W' t1 f5 p6 e9 J - return 0xff;1 M" a$ i5 C' Q0 Z
- }
& ^9 g* s. P1 `! v& K
% l5 `* ~. q$ W% g5 L6 L. b- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)* x& ?& j% O/ B0 ~( U
- {
5 f& T ? w; j$ q8 D! |' S - return 0xff;
0 u7 D/ L/ c+ e - }! ^' r! n6 K1 R" W4 J( @4 V7 X
- }
& A7 r) `1 C$ E! i) K - }
" n: g, k% Z" F7 e+ o6 E0 s5 M
. d6 |8 [0 y2 k Q- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
1 S; E9 Q: j7 x; H - HIGH_level_read_time = micros();
4 j3 O8 p f- z+ m" G- Y - LOW_level_read_time = micros() - LOW_level_read_time;" p- w: Q. ^! \: h
J p+ U1 G/ b- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
+ G& Y* [5 y3 m - {
2 r P' o P$ a3 [- R5 S - return 0xff;
$ U* P- Q7 X) O4 B( u) h1 s - }
! {* T3 S5 {- E - , {3 ]" [5 V" v5 e# j1 e
- pinMode(_DataPin, OUTPUT);3 G- ^8 m7 _8 t7 y( i
- digitalWrite(_DataPin, HIGH); ^# S, `0 J+ ^. G8 Q/ J
- ) G: z. I/ e0 F+ q) Q
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))/ I, V- F8 g" n3 h( z7 g5 F; n
- {4 t7 E) S. ?/ i) p
- old_data = Sensor_Data[0];: a. ?/ N g% n3 j1 @
- return Sensor_Data[0];
1 M' d- C) C5 c' j" B - }
5 `- P, K5 S6 c+ u: e# f& ^ - else! x/ c( n! Z; c
- {" ~! q. H3 E+ z& B* x% R
- return old_data;
" a2 C7 L4 P. \* v# C( z - }
; P' Z" E( ~5 I( C - }: F- `! h* j2 C- j) y
複製代碼
5 r' d9 D$ S' N3 TMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
6 H5 b) {0 v# L5 j; b* M2 X
@6 E% ^+ [2 p9 d g |
|