|
|
9 H4 o0 U6 h+ N5 Y* x- #include <Arduino.h>8 V4 _4 \3 C; Y B9 O, j' n7 K
- #include <MeAuriga.h>
* k. J8 p9 `8 l - #include "MeLineFollowerArray.h"% M Y4 d8 w$ @4 m1 V3 m
) Q7 l" t- ]4 z. w6 b6 @4 n- MeEncoderOnBoard Encoder_1(SLOT1);( ~: u- z0 U. a1 Y; r2 a( I
- MeEncoderOnBoard Encoder_2(SLOT2);- v0 J i; _+ [
- MeLightSensor lightsensor_1(12);
2 e/ q' X1 C5 O& L - MeLightSensor lightsensor_2(11);
) {7 }( s I4 b- X1 M - MeBuzzer buzzer;
0 _5 \' H |! V# h9 O - MeLineFollowerArray linefollower(PORT_6);2 i Z8 }0 _0 `/ y5 X6 N$ n
- ! A/ y$ n* t. f, T
- #define Error1 1( e6 M1 R; r7 V( B |; o
- #define Error2 2
7 C d* k* X! V! ], j" T1 ] - #define Error3 3
8 Z7 `8 |# |* z. ^9 U - , ^- i8 O9 s; [' T# A
- #define Kp 15
. z8 t4 T. f* c* x; {6 V& }) _ - #define Ki 0.15* G% t" ]5 S: `9 ~, x1 ?9 I
- #define Kd 0.03' y6 n F+ N3 A2 @' E4 I# N; ~
- 3 d' ]! h" u4 e- b7 _; g& T+ t
- uint8_t sData;
' J: ~: D4 O, `* C. l! \1 F! X - uint8_t D1;4 {5 _) _' y: C' Q8 | Q
- uint8_t D2;
! `' T q( o- L+ g) f" _3 m - uint8_t D3;
9 @) r& _" ?- M p - uint8_t D4;
; o/ A) ]) A, G - uint8_t D5;0 R6 J, P) m) {/ e* z1 k" ~ o
- uint8_t D6;
7 U" L" r4 r3 A$ S6 c& M - 6 }3 s0 U5 x0 L1 W
- float previous_error = 0;
$ m) _ e5 T J - float integral = 0;
6 ^; I! w9 ^3 |9 Y - float derivative = 0;7 b6 w0 O" i$ S/ f# O
- int Speed = 160;
7 R) T, g% X( L, E - float output;
! S; @7 A' e% y; ^ - 6 H' Z. f2 h9 R9 M- o
- byte Left;$ e6 W. k* ~7 ]/ i7 ]9 D
- ^$ N% t7 @+ s0 _- void setup() / T3 j$ l( J. L" d
- {
$ P7 O5 `7 K4 p - //Set PWM 8KHz* d7 W$ d' ?5 y2 Q9 l0 @
- TCCR1A = _BV(WGM10);
' K# {2 [2 |0 N: Z: |* A+ _ - TCCR1B = _BV(CS11) | _BV(WGM12);+ H9 t6 O; H3 {7 `, z2 j& r3 d4 A
- TCCR2A = _BV(WGM21) | _BV(WGM20);' r: w3 u% x7 x
- TCCR2B = _BV(CS21);
! d/ T% o+ p- I6 r7 r - Serial.begin(9600); ]4 T: o+ l- c; i+ q
- buzzer.setpin(45); e, x# y" r' ^4 a/ c# [/ g
- }& X0 O/ e- o, P( h! T
- % K' c7 d$ n5 ]: R( A/ a2 Y0 x
- void loop()/ A5 G) f" Q$ z6 ~5 V- Z6 T4 y
- {
- B( c9 }6 s: `9 s - while(!((lightsensor_1.read()) < (10)));+ j: v: P$ s1 X7 v8 l. ^
- buzzer.tone(1047, 500);* y1 z0 E# l2 W( X* o! {
- delay(500);
6 u" q6 N" {, v" B - do
: k: d' v! w R/ a - {' g; e6 ^1 f u9 y8 _
- int Err = getErr();' }: j1 p& [; L- |% k u
- if(D1 == 1)) }& Y/ \% r+ r. `7 Q
- {
G7 [' K; o/ q - Left = 1;+ S7 v n9 I5 W/ ^5 ~0 Y
- }
) x3 ~+ m0 R* f c( |9 |6 J - if(D6 == 1)
0 a; ]( g6 g. f6 ^/ N* O6 | - {
& }" H4 L9 t3 p - Left = 0;
/ k* G9 G2 y# ?) q1 f8 H2 \: ~5 J& ^( \ - }
7 b3 |8 p/ k; B; K3 D - if(Err == 99)% n8 Z1 T0 m- l
- {$ V* v$ O; F' p N
- if(Left == 1)
6 m( M3 j5 _, u" k% u - {
1 e; r& M( @: M - Speed -= 5;4 g# |8 b; v) r( `6 e. Y% _; z
- moto(0,Speed);
& _9 A$ ?* W9 W) _ - do
! V R/ V( B4 Z# W0 I - {
3 ?. U" [) g+ P+ Y$ ^7 j - Err = getErr();6 t, Y. z0 F: X+ l) \$ f
- }while((D1+D6) == 0);( h, w0 l K4 g3 G3 P
- }- n% S. b" y/ _1 D' h
- else2 e J0 R, L* K9 Y
- {1 w; k5 w$ y" t+ v. c+ g- g
- Speed -= 5;9 b( j0 S& m- f0 M A# M6 F9 r* m
- moto(Speed,0);
6 Z7 C3 W$ B& Q/ {- j - do
/ R: I3 c7 H6 u" }5 E) N* w - {
* l$ g0 ~! q; q2 P5 [' E - Err = getErr();( U# B- b9 H2 L
- }while((D1+D6) == 0);
0 q8 I' E+ B) J F4 y1 | - }
! Z9 W# t2 k- E( w! F - }
6 C: V: r5 i( W5 H* \ - else
1 U/ X1 a) c5 K$ o7 a - {# M |- w0 Z& r& Z6 g" Q$ D* r
- if((Speed < 160) & (Err < 2)) Speed+=1;
5 @7 z. H1 h( }# B3 D3 l2 v' x - if((Speed > 100) & (Err > 2)) Speed-=2;
) l7 T: ^& p" C% @$ a" Z) W - integral = integral + Err;
/ {& p; m" `* [ - derivative = Err - previous_error;
& V: K6 K1 Y9 q } - output = Kp*Err + Ki*integral + Kd*derivative;
Q; d" C& L. w0 N1 s6 @ g5 M - moto(int(Speed-output),int(Speed+output));6 S( ^% m* }% n3 c* d9 {3 [2 J6 U& D6 s
- previous_error = Err;
+ ^* m! E, x- w0 R3 Q - } _" g( l" Z0 x, b
- }while(!((lightsensor_2.read()) < (10)));
" n9 H1 o0 Y' z U4 ` - moto(0,0);" R' I+ ^& ?( O- G
- delay(500);
5 F& H6 c( w. Y0 w; X# \ - buzzer.tone(262, 500);' k2 H) ~0 w! D0 q, }! I! `7 ^
- }: U8 ^0 S8 R* n$ w- l
- , _1 |. u l, [, }) u7 F f- Z
- int getErr()* }9 v7 a+ G! {* v0 F
- { ( \! c8 ~- M. i4 t0 K
- sData = linefollower.getValue();* |, a& l$ `0 \" F4 N9 }
- D1 = ~(sData>>0)&1;1 u' Z/ S7 V; E* d
- D2 = ~(sData>>1)&1;
o2 b0 f( d2 r: f# C: M/ h1 Q - D3 = ~(sData>>2)&1;
6 {1 Z& ]1 q: M/ q- d [( i - D4 = ~(sData>>3)&1;
$ z8 _! u6 Z" i* j - D5 = ~(sData>>4)&1;
# l% x% X8 S/ f" q, m9 B9 e# f: H - D6 = ~(sData>>5)&1;
$ v$ F+ {8 X, ~. P9 U+ F9 s - int downD = D1+D2+D3+D4+D5+D6;4 O1 E. K: M4 H* t* Y
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
: {1 {& K* v' ^2 {) l& R/ N' x' W - if(downD == 0)0 T, j: u( \1 R. ]: k% ^) A1 P# u
- {; Z# g( d! n% C% v
- return 99;4 o+ A, K+ u' F: M: i8 ] ~
- }
' C& F! P w/ ~7 x6 w2 t - else
9 Y$ A1 E( s% J8 q - {
# c0 y7 n, t- b - return upD/downD;
: m& u" l# s$ E4 P/ z - }7 O9 @: O2 X! b# v
- }* E8 I5 U, V* r
. G$ c3 M0 c# B+ R$ ^ N! X- void moto(int Speed_L,int Speed_R)6 D$ c$ P4 [" `. j" l
- {
0 {: p; q. w3 B* u; ?7 z/ h, r - Encoder_2.setMotorPwm(Speed_L);( {3 a: E- G% }2 |& M
- Encoder_1.setMotorPwm(-Speed_R);3 }1 c/ d" g7 F5 c! T" a& A
- }
複製代碼
2 j) S. s3 U9 Z3 |MeLineFollowerArray.cpp
7 L- L; I" v8 q- G7 R4 ?4 O! L+ e- #include "MeLineFollowerArray.h"9 L# J( o# i: a1 U- Y
/ S6 |" K' p3 j0 {, W" n. e0 x- #ifdef ME_PORT_DEFINED- e" m+ P7 ~+ J: M
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)% s2 g/ Y1 m6 Q! w
- {1 A6 l7 c, N" w' g* a; c
- I4 i# D) P* g* W q* j- }
( X# m- s/ i" j: ^, d - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)+ P7 V2 N9 T% n
- {
* m0 G( W7 |' e, X7 \ ?1 s - _DataPin = mePort[port].s2;, \4 ` H4 x b3 q, |
- pinMode(_DataPin, OUTPUT);
8 K9 a" A' F# ~! W6 y' ]5 v - digitalWrite(_DataPin, HIGH);8 r- Q# y% }$ i
- }
@9 a% g3 w! h3 X - #else // ME_PORT_DEFINED) t4 W# L6 f( ?7 w+ n+ O) N. L7 j* W
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
! q; A9 C# Z+ w c" ]( S$ k& z - {
0 V+ P, |3 t' N. c% T0 C: Y9 A - _DataPin = pin;
3 Y% X5 u, Z, ] - pinMode(_DataPin, OUTPUT);
5 l4 `4 ~7 M( C - digitalWrite(_DataPin, HIGH);# ^" g- B6 P! i" t" s
- }
- D) L/ S% V A' B - #endif // ME_PORT_DEFINED
9 L1 I' C* ~5 T1 n; J - , m7 ^- ]! K# e
: k& y# }/ A7 P, O4 p _+ N- void MeLineFollowerArray::setpin(uint8_t pin)
* B+ F/ N5 W: i) X6 G$ g3 F- z9 T - {0 w, p; p0 t- F; i+ z9 o
- _DataPin = pin;
4 H! Y* x, o) R( |" C; F) i6 Q - pinMode(_DataPin, OUTPUT);
1 O( }: [7 z- Y! R/ t! z5 ] - digitalWrite(_DataPin, HIGH);
% ~' v+ D5 I! b# w; { - ! e: V4 ~& Q+ I- B# k) k' [
- #ifdef ME_PORT_DEFINED* Q' A8 P, F$ F
- s2 = pin;
5 F6 U* J. V; @4 a - #endif
8 v& O# |$ q# [5 v8 ^0 E - }
1 ` l, a& ? v6 e. ]7 R
+ @( @) w# ]$ @7 V- uint8_t MeLineFollowerArray::getValue()% E! M) o e W8 }9 @* W! n
- {
[% f" x' u2 ~% X - uint32_t LOW_level_read_time;
: R6 ]' Z7 d2 O- s/ h9 I+ g+ r - uint32_t HIGH_level_read_time;
$ e: J, e/ A% | X: R2 Q - uint32_t time_out_flag;
* W" P- H- _- O% I* D% O) c' D \ - uint8_t Sensor_Data[3];, b* h7 Z @; r& M8 h- \
- static uint8_t old_data = 0xff;8 Z6 v- r) `% P1 B6 W
+ Q% U. c) f6 E. p, X0 ` G- pinMode(_DataPin, OUTPUT);
" ?8 W/ I+ x# Q& x2 n - digitalWrite(_DataPin, LOW);
( @; ~0 I* y5 e0 Y! Z2 j @ - delayMicroseconds(980);, g& k% L; D0 x3 ~
- digitalWrite(_DataPin, HIGH);
) ]+ ~" Y: i( R - ( } ?& c; O# r. G1 Q5 F5 B
- pinMode(_DataPin, INPUT_PULLUP);9 k$ q m! {- R/ t; d$ [) x2 S
- delayMicroseconds(10);4 E' [6 X6 m/ B: p4 D- j
, Q# s% I L, d) m7 ^7 l5 n; S8 J, J0 M! t- time_out_flag = millis();4 s" L% Z* M6 p3 X6 V1 \* A c
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
1 v }0 V$ n2 X4 {# x' p
: U' {1 I4 e3 \ W; f" C- LOW_level_read_time = micros();3 U& J' p. v1 v2 o" n( C3 l" n
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 @% t' c1 n: J/ h+ L - {
" e0 N4 p3 s4 E7 r0 M8 Q - return 0xff;! C2 \: S1 k8 M# K
- }
) W2 c$ g X. A5 f& o
! U/ N2 u; V" [: `. ~, [- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. E6 V3 q% x% C4 i9 W, q
4 y4 s" l" @, o* Z# E. e- HIGH_level_read_time = micros();: ?8 b& Y0 R! `- F
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
: e% ]5 ?# r1 \6 @9 G - T7 |% V+ a, R% k
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out4 f+ W, I( s9 A
- {
* a, O# `! y5 K) n* T$ ?8 a - return 0xff;
3 n5 V- {0 W' z! Q+ |7 F& i - }5 c/ t- ], \+ x2 i
8 L7 F; ~. h9 Z% @/ |, W+ L+ b- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)) R9 \/ z" i+ a
- {
' Z* H; R6 r7 e - return 0xff;
- l) r6 G7 e, g5 R - }( h$ }7 `4 d1 E6 M" P9 q
- H' A3 n9 V6 O- ~, x! {. a" m- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- Z1 E5 p- c/ I" k7 M0 |* P- w0 I8 C
- LOW_level_read_time = micros();# I) m# V; E% |
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
- k! }+ s' W: x
) Y+ n- P, g4 \* b7 z D/ P- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
( Z4 O$ o# O# U5 W$ b/ n4 ? I6 ^ - {
: H. m: A d7 `: G% L - return 0xff;
5 ~3 }8 `4 r2 T0 Y - }& v$ _$ C/ ^/ ^! e% M8 m1 K
- 6 v0 R8 c$ O6 h" F" W( K
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))2 k; S6 S9 f# m5 j
- {5 G# L$ F; z R3 c1 L! v
- return 0xff;+ q# C; H. J5 B& |5 h {( x7 i1 S8 x
- }- f, Z( V3 h2 V# Q0 d
- ' i, r! |- v8 [# x7 z* ~2 V: }! D
- for(uint8_t k=0; k<3; k++)! t& t7 u" S7 A- v
- {0 m1 B# I$ E/ P) |8 { \$ R
- Sensor_Data[k] = 0x00;
, G) e- p, {# _; e6 v H" R4 ^& `' Z: _
# c; Z& {( f: o, w/ ^- for(uint8_t i=0;i<8;i++)
3 ?0 U! T; T$ `; v# B$ P9 G% v - {
9 t! u4 W1 i9 y0 K2 U0 {( x - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
5 h' V D& Y' d/ d8 k9 v - HIGH_level_read_time = micros();1 Q) L* R& E O" S T
- LOW_level_read_time = micros() - LOW_level_read_time;
# V" k& F( W; n. X" O5 b - D: m0 z! |* e: o% x% `) H4 R
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) y" V' y9 }3 N! _
- {
/ c) U# m$ B v& C3 E8 M - return 0xff;
8 z' n q, ]* p) t; W6 f - }
$ Z9 Q: v3 _" w
$ K, [1 t( |/ \) n3 R- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );# t7 i _8 c0 a9 L. k9 f2 B
- LOW_level_read_time = micros();. A0 h% a( W7 O
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level7 f8 s. u) ^# D& g9 I
* y# ]. {) _ q! }0 H+ G; K- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
2 z. U) n; Q9 U$ S& k# | - {( n3 u( [$ H! l( D, g e" O
- Sensor_Data[k] |= (0x80 >> i);
' y; K, y( g1 k$ a6 d - }
' \! `/ z: s0 f6 T+ G5 H. A - else if(HIGH_level_read_time >= 100)
. P# V G* [/ G1 R0 d3 f - {
, A/ G( b( u& A( V. z - return 0xff;
5 V% i j$ e! {1 ]+ [6 C! @5 b - }
/ L o- t; W; i2 I; N7 h' @ - & i, o* K% a, p* b' O
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)7 |$ o- A2 z0 v2 ?
- {8 ?1 h1 E8 X, s. X# T R. X
- return 0xff;2 p, i3 M0 i* r9 O) e0 F& S" W
- }2 |, q" B' N* ]& ~6 u6 H/ p
- }# N" ^3 C. i2 ?. e
- }
Y, d8 n* r, g* f2 |2 C; @& l - 3 C4 L& I& n- t
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level: `3 E0 P# A) `; r1 \) B
- HIGH_level_read_time = micros();5 v1 g0 o' A( G/ }$ \
- LOW_level_read_time = micros() - LOW_level_read_time;
+ a8 u/ p3 l5 C* d" {! w. _1 g
5 D B8 S, V/ Z0 u; E( G; J/ \3 V- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
* S2 b( j! \% B: y! y6 n - {
+ Z9 G6 r/ O: k3 [ g0 R - return 0xff;$ b; b5 ^* S( t# P
- }
, B) Z/ d3 f( ?1 p* P# o - 1 T9 O' x+ E4 i) H) |. ?8 L
- pinMode(_DataPin, OUTPUT);7 Z0 ?1 V; I @, r) Q0 `9 l# b! t
- digitalWrite(_DataPin, HIGH);
0 a+ ]2 ^9 u, H: Y; z - 1 b; W0 s% o4 R
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))( e0 K* h1 e8 O7 i2 U
- {
- k5 }' k! a- Y* j; O: c - old_data = Sensor_Data[0];- x4 l8 }/ N Z: ?' p
- return Sensor_Data[0];4 c0 Q6 P# P8 ], R
- }
- r+ Z4 ^. o$ w% V- X, f - else) U: F w5 X% e
- {! O" U& n; o' L" U3 {
- return old_data;# N. n& I1 H7 M0 ?
- }9 X" V7 ]! q1 s8 X( O* \
- }5 D, j1 I+ E r
複製代碼 : ?2 Y! c0 z# f2 p2 j6 _
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
9 |* b( T' U6 k& G6 B
: w0 M: @5 @8 f: }* ~2 G, `, O |
|