|
|
|# b2 @4 H! t, J! w' t$ o- #include <Arduino.h>+ Z {- M' I6 g- a" r) u
- #include <MeAuriga.h>
# R: C- O0 p- k8 W9 |$ } w - #include "MeLineFollowerArray.h"0 p/ o r7 n0 s. R5 s9 b. c
- 5 ?- Z1 h& t- q
- MeEncoderOnBoard Encoder_1(SLOT1);& e6 O: m. c) u d
- MeEncoderOnBoard Encoder_2(SLOT2);
/ ^$ Q" p0 @' i! F% Z" A7 O$ } - MeLightSensor lightsensor_1(12);
! U( z2 ^) Q8 m% e3 P# I% f - MeLightSensor lightsensor_2(11);
/ t3 f5 d" [2 y K+ ]7 U. j - MeBuzzer buzzer;9 W1 ]6 Z/ a5 e
- MeLineFollowerArray linefollower(PORT_6); Y6 b# z8 x. [5 U1 C, E
- , M% p5 q K4 J: Q8 k; b- h5 C
- #define Error1 1' e. ?- u; L$ N' ?& I: i
- #define Error2 2
8 [; {3 @3 S, ^ - #define Error3 39 U: J% F/ @; c7 `
- % X) B Y% ^2 b/ V
- #define Kp 157 O3 q- s* r8 T! Z# B
- #define Ki 0.15
/ U1 U4 _ s- {5 K) {! ~' ? - #define Kd 0.032 e- L/ F0 @9 @$ i q2 K E4 h
- % ]* N& P1 V& g( h- F0 I
- uint8_t sData;
( R$ R3 O3 w, r - uint8_t D1;) O: ?. z: w' D1 _9 S0 j
- uint8_t D2;
; @/ r8 |& t- t" n# @( ] - uint8_t D3;
7 E( u- v1 P6 e! @ - uint8_t D4;! {: N0 d. ?4 X) o' }
- uint8_t D5;
2 J+ ]0 c( K* q4 U1 c% g - uint8_t D6;
* y# a7 U+ Z. b2 u" u - ; z& C9 y6 F: [% N/ C* ]
- float previous_error = 0;
' N/ ~+ O0 S; g0 v2 `" ]) f - float integral = 0;
1 Y2 w: J5 t% \7 P1 F! q b - float derivative = 0;
5 B, M0 ` x; h& q$ Y: ~ - int Speed = 160;
2 a5 j) {- p! [ - float output;
5 z# c$ H* ^! J9 Q - 9 D1 ` `* J l/ }; _4 |# g
- byte Left;
! G6 q, o. B4 [8 P
2 ?1 [# ~7 g1 G6 x- void setup() " S e. u2 k, b" j, h( v
- {& |) ~9 }) U, w3 @
- //Set PWM 8KHz
4 m8 b; a+ |4 Y3 H" j' k, y - TCCR1A = _BV(WGM10); d1 z' y, @/ w2 |/ ^2 N6 Z( b! I
- TCCR1B = _BV(CS11) | _BV(WGM12);( m9 ]: @) e% l1 o+ r+ P: Q
- TCCR2A = _BV(WGM21) | _BV(WGM20);3 C; h/ f( g; }
- TCCR2B = _BV(CS21);
- S( N% C: M3 U3 c4 O3 m - Serial.begin(9600);
; \- P. {2 s6 v' f4 T# q - buzzer.setpin(45);
& e) G- j& @- x0 i1 K2 P5 k3 o - }9 m% c8 l! U/ x4 l2 L1 K7 w/ t
- ' Q/ E1 }4 {' W3 ]7 f k& J
- void loop()
& ]' Q& k5 o4 {& k* X4 K - {
& S% m! T" w- ?. Z( N# ] - while(!((lightsensor_1.read()) < (10)));
7 f& \8 M: S- Y - buzzer.tone(1047, 500);7 A4 M$ v9 q5 ~1 ^' S$ Q% u
- delay(500);7 H1 K1 o5 U$ {0 \
- do* L8 t1 M& Z* `" D% n. U8 b, F R
- {
5 @! [+ ?; Q& V# S6 A" B/ s6 E3 _ - int Err = getErr();
, R8 j9 G9 {! A0 W5 |9 C - if(D1 == 1)4 E8 O+ J) }' ^7 j' F0 }# N) c F
- {
& W- K' i2 l2 [% n- F z0 k - Left = 1;0 |& c+ d7 @' E0 I
- }- P* S( |; |! P# I+ t5 g
- if(D6 == 1), E$ I$ A# e2 g# @4 [0 H- g3 s
- {6 u* {1 W$ \6 d& O+ e
- Left = 0;
8 ?( W& r/ x# _ - }- X1 [ G, |7 l# c& @$ A3 |6 G0 h, v
- if(Err == 99)
6 @- `5 V2 V" ^( Z! ~5 M# p* I& R - {' T! g" `, b h% D$ ?9 d3 O3 O
- if(Left == 1)
. l: v' z) ^: d3 ?6 a- t1 k - {
9 b+ Y3 \ H; A2 P: h6 J- B+ m8 D, y& h/ I - Speed -= 5; U) O5 o9 k1 v8 d, C/ @
- moto(0,Speed);
4 h2 j2 L2 {' j; E# Y# Z5 | - do
# m# ~. t+ W/ T - {7 o5 L) k, W& m
- Err = getErr();. j' V/ B7 F0 ?# H2 x
- }while((D1+D6) == 0);6 P0 ^' U5 N3 ?6 @& l$ L
- }; [$ m( b0 t+ L& Y5 |) x9 \
- else
( r. [1 I3 J/ P1 ]& u - {
9 u7 U6 Z# A) W) d. Z - Speed -= 5;
( g) u- } f0 q! x2 y) m - moto(Speed,0);
/ ~& r2 m0 v. a, C - do
2 F% N% f; S( m3 d - {& x' y' w& n+ O
- Err = getErr();
/ o8 p2 m4 T# U# q# D* b& T - }while((D1+D6) == 0);# f, t( N+ F* x! \0 I& v
- }
' M' }5 G4 c' U/ H" ~ - }
0 F' D( a5 e" m& {3 }6 @9 N - else
' E% T q$ Y$ R6 n: Y - {
+ @$ q2 }, X7 q - if((Speed < 160) & (Err < 2)) Speed+=1;
4 L& p4 n- H9 }) @0 w V e - if((Speed > 100) & (Err > 2)) Speed-=2;
3 c1 ~. ]8 G! _" u& O; n( L - integral = integral + Err;
* {& ]; Y1 }$ {) j - derivative = Err - previous_error;. r3 z% d, p. g, B: N) J/ s
- output = Kp*Err + Ki*integral + Kd*derivative;& c3 Y. I& y5 b+ [
- moto(int(Speed-output),int(Speed+output));
% {) q! N ?8 \' _4 J8 s - previous_error = Err;
5 N6 a% L% ~0 w4 n8 e7 P - }
t; ?0 a0 `5 {. V m a- L - }while(!((lightsensor_2.read()) < (10)));
! ^+ i! G6 l: o) j5 J - moto(0,0);- s; x; |' Y2 G2 l
- delay(500);# ]! D% w6 m& G# W0 g6 e2 q
- buzzer.tone(262, 500);% x0 ^5 n4 F7 D: ]/ m% M
- }
/ V: t' h" e$ X% ~ - $ [ F. k+ K3 y! w0 v
- int getErr()6 ?/ p. @- ~9 U* X
- { n3 U: H8 I" U2 M9 m* H
- sData = linefollower.getValue();
3 m n7 D5 |+ u: M- B/ j5 W0 r - D1 = ~(sData>>0)&1;
9 A! T$ s }; G: F - D2 = ~(sData>>1)&1;# b8 F( R$ m* w3 j
- D3 = ~(sData>>2)&1;
0 p- a" P; U9 O. x - D4 = ~(sData>>3)&1;
+ q# V, Z$ W8 m! D* x" G - D5 = ~(sData>>4)&1;
0 c: k/ s: T2 S b% N$ P - D6 = ~(sData>>5)&1;. G! @% K, [& Q$ l
- int downD = D1+D2+D3+D4+D5+D6;
9 \! D/ \# q3 Y - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);+ |" d- z% F& M+ b1 l# \0 N* g1 x4 l7 p
- if(downD == 0)
% x3 A9 u2 d# v8 ]+ } - {
* D" O# v/ s( `' d9 q% d0 Z& c - return 99;
$ }- f% |2 B& x) i$ e - }! C) |1 F$ q D! `$ v
- else, E E, o/ F8 d( c7 R5 u: ]# K: B7 u1 ~
- {
- l: [8 x: u& j1 i. y5 P" Y- N - return upD/downD;4 D, R( f0 q1 H& ~0 _/ D0 l% F' q
- }- N" U1 Y% B+ |& {# o$ a& w
- }! ~- u5 ?* ^: ~- }+ w
4 ?+ ^9 C" X$ N' t. ? u- void moto(int Speed_L,int Speed_R)" O; c- @* [+ K
- {1 ?9 u9 ?0 s4 R. T( ]5 q
- Encoder_2.setMotorPwm(Speed_L);
& E! g% ^* A0 e1 u6 S4 w2 C# [ - Encoder_1.setMotorPwm(-Speed_R);1 G( s3 {8 V9 S5 X
- }
複製代碼 ! u( I" B4 H) y
MeLineFollowerArray.cpp$ {( R) p3 t9 O d
- #include "MeLineFollowerArray.h" ~/ u( S* G( q$ D- P8 Q
_% [1 N( g F g! O- #ifdef ME_PORT_DEFINED
% l/ u+ W# j1 w+ D: l% @) n - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
6 a2 y0 g& ]5 S5 K$ o - {
/ q( Z& I* K+ F3 x, H - + t: t! s& \5 ^5 X) R8 ?
- }: c( Z( r) i3 S( [: A$ ~
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port), P* y- L7 _/ ~1 a" X
- {- F1 d% _- }5 s8 a+ m4 x
- _DataPin = mePort[port].s2;; I0 Z" J6 w; n4 |5 h: P& J
- pinMode(_DataPin, OUTPUT);
- z9 w/ M: s# B/ | - digitalWrite(_DataPin, HIGH);" u* n2 c0 \+ N5 X# G/ b l
- }6 |; j; E' T% a5 |& m
- #else // ME_PORT_DEFINED
7 T$ f L( _! z. Y+ ]8 B* J) w4 H* a - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
5 F6 ^2 B/ D" I3 Q/ U3 D - {
/ S/ x. C1 R0 i - _DataPin = pin;
1 F( |( [) H( Z9 w - pinMode(_DataPin, OUTPUT);
9 p- |0 ^ s$ w" \, H. S - digitalWrite(_DataPin, HIGH);
( Y+ T6 M% r8 I - }
8 r0 c6 y% Y" a N - #endif // ME_PORT_DEFINED
9 W9 u" a5 p; }% N
6 H4 V8 r" x8 d5 b4 f
9 @' h/ E- X& D0 f! I- void MeLineFollowerArray::setpin(uint8_t pin)! g) w5 f8 U; Z
- {& W8 H7 q& e( j G
- _DataPin = pin;
5 P" T' A- o$ N' S" F2 f& b3 Q - pinMode(_DataPin, OUTPUT); H+ g9 a6 v- U4 L" O! v$ h
- digitalWrite(_DataPin, HIGH);. U6 z0 b/ |3 y
- & { ^# D& S2 @
- #ifdef ME_PORT_DEFINED ]8 ?% g6 N( ~% V4 }( b* l3 ?
- s2 = pin;
$ ^( {5 Q% [1 o4 [& R& I - #endif
. C; i. m/ ^+ ~ - }5 `8 \: j6 G* j! q B N7 T
- ! M/ H6 }- H% [1 ~+ D) d
- uint8_t MeLineFollowerArray::getValue()
# U6 ]9 ?1 y H1 D$ d - {" f- j$ q+ l1 J5 Y7 G
- uint32_t LOW_level_read_time;3 F2 F5 D' f3 n8 G8 |8 u
- uint32_t HIGH_level_read_time;
: p4 i9 l: p' }1 S' M4 m - uint32_t time_out_flag;0 O; p- ]4 T- c8 `6 H6 C
- uint8_t Sensor_Data[3];7 x: h: \; A$ N4 x1 y# a; Q
- static uint8_t old_data = 0xff;& s: @9 b) T% g; b( A3 [
. I; U+ ?/ r1 b- e- pinMode(_DataPin, OUTPUT);' S6 \" P! w" v7 L- C9 z: s) V" o7 x
- digitalWrite(_DataPin, LOW);
' m3 a8 Y* u! w/ q - delayMicroseconds(980);
4 p' w5 z2 ?0 E$ `, T3 y - digitalWrite(_DataPin, HIGH);
! ~4 l: D( N2 D! w4 r# ^
$ C6 b, {/ R4 J8 b. q8 I; s y- pinMode(_DataPin, INPUT_PULLUP);
9 N8 t( \# j9 M+ D2 \ - delayMicroseconds(10);
3 \; j+ M+ C) N. Z) w K$ Q# k
0 f* R. J- P; ]4 {4 F- time_out_flag = millis();
8 `4 y) c; J0 ?5 L& S - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
1 _/ x1 d$ r1 k* ]6 `4 l* D" \$ k
7 I: Q. k7 A- D* W2 H' k+ g- LOW_level_read_time = micros();/ X( L' U6 r4 {5 G2 Q0 g( N
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 d+ Z" V2 M; G* S# ~ - {. R* J' m: P0 t6 a
- return 0xff;
# k$ Q4 g; \( z# _ - }5 a V3 A% S" ~# D) p* ~6 X. d
- 0 s) L; _1 V' R
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );% ~ O0 r* w, I2 g9 E+ v: a4 Q6 e0 P
- 8 V' {! g- t9 G* u, u2 ~
- HIGH_level_read_time = micros();
$ E" P, `* m& [6 p) ~ - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
0 g5 e; F" |% c$ ]
# j0 m3 Z; a! B- a% P9 E0 i: L: d- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out& }4 X+ {3 g7 {$ h$ S
- {, I. }3 I2 @* |+ K( L- B
- return 0xff;
4 g4 W) |8 |, x X/ e1 `( D% Z - }( W5 k8 t& V0 c$ W- i% h6 X1 `
- % J/ i* r, ~. w' S: `' ]& p
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
) E e' E3 Y q' X6 X6 n1 a) t - {
0 g& f: g% ^+ Z+ M* e' R - return 0xff;$ @: J+ ~1 \2 a
- }
. N9 W8 s7 H3 N& D. X T - " G2 Y+ [# Q) r0 \; \) W+ ?
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# F9 f1 U5 ?- ?: e* h - LOW_level_read_time = micros();" P6 X7 M% g9 g
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level) H3 h- d+ }( j' G3 N
- + K# M2 f: V- U+ ~+ ]
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ T- p; h* S+ L9 B0 |
- {" O, w7 z0 w- A% C/ Z. F
- return 0xff;& G1 v+ s5 R. ~" N# q0 t: s
- }1 |1 a/ t( V8 R+ S$ ^$ }
% n$ q [) {7 @' I8 f" O+ Z+ W9 |- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
/ `, f2 s5 `6 {: _8 n& I! C3 _ - {
) W- N o2 ~& _ - return 0xff;$ I$ J: m7 a9 E3 R
- }$ e' A: U& f/ N4 ~
t# @. g/ S: l$ T4 x- k- for(uint8_t k=0; k<3; k++)& n, V: O( m5 Q+ Q( C. K: \
- {
# a$ K4 B6 x6 u( r! `4 J$ u - Sensor_Data[k] = 0x00;3 z2 ?- E7 ~4 k% J) a3 H: s8 T" |$ J
- " [: V3 s3 n1 K5 h1 E( Y
- for(uint8_t i=0;i<8;i++)# Q7 q S9 u' {, j% d6 {, l# w
- {
/ c% u2 z/ K" b' p9 i - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
; U, s8 _7 @( }; }- a& M* B- h: B! v6 U - HIGH_level_read_time = micros();
2 ]' I2 x" E5 k! O& M" N - LOW_level_read_time = micros() - LOW_level_read_time;
3 u& `# C7 K/ L7 @6 V# z' E- s+ x w
9 I+ u, z" m8 j; e( [/ T- h6 s- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ), F; _, a; x- ~2 w# @2 D4 x
- {
; {% f9 k- q3 ?+ K: h - return 0xff;
5 ?2 o+ b( I& M# f - }
, T" ]7 `2 ]6 e8 T7 E8 y' V' X - 8 Y2 { q( m. q0 S8 \
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
1 ]) @/ R$ V. v$ Q+ v - LOW_level_read_time = micros();2 t0 O: n" [$ H ^6 N
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
. q1 c! D* w/ ?" M! M
* t* M! h x0 x' _- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
$ w4 @6 _) T. {; i2 N - {
" k8 |2 ~ r5 v- H: r8 c u - Sensor_Data[k] |= (0x80 >> i);
" w! g& h- `7 t8 j - }0 b2 ]' X! e. r7 }1 o
- else if(HIGH_level_read_time >= 100)$ i, N( O0 U) y/ E& M
- {9 J* b$ Z+ w$ l8 S
- return 0xff;% E2 ? m1 }. G6 Y2 f& x& [
- }1 n/ B3 N2 t9 j4 G) E8 Q" i5 d
- ' u% |- v7 j# v6 O- x
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
4 c! F9 ~: Z w$ V& z - {
$ w5 e- h% J5 M# `3 H# H - return 0xff;1 j5 k: c8 F* P. C
- }- d+ h1 ?/ D7 W! J# {* D
- }
, a7 g/ |) c4 n- I$ V2 I - }
% L) w9 {3 V1 J0 t4 a7 |# ]5 {) v, D - 6 ^. q8 V4 b6 R
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
' y9 h, @) F( a U; V- A" g& K( F - HIGH_level_read_time = micros();/ d9 W$ ]7 [1 P6 w% k! o, R
- LOW_level_read_time = micros() - LOW_level_read_time;7 A$ x4 i6 C5 @2 Z
- : V' i4 w& w$ ~# |3 G# ?
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )3 l* J- }! i7 \/ Q6 d/ z
- {
7 d+ j! z* |0 x/ Z: ]5 }" ? - return 0xff;
5 h9 {2 ^6 }& T$ P# J- Q' V - }5 r1 c' Q$ c9 d2 n0 ^1 L/ j o) I
- 9 F$ i! d$ ?0 f( A3 N6 v4 G' w
- pinMode(_DataPin, OUTPUT);* P$ q) o- Q7 F
- digitalWrite(_DataPin, HIGH);
# Y) q) u" o4 R/ n, i1 ]8 ^+ Y6 v - ) U8 W2 ]% O8 {3 ]) u3 s- y
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
/ \5 d$ |: c2 U' s" u& [$ d, B - {
9 U4 n) U+ C3 \% E E0 K1 K6 w( ~. `3 J - old_data = Sensor_Data[0];$ @& c9 H C! U0 I0 l1 _
- return Sensor_Data[0];
3 g" n* a8 S* U7 T( R0 M - }
$ {) L: T* y' P: X - else
* p4 ?) i- N4 b' [/ X - {; b+ n6 B( [6 O5 `0 n, i/ t y
- return old_data;
; h1 f8 _" S/ s/ q9 v - }7 ?6 \+ w2 B1 o$ M+ T o
- }
. Y( @4 R2 G. F
複製代碼 / r- w+ s, S) u: U* A# s
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
, p) q, a% H% i9 I) h) p
7 c% D6 Q* ]* o Z) x
|
|