|
|
! _9 p4 d8 I+ v! ^ R
- #include <Arduino.h>
$ ^. K2 Q5 t; v6 Z1 n - #include <MeAuriga.h>. g1 E$ b! l1 [' b5 k: L6 z4 ?; A- k
- #include "MeLineFollowerArray.h"
* L! f- q) ]) |% n% s/ i - ; m# j n* }4 H1 Z
- MeEncoderOnBoard Encoder_1(SLOT1);
# H- Z, e; B* V; k1 v, G% x - MeEncoderOnBoard Encoder_2(SLOT2);
4 M2 q0 j/ I& Y+ W - MeLightSensor lightsensor_1(12);2 m2 k2 f F: L8 M, ^
- MeLightSensor lightsensor_2(11);* }4 `. q9 P0 e. Q5 N" G
- MeBuzzer buzzer;
2 C/ F! b8 w2 m% r# E - MeLineFollowerArray linefollower(PORT_6);1 S; {+ o% S0 t9 S" u1 y/ B7 W* G
3 j6 w! s: S8 k) ]; R- _- #define Error1 11 P8 K( J. b; s) J9 Y
- #define Error2 2" X2 e3 b; W1 G/ {/ y: `# R
- #define Error3 34 L+ F) Y3 {+ @. O Y
- 9 m; w+ T' b- V1 T9 |2 W, ^) Q
- #define Kp 156 N5 B; g5 Y i' l
- #define Ki 0.15
u, D$ E/ E$ x$ F/ B! H - #define Kd 0.03
5 _4 s# a" j, b0 y - % a# n1 g2 n+ s$ G
- uint8_t sData;& T; z9 p6 c# Z$ g! q
- uint8_t D1;
1 h9 o; I* d, ]# ?$ o& y1 O - uint8_t D2;; X5 |4 c, V" J
- uint8_t D3;
6 K! p7 N3 i0 `% ? - uint8_t D4;
8 ]3 B1 T5 K8 B4 m; ~" N - uint8_t D5;
% `/ C$ m" w6 D: k5 @' V* d0 e: i6 ` - uint8_t D6;
" S5 h% X \- ` - 5 p9 n% {+ O: y! f
- float previous_error = 0;8 `: H" P0 k5 z- Z
- float integral = 0;% q) K1 v. S: {
- float derivative = 0;8 @( n3 {7 \/ w$ t6 }* o, ^0 s# k
- int Speed = 160;
+ m3 G! m X8 q/ Y2 L' L( ], y, ? - float output;! U$ y" e7 A* \1 Z! `$ K
: g9 q' b& e# L0 {- byte Left;
9 D' o2 G& ^/ Z4 Q1 p2 l; [
5 v$ `8 B7 c# E, f- void setup() 4 D; Z" ^1 E" Q0 q8 @9 o6 q" w
- {
; v" _3 U% r4 F5 M% U5 m( ^ - //Set PWM 8KHz* `% X4 T8 J) g$ T" W: j0 m
- TCCR1A = _BV(WGM10);
% k/ H" b; Q' A6 w - TCCR1B = _BV(CS11) | _BV(WGM12);+ v' o1 y, t2 a; k: J
- TCCR2A = _BV(WGM21) | _BV(WGM20);5 L8 \% W) G7 G) ]
- TCCR2B = _BV(CS21);7 i2 k& u1 M5 M% w, {* z5 S
- Serial.begin(9600); c) c% L' g( j3 J+ W6 A" n
- buzzer.setpin(45);+ e: R6 `4 }$ o7 X4 m. |8 H
- }$ @7 k3 m$ h& d7 m" Y
- . {3 D* M |2 T9 P) A
- void loop()
* A: n" p' r% |* [& I - {
. B9 {2 L$ }+ T2 j - while(!((lightsensor_1.read()) < (10)));
6 u N! o, _" G4 q( \ - buzzer.tone(1047, 500);7 f0 A" H! q5 B0 v, H( J2 \! o
- delay(500);
/ D" U- T( Q" [) s" A - do
, Y% G6 I$ K# H& g7 G* i - {7 V/ I1 ?$ }/ G1 Q' R$ t' i/ K7 o
- int Err = getErr(); Q/ E- ?" b, ?+ r7 `$ ]
- if(D1 == 1)
; B; n5 S4 l# t7 \ - {
8 D% u; }2 w# j - Left = 1;
1 c( q2 ^8 n* O+ N1 Z% ` - }5 G( K4 A0 J/ Z0 V8 G- f
- if(D6 == 1)
, h9 a. u: U; J! o O - {" r3 w) V$ X" E$ O. k
- Left = 0;
% K4 x& {! E o7 T, L' D5 M2 v - }1 N6 g& M! K U+ y- J$ g& E3 l8 |
- if(Err == 99)
t; H- N _. ]( j5 E - {" J9 i; F# z, D% }/ o* @
- if(Left == 1)' G; F2 `) j7 a ]9 ~ B1 ^
- {
) t' Q D' Z8 J/ f6 ~; b0 P* I" K* X - Speed -= 5;
0 a U$ s: y* q: ?0 p0 _; y - moto(0,Speed);
8 x: M8 P3 M& }+ u! |- T - do
! ~) d6 j6 Z- r1 g. \ s$ T - {
0 Y0 Z' n1 I4 w8 N9 f" P - Err = getErr();
# l+ K- v8 } ~; Y! l - }while((D1+D6) == 0);, z G1 n3 n% `! `
- }$ M- I! g z" p
- else
: O; U+ p, [$ Z, Z( ]. Z - {
! |" m6 R2 ~* b8 @: ]1 k - Speed -= 5;' D9 I) f9 ^5 C( D
- moto(Speed,0); 4 F# c& I+ o: r# v! N
- do
! B+ `! h+ ^" |; E# G, a8 I - {
. e2 C- N" x5 L# } - Err = getErr();( e+ ?6 Q* {5 C# g S6 ? J
- }while((D1+D6) == 0);3 |2 g% c+ N3 I3 c6 p8 i1 G
- }
' ?' C$ v$ `4 T% m$ O: g - }" A: E: R) X8 B0 [7 F$ x
- else
+ v1 |( |' d7 N! s - {2 [/ W% Q" b/ R/ f/ b L" v
- if((Speed < 160) & (Err < 2)) Speed+=1;
# _; _! z- y+ X2 d+ j3 c/ Q - if((Speed > 100) & (Err > 2)) Speed-=2;
0 y3 p, R7 ^3 }: o - integral = integral + Err;
5 A: ?: b0 T& w' I! y/ k - derivative = Err - previous_error;8 V0 e3 c$ C: m1 G, q( J" a+ x
- output = Kp*Err + Ki*integral + Kd*derivative;# n1 o8 H' t, j4 b. {
- moto(int(Speed-output),int(Speed+output));* `0 r6 _3 }- Y- v5 T- |0 E
- previous_error = Err;
, G# S9 C- z6 j+ t9 f4 R: g+ U - }# H& ~0 {, i7 L9 ^' c2 W
- }while(!((lightsensor_2.read()) < (10)));
/ p/ g! [: Y- J% n' A - moto(0,0);
5 M, Y }4 R+ N; n6 ? - delay(500);
% X( N) y2 v; X7 _+ F) D: A$ E# @9 Y - buzzer.tone(262, 500);
# l7 n7 H* G1 W' S - }: b3 P. r( Z/ E8 H
- 8 m! h7 Y" N! V. [: [ D$ b
- int getErr()
5 q1 t, t$ _& ?9 z) W( i3 p - { 5 T& ^' x y s3 e, h9 E! s8 s' L; k
- sData = linefollower.getValue();8 i* K C. |! |* J
- D1 = ~(sData>>0)&1;0 Y1 H- A# }- f. I- P
- D2 = ~(sData>>1)&1; x3 c! f5 @4 t) `. A
- D3 = ~(sData>>2)&1;& g5 p/ b) D$ w
- D4 = ~(sData>>3)&1;4 t& K: V% z3 d. W& z! r7 e4 |
- D5 = ~(sData>>4)&1;
8 U& B) c+ m4 f3 q - D6 = ~(sData>>5)&1;
, C, I3 z/ b5 o# M* c2 q" h - int downD = D1+D2+D3+D4+D5+D6;& H) [* H% ~% I# e& i! `4 c
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);; t1 }3 s8 R6 G& Z/ A- R8 A2 G, N
- if(downD == 0)
, E6 B, k' }9 ?/ X' v! ]6 K; H+ i - {1 I( X, B- e A! E, _
- return 99;! Q, S' P* I6 O3 ?3 _& j
- }
' v# k* w; [: s; M - else
! ?; k. g% D g) v - {* ?+ I- c7 x, b* y
- return upD/downD;
4 `- c) y, c% w$ S% t; e - }
3 B) G, c- G0 h" e, z4 c- E - }( L n! [0 D6 {, z' W
- / w/ ]" R6 d/ E/ J
- void moto(int Speed_L,int Speed_R)
0 k4 E' [4 f, B - {
; v( P. G' [& G2 J& q9 u9 n - Encoder_2.setMotorPwm(Speed_L);
4 d! _9 i# Q- M - Encoder_1.setMotorPwm(-Speed_R);
, ?$ M/ A7 v. S1 M; E* d - }
複製代碼
. J% M; `+ U* I. l9 F4 O% V, nMeLineFollowerArray.cpp' f5 Q% w: ?. U! l) Z
- #include "MeLineFollowerArray.h"0 T2 Y" o* l3 M: a% \. y( E/ {
- 0 n7 Z2 ~5 {3 Q# E ]0 b! D
- #ifdef ME_PORT_DEFINED
$ a. R- U$ b3 c* V - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)$ Q: D; X1 Z2 L% X
- {
k/ \0 ~& M1 a
/ m! @. e! v; R8 D% T) c4 m3 s- }
: N& b! {! X) h# _3 @ - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)( y8 L8 ?4 E! I1 @" \- d j" t
- {; P0 x9 ]5 x8 C, g2 L
- _DataPin = mePort[port].s2;
. L( a1 i; O( `1 F - pinMode(_DataPin, OUTPUT);
" u$ F+ w! o U, Y1 k - digitalWrite(_DataPin, HIGH);
$ c2 O. d) A: f% Z! Z - }9 _8 ^+ W: f1 p# p. o) v
- #else // ME_PORT_DEFINED# f3 B# U! V0 U( t* S, a
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)- N L, t |7 Z i1 _ h; j8 e
- {
& W* ]/ ]) y: P4 `' M - _DataPin = pin;
4 p2 h+ [' R) W4 X: V3 K2 \ - pinMode(_DataPin, OUTPUT); 4 l7 W. j" H7 e; G+ r) Q/ N
- digitalWrite(_DataPin, HIGH);
?8 t8 a+ ^3 A) L" Z3 W7 E - }
& P1 A% S1 A1 _7 t - #endif // ME_PORT_DEFINED& E4 x7 r+ g9 f/ @0 P# o3 f
- 0 Z) H1 h+ l7 M/ {$ L9 P
4 ]4 g( k$ R7 B0 N& v( I% T- void MeLineFollowerArray::setpin(uint8_t pin)
( Q6 F) c; d; N% _# B w8 G( e+ Q4 A - {" @9 o2 k( |3 w/ _3 u5 e) G' i+ b
- _DataPin = pin;# j) H8 U- V# \
- pinMode(_DataPin, OUTPUT);
: o; K7 ~! n* r3 B# I/ F& _7 b- p - digitalWrite(_DataPin, HIGH);
$ b" b" T4 \9 E/ c
3 w6 u5 x: x6 P; I# n5 D- #ifdef ME_PORT_DEFINED a# E' `2 o5 x% ?1 O
- s2 = pin;
4 q: N) S5 O# c8 H; Q/ C- i - #endif
, }1 n. ]0 O1 _7 W: F1 N5 T' v - }" E2 a7 U4 y: i5 ?( V
- ; f& w( m/ R) l h' Z
- uint8_t MeLineFollowerArray::getValue(): I( L) C7 Y" n4 a! ^3 j6 q
- {
6 P2 J- m" L; d0 v - uint32_t LOW_level_read_time;9 p5 p' U* v w& z' S/ ~
- uint32_t HIGH_level_read_time;
$ N' p4 P X1 b. R0 d* M - uint32_t time_out_flag;. w. c# R8 p; _
- uint8_t Sensor_Data[3];' m% h' Z% V* P9 U) L
- static uint8_t old_data = 0xff;. B2 }) I) B) Z$ ^1 q/ X+ W
K% B \6 D2 o2 Z0 J5 f- pinMode(_DataPin, OUTPUT);
& ^5 A, c( g4 q8 ~5 \ - digitalWrite(_DataPin, LOW);
4 |' ^+ w% v# `/ J3 E; W/ x - delayMicroseconds(980);
: `: W! M$ M! ]3 R6 g - digitalWrite(_DataPin, HIGH);
# ^4 B4 d4 @0 w) O) o - 2 H. h7 V, m) U$ u6 W1 Y
- pinMode(_DataPin, INPUT_PULLUP);
& d; |0 z/ A: @$ L2 l9 Z# ~4 M* } - delayMicroseconds(10);9 K7 R7 q+ ?, f
$ S. u! d# M @, o- time_out_flag = millis();
0 D8 ~ S6 t* t a - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );# p, Y6 Z) {! A+ ]/ n9 i% l
- - b' l0 @3 v7 P k% _' _& `5 u
- LOW_level_read_time = micros();) x# {9 w( y1 P
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
' `/ m$ l* R6 B1 H' h4 Y; e - {
- G7 T8 K2 E" i" G6 b: J - return 0xff;
7 X$ \. a0 k* A! e - }+ R2 d4 D, n) k( [( i) l9 a
% _) J6 {# a% A8 P5 y8 x- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- [5 [; e: ?. n/ A4 @" l9 b
$ _) z6 g' Y( U$ u7 M- HIGH_level_read_time = micros();- F3 c7 Q- J% G% A% @; F
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
' j8 ]! Y* t$ S! h$ K3 q3 {4 z" z
: R, I `' s6 O/ _! P- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out a! i9 j6 A1 O& E4 o. r5 ^! Z
- {
1 C& z$ u- a6 |& k/ n2 p9 N - return 0xff;
2 S) ^% Y* ^' {9 Y! k - }
* M x$ F2 B- x! b+ P
9 w Q! H& b- i' t6 g" _0 [* L a; F- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
& G6 r1 E# w1 Y# X* y4 B% {% [! Z# V - {
6 @/ n5 R. [/ P# G - return 0xff;
# T% _" C+ T+ I3 E! Y% u - }
1 E- U8 V1 m* s9 ]6 K# Y" X+ S. b
8 D$ H- a6 m8 \1 B" U- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ e& G6 x* j9 }" v3 e5 R, p - LOW_level_read_time = micros();
5 C, D* o# A" @' V) `5 C% t - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level' h" U& l* \0 s1 [0 `6 c0 j$ u$ T
- ; j4 b9 B5 h" k( m9 G3 ?- M9 H
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
8 y) ^& v6 v [$ j; i - {* g l2 k& Z i1 r
- return 0xff;) r+ ^9 R+ ]: p' y& U
- }
1 S- k! X, @! Y9 W% e - 2 ?6 t8 ~0 s3 a( R% q' C3 E/ x+ ^
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))# b- [/ P1 v( I7 r. t' F& p) X
- {& k8 n# n8 S' w: v* r+ R! T
- return 0xff;
& T! l0 G X+ V7 i - }3 b7 h- Z+ U' r
5 u) \; N7 G4 l5 H; |7 o- for(uint8_t k=0; k<3; k++)
) P' E n7 b( Y2 Q. ^ - {/ V G; v7 o' R# N
- Sensor_Data[k] = 0x00;
. M, {" A# d4 U3 O" g6 d% j
2 }% _" y3 E+ [- for(uint8_t i=0;i<8;i++)- A: {+ h% C0 S* j, Q
- {! \6 w8 k' e1 J- A3 `
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
% o( W7 F" F. o2 I$ K - HIGH_level_read_time = micros();1 ^5 a" _% ^9 i( B- J V
- LOW_level_read_time = micros() - LOW_level_read_time;
# W1 E2 X" D* c1 N5 a
5 B3 a C; k* R# h& Q( Z8 p- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )5 H7 W7 S: ^& w, B+ }8 y+ @
- {) ~6 c1 M1 D' s. v9 q% S
- return 0xff; V: A' u# }, L
- }* \! g0 V4 e; p0 Z/ ~# H
7 R k& {' \' T; B3 n6 f: `0 z( ?- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 f. W9 v' D2 E
- LOW_level_read_time = micros();
1 R: \+ W7 X5 i& S - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
5 I: `8 J/ T }, { @+ J
, w2 Y' M9 m7 _* k& r% v7 p: R' C- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
$ }% e4 ]) @" i$ h4 B - {
5 }9 r7 P0 a) [, E& @, ^$ O& X0 E6 G - Sensor_Data[k] |= (0x80 >> i);
2 O+ i9 @7 |* V - }% c+ c# Z. {7 B7 _( ~, p/ I
- else if(HIGH_level_read_time >= 100)
2 T3 h" ?5 I$ A8 R3 P - {" ]( i2 _3 r) {* X5 i9 T9 b9 i% v' n
- return 0xff;2 d# J6 ~3 l( m- \8 Q# R8 W2 j! { R# v
- }& {' M* Z: |' w$ \9 u5 Z
- " M* k( f1 H' T4 ^2 ?3 |
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
# O: F, N6 K; ?+ a3 j% G - {
5 K! x0 b u' \( V( h' X) S - return 0xff;1 n5 I1 p5 ^) l$ B d9 f5 d
- }! J2 ]) O; v2 x- t6 U" q _
- } ~$ X9 J; l) w6 _& f* g/ \3 Q( A4 h
- }
( \" `/ c1 b( F `3 }+ Q
& L6 O" S p2 L2 G- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level+ E7 P0 o0 y. N( r6 o
- HIGH_level_read_time = micros();# s* ~" ?' j& n+ T6 i3 c
- LOW_level_read_time = micros() - LOW_level_read_time;& R& Z3 b6 G% s& {: X
+ M# `+ ]% }( R5 {: w; I- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) Z9 f- G3 z/ ~" h
- {3 T: f2 [! H ~* A) X) e' u
- return 0xff;
1 [3 k8 D# D- Y( L# S - }
) f* w- T: ^, @! d; W
: W3 P, ?! K5 B- pinMode(_DataPin, OUTPUT);/ Q+ C7 E. N/ G: D! S h9 k
- digitalWrite(_DataPin, HIGH);
! J+ _6 r8 x) m5 ?& D
$ g* v) h! e$ ]% Y- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
9 P: y8 C9 ]1 z# Y [ - {0 B" H1 k4 l, j- r) D
- old_data = Sensor_Data[0];
( M$ _. r( e/ q* f( M) W5 k - return Sensor_Data[0];
* M3 k& T) m% i( W# V d5 A - }
( ^8 l$ c1 D. P# h& J+ B - else
7 c4 K' a3 r2 X$ V6 w6 \ - {
* E; _$ Q5 ~) K - return old_data;8 |7 F- z7 k; V' X" D! Y3 t) P/ Z4 F+ J
- }( I8 \0 H5 Y5 @; u( q5 ]+ t
- }0 n! ~6 \$ ~( f: [' i9 m
複製代碼
; P6 U3 y) f' a7 N2 ?MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
6 Z0 g' ?6 U2 Q' J* M% q" ]4 A; {8 |
& {! F( ]1 ^5 e& M* x |
|