|
|
* S g# ?3 j% L' P" m/ W
- #include <Arduino.h>" H* j7 v, v6 e. @, m
- #include <MeAuriga.h>: p; e8 ^+ o7 E2 D5 \# w
- #include "MeLineFollowerArray.h"
. h% Y) r$ T k0 i - I/ z. c& s- t/ b# P+ S
- MeEncoderOnBoard Encoder_1(SLOT1);
$ G: w3 H: P: J - MeEncoderOnBoard Encoder_2(SLOT2);
( ^& `+ H+ E" A4 Q/ F5 E. F) X& ^4 @% g - MeLightSensor lightsensor_1(12);! ]4 D7 G! W/ G1 w
- MeLightSensor lightsensor_2(11);2 a' E/ f6 |+ K x" [, E& m
- MeBuzzer buzzer;
, I9 O4 B# V: q) N. p - MeLineFollowerArray linefollower(PORT_6);
' g& Z7 n# x& C, @. C4 y6 t' B - 4 p- ], A. y. k8 z' @
- #define Error1 10 g! Q# a: [8 ~8 g) O u
- #define Error2 2! ^" g# O/ K. j% s( U m5 W A* f
- #define Error3 3* C4 h1 h" |' K; v u
- 2 @) D. U/ t+ P
- #define Kp 153 V& M- t/ \8 ~; ]3 d
- #define Ki 0.153 j9 R- I$ M P" c
- #define Kd 0.03
: l3 R7 f- b' G! I- u - * ]2 e7 y& W( _' D, @9 s7 X
- uint8_t sData;
r0 X+ R* B: u; Z! C0 b - uint8_t D1;2 o# K# j) q9 K2 `: _0 h
- uint8_t D2;
" ]8 ~ z( W- P6 m! ]& H! p! A - uint8_t D3;) D* r8 m: ^% d- r1 M
- uint8_t D4;
$ E1 v: f$ M8 F T5 S0 x8 y; V1 i: G m - uint8_t D5;
5 \( P6 ^2 L2 V6 I5 W - uint8_t D6;+ K% @' A9 ]- j! N+ K, B
* P# m! _% a& u1 I- float previous_error = 0;
2 `6 a: T# c& G6 H7 O7 Q: R- _ - float integral = 0;
. A$ r- C. f7 z- B# X9 R - float derivative = 0;" T8 f* `) M# W2 B( @
- int Speed = 160;. ?4 C4 Z3 k) m3 V4 }# v3 M m
- float output;
* H4 M! r7 m: w, u( ?
8 J8 _2 D# Y3 p, a2 ?" @- r- byte Left;$ E9 Z3 L8 }! h$ p! C, Y- M
- . n4 Y! q# b9 b3 Z6 s" s& q
- void setup()
- r/ F+ I; \3 m8 N- @% x: G - {. y- P7 Z) _. k& q6 r1 Y( H
- //Set PWM 8KHz
# p* W2 L# D$ { - TCCR1A = _BV(WGM10);$ l* Z$ H$ m8 k& U+ |6 X" k8 k9 ~* @
- TCCR1B = _BV(CS11) | _BV(WGM12);2 o8 j4 K& P! K& C
- TCCR2A = _BV(WGM21) | _BV(WGM20);
" n6 R; z) m4 M: n - TCCR2B = _BV(CS21);
* B/ m2 P3 F1 g - Serial.begin(9600);
8 G; Z( f8 m) v/ U( w - buzzer.setpin(45);
( w( A% O5 l6 }% d6 |1 ? - }5 D5 m0 c4 o' d5 o4 O
9 g6 O9 d% F" Y0 r7 H2 N- void loop()
8 z+ V) t$ e+ D0 Q - {
' E9 _) R* k$ c9 u2 S# @! P5 ]& _5 e - while(!((lightsensor_1.read()) < (10)));6 T; M& M {" T* S3 V5 K
- buzzer.tone(1047, 500);
; Q9 J0 H- [# ~ - delay(500);% `! U8 U! n8 y: h. T+ z
- do
, L, s/ U1 s! d2 }, c% x* ] - {
3 T3 \$ K4 | U+ U - int Err = getErr();5 u" t; i) w6 R+ W
- if(D1 == 1)
4 o7 n) d+ p$ t+ y' s( h' \7 @( q - {
+ D, M4 f6 Q' h - Left = 1;- v+ k7 L4 |9 `7 D
- }2 l! i- b" B# u$ ]8 G
- if(D6 == 1)
% x- E8 ~ s+ e& \/ _! \( ~ - {5 ^( O8 ]4 w- m: H
- Left = 0;0 f2 w6 R7 Y; _
- }; A7 v/ |! l! j6 V
- if(Err == 99), w0 P: z5 c7 F' k/ Q1 L- ]! T3 m# b
- {$ h2 f7 h ?! {7 Q& L0 i
- if(Left == 1)2 F4 R" k# W' g* Q0 P
- {
+ |" E* }$ x* [8 Y! L, T - Speed -= 5;
8 C! l7 d1 C- Z2 B2 ?, F, k - moto(0,Speed);9 [3 _7 [0 b! m3 A5 |$ h
- do
/ X; ~" Q' C6 G ~ - {
0 ] r$ I7 M& V - Err = getErr();( }& K+ c/ y8 p2 T# C0 ?
- }while((D1+D6) == 0);
) K5 W! I: u5 E# n l1 h - }) L4 `+ O( Y" [
- else
# k2 Z7 @& ^) Z0 K& n Y; @ - {
: u3 f* i4 o: A1 v7 R - Speed -= 5;) V' y1 ~9 C9 I5 n% h, H
- moto(Speed,0);
_7 `: u( Q( [9 F' |# O: j- c3 w - do* N; o; z/ C* j' j H
- { Y6 b& L5 n$ K# U+ S
- Err = getErr();
$ E* e, | r/ i) j* l - }while((D1+D6) == 0);
" E9 ?9 R7 e( H1 G8 m - }# Z0 T5 W" B" O& _
- }
: m4 \& h! o Y3 b" s - else$ L% J& @- m0 ?5 R
- {
7 v& J, Q- i f0 J- H+ T' E1 k - if((Speed < 160) & (Err < 2)) Speed+=1;
" G7 R- @/ ~4 g - if((Speed > 100) & (Err > 2)) Speed-=2;
- O8 S. ~' d- Z2 t# ~* z) @5 t9 [ - integral = integral + Err;
; r) Q" M! }. s. w; @8 w - derivative = Err - previous_error;$ P0 ~8 n$ Y- R% t5 b$ j
- output = Kp*Err + Ki*integral + Kd*derivative;0 z/ `: ?8 t% S5 {& W% b3 ?+ C
- moto(int(Speed-output),int(Speed+output));
; ?1 [% J7 I5 |9 b! y4 d/ o: ^ - previous_error = Err;
: |6 Y! Z8 f8 P2 C6 S% |4 O5 ~" E - }
9 }3 s9 {, m( f+ p; }& u* M( n8 e - }while(!((lightsensor_2.read()) < (10)));1 M4 B+ \( g( Y
- moto(0,0);
" f b% q$ [6 N8 M" ^" s - delay(500);* i) x$ l2 V% Y3 x! t& j1 f$ n
- buzzer.tone(262, 500); z. a' T7 `4 R. [- O6 X9 L1 j, f
- }% R$ v5 X: W- \9 Q+ d
/ l& k% O; ~% B- int getErr(). B' x! c" N- |) H) I0 C; c c
- {
9 d! o6 A& v" x9 c: h - sData = linefollower.getValue();
9 ~4 s( q: a0 w - D1 = ~(sData>>0)&1;7 x* ^7 Z: a3 e0 {8 @; E: Q0 u. Z5 e$ q
- D2 = ~(sData>>1)&1;* y5 H h k! V2 \7 r
- D3 = ~(sData>>2)&1;! d* ~* B8 R1 g" ]+ @* \
- D4 = ~(sData>>3)&1;
, q9 Y! p5 s1 V+ n( U8 F - D5 = ~(sData>>4)&1;
. M# u* ~/ j1 B4 @ - D6 = ~(sData>>5)&1;
; S8 m, v2 y$ }' `, O6 F! G+ N0 } - int downD = D1+D2+D3+D4+D5+D6;
$ I1 p1 ?& |2 {" h8 k7 [( ^1 K& Y - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
4 R* Q# e- b0 v1 P - if(downD == 0)4 d! v7 p5 @* d8 e
- {) ]: k$ `( g# H1 ~
- return 99;: E7 h$ T* O6 X6 _! F* G: A9 b2 t4 U
- }) w6 w0 [1 W2 d j- U. h$ d5 g
- else
6 G- i% I: K, I: C - {
& `5 H- s4 \5 Y/ G8 j - return upD/downD;
1 S' }0 y; X/ N9 M& p( q) ] - }
- E$ {: R/ F j5 w9 W - }
% O9 D, v" Z1 T8 e! j* T
( c0 W7 O0 J9 `* m: i- void moto(int Speed_L,int Speed_R)
$ a/ ~) A. P/ |! r! \ o - {
9 c* X% N; s4 H+ f0 o% C - Encoder_2.setMotorPwm(Speed_L);
: D' A# D0 R2 y3 I9 ^ - Encoder_1.setMotorPwm(-Speed_R);% W4 O# E, L1 m0 @3 e; p0 H0 D
- }
複製代碼 2 S) F9 C& h# W; r" u2 N. I
MeLineFollowerArray.cpp3 F- s3 k4 g X6 S% m" v$ c
- #include "MeLineFollowerArray.h"9 x/ ~$ p/ z$ q8 z, T5 X
- $ L9 K' J- U; n& y0 R
- #ifdef ME_PORT_DEFINED
2 y* S9 q! {4 Y1 } P3 f - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
: V+ k: S: h$ c - {
8 b i" T- _; T M1 Y8 n - * Y2 {# N' h8 u1 R. ]9 \
- }
' @6 A/ W& i7 x+ }5 W" a2 t, ` - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)9 @# c, |0 {$ C9 q+ z9 I
- {
1 T8 z+ R. A: n7 I, v - _DataPin = mePort[port].s2;
& J P6 v% o) ~1 a" D$ g c - pinMode(_DataPin, OUTPUT); 5 j: g- H- o! f+ i
- digitalWrite(_DataPin, HIGH);/ c) o8 V' Y! c/ p% J$ r
- }
" |$ M5 W' s2 f# q - #else // ME_PORT_DEFINED, |, Y# E, r% w- [$ q. D5 y& W
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
( T0 ^5 p- g. s, I% q5 _+ Q- n' f7 M - {
# i- n a, X& y2 x( d, l. p& Q - _DataPin = pin;9 F0 p9 |# n% A' f9 ?: x
- pinMode(_DataPin, OUTPUT);
, Y1 B2 h( x0 V6 n: _ - digitalWrite(_DataPin, HIGH);
( c; S3 W6 ]0 j9 e1 o - }
% d* _3 O$ _0 f; o - #endif // ME_PORT_DEFINED' }5 A B4 v1 ]9 s
0 u# C! a1 A6 {5 j
G. m3 _3 r6 a. p- void MeLineFollowerArray::setpin(uint8_t pin)
) T4 N4 Y; H+ G$ j: b& [2 E - {
2 K: Z" h- w8 A% Z - _DataPin = pin;+ X5 ^& ?, A7 A0 V Z* O7 q
- pinMode(_DataPin, OUTPUT);
/ [; M; y+ {2 g6 } - digitalWrite(_DataPin, HIGH);) j$ Y( V5 ^# b# \& b0 {
: `" g. t! K! V" v- #ifdef ME_PORT_DEFINED
1 b/ A3 A( K3 p4 _9 q( B4 a$ j - s2 = pin;
8 f6 J3 k* y9 q' L - #endif
# P" _9 n8 N) _/ w - }' o5 ^9 L9 A1 r
# J& Q0 d$ J3 h+ p, z2 F" i; t- uint8_t MeLineFollowerArray::getValue()( y% y1 b% I: m% E D
- {9 \2 A; r- l8 @9 Q! @% N
- uint32_t LOW_level_read_time;
w7 |6 c2 J; b7 F - uint32_t HIGH_level_read_time;
2 y9 a! @/ X- i) | - uint32_t time_out_flag;
. c3 e0 X4 j& o8 F* _" Y q3 u) ~ - uint8_t Sensor_Data[3];
) o/ |; _- H. y1 k - static uint8_t old_data = 0xff;
" H6 A# W. q9 w7 C4 l' { - # ~% a9 I" x/ ^: o0 X; z$ k
- pinMode(_DataPin, OUTPUT);8 Z# o$ g) u* \- W
- digitalWrite(_DataPin, LOW);
8 {- s. p* l1 ~; j3 v& h - delayMicroseconds(980);" d1 t- {3 s: V4 B2 ^
- digitalWrite(_DataPin, HIGH);+ `; H4 D \- s$ y$ Y; S% `
) F- B$ [4 N# m Y! I" i- pinMode(_DataPin, INPUT_PULLUP);. J" Q5 F0 O5 C) M6 j6 Z1 {9 B4 a
- delayMicroseconds(10);
/ @% j( N M5 z4 \" N. h9 [
! A4 s" \& B( t0 B0 v: J5 e- time_out_flag = millis();2 J, K. m8 n8 b
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ \* _3 e* D. O2 p7 Q$ j' Y E) r" T - 9 c0 A3 I) t, ^( h& y
- LOW_level_read_time = micros();( F! @) X0 X5 F* d: U: t' y
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out1 u! J U2 D9 v. N. M, K& c" A
- {. H1 Z% X1 H6 [$ \
- return 0xff;
, j% j4 N% @* b" A! D! H! | - }: X7 | s0 j7 x+ _9 x6 R9 V% D3 c, L
- S5 X+ L. B8 U3 B$ u5 H2 {
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! }5 @4 M5 ]1 G0 \# n% N - & Y2 `9 @/ t; w* O' X) i$ y% i
- HIGH_level_read_time = micros();
# k& ^9 t5 z. W4 X) z5 k* ]( ^ - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level `9 M8 @+ ~2 ?8 `/ }5 R w
- + @ J2 y' J5 @6 W0 q: ^, I9 h
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
- _/ L! c/ h. u5 d9 c - {, {8 f/ k; z" T' t2 e
- return 0xff;
" n! j: h8 K( u - }
1 l$ E7 i c1 D9 x - * P. J% L% n) B/ @9 O) G
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
& }7 S) |4 o, Q) T2 C4 O1 f - {
# s- h/ x/ u2 g+ D9 l - return 0xff;0 C* ~. Q/ E e: u' s/ Q
- }+ c( V, L' j3 X( w! m& z; m
" Y5 Y8 H1 |; H3 R; y- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, n7 o+ U0 n9 I8 Q; A
- LOW_level_read_time = micros();
3 _- F4 ^: H% e. v1 H! M - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level9 R* n7 k( {# m6 h+ n. p+ X
+ @9 W1 ~2 \0 n- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out8 A0 |, r$ ^! a: a3 F
- {) f) X: ]. }9 P+ r$ o
- return 0xff;6 i- Z/ i$ m- o) e( M
- }# V1 s1 f9 P' _+ v( g
9 [2 t, E2 Z5 {/ o+ r2 u- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
) i+ V" V1 O. t! V8 p - {
4 F; q( U9 L! S R - return 0xff;7 ]# d6 |- t" d5 b
- }) f8 @4 R3 @% S5 \% t
; K9 o) i' }0 t5 ]- for(uint8_t k=0; k<3; k++)9 Z! g: X5 P' D
- {
9 J' Z4 l0 Y+ _* [5 @ o - Sensor_Data[k] = 0x00;
. a' n, h4 P1 f, ]9 p* r, m4 h - 9 _- `% r; I7 R+ i
- for(uint8_t i=0;i<8;i++); g: K% A- g& w1 `
- {
- S8 l( `6 \" ]( I8 s* J: F+ G - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
. e7 e8 K7 m* ~# O - HIGH_level_read_time = micros();
# Q6 w8 h7 \& K' ~ - LOW_level_read_time = micros() - LOW_level_read_time;
; `7 T4 u7 J9 i! b" W S
; b3 ~- g" V2 t, @- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )1 N5 O5 J* `2 Y+ }5 r- j
- {
' V; }- i: c3 E0 \$ t) Q/ B* r - return 0xff;
9 L8 o3 z, ~0 H, z6 T- w - }
4 D% y; q$ O5 e7 d1 b6 H9 _" I - , Q/ w* ? d( H
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 Z: s9 q9 A$ C4 L8 z - LOW_level_read_time = micros();
2 U: h' n* Q9 U$ P; z0 u - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
% |% _ ]" D- G* N! Y J
8 W# F+ s% n3 O c, f- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
4 A7 q5 S; e: q - {
: N6 D3 e" ^; |+ u4 l - Sensor_Data[k] |= (0x80 >> i);
. |( L2 p5 {1 M9 B7 {& T - }7 f7 w F- n+ Z0 y8 r
- else if(HIGH_level_read_time >= 100)
2 L6 A0 _2 |% I( e - {
, V: `9 G1 `& e: P! a9 ^ - return 0xff;$ j% I$ G. e! A# k- a5 Q
- }
: b" ~6 d0 i) S% u1 R5 T3 O) S
7 V) w% Y" F- ?# x. K5 \3 w- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)' y$ I& i3 C2 n1 W
- {( z8 Z0 ]0 y* W B, O8 A
- return 0xff;
- r# a) n/ |. z; T - }9 X. \& j! j* K
- }
8 |) K- u/ v m - }4 e2 R& o; r& [3 A; G' p- H, Q
- ) Q. W9 w, W( r1 L! k$ g
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level* _+ f& A: W, U/ i" G1 i! h7 q
- HIGH_level_read_time = micros();
; s; A& ~ A( F1 K9 Q, }5 `$ G - LOW_level_read_time = micros() - LOW_level_read_time;# W, w/ @4 U2 [6 x& N
- " q! w1 m3 q8 g6 ~8 l! e
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
5 d# @' H: L+ K% P; h - {7 ~7 b h. h1 [9 a" K% U. p3 D
- return 0xff;
2 d( {) u8 ^( l6 t - }
- ^1 o$ ?6 z- ~; b6 J
6 n( b- H1 W- J7 ^- R- pinMode(_DataPin, OUTPUT);, G" y: }/ E1 C% J1 A
- digitalWrite(_DataPin, HIGH);
5 X; N% j9 V- b3 B - / A& L' N, h$ g- c' y2 f
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
5 w f% Z1 n: [; Q5 E7 A) W - {) K0 v+ j- @& p M0 B9 K
- old_data = Sensor_Data[0];
; G" P/ i' K: Y - return Sensor_Data[0];4 A# j, M* Q W+ Z
- }
+ |/ Z0 g3 y4 _5 C - else6 t- p3 Y0 E7 w. Z3 A: ?! h5 ?
- {* g9 k* w6 T' {$ ^2 O; F
- return old_data;
' S4 ~& `" d0 Q) k - }7 r @8 t2 v& ^
- }; i2 N5 w+ _1 ~% J- D1 v
複製代碼
% h; W4 r; e) F9 e3 h: K" }' D! iMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
4 }0 l# }. R8 a, v
% H B A2 s5 V; ?% h, E |
|