|
8 C0 g: [& m/ r2 G( [+ F3 ~. J: A
- #include <Arduino.h>
- O' s. A8 |7 M0 e+ L$ [, [ - #include <MeAuriga.h>
* x- ]9 b0 M4 f/ A* j+ w; n - #include "MeLineFollowerArray.h"
, y4 ?9 ?- G9 ^7 t- j5 N, V6 v - ' l4 R$ _, \" R, ~ d/ O) A
- MeEncoderOnBoard Encoder_1(SLOT1);' I7 P+ \! X& e7 s6 c
- MeEncoderOnBoard Encoder_2(SLOT2);
: P/ G9 r- I% ]2 c - MeLightSensor lightsensor_1(12);
- ~% z- j, l% j2 y# ?: V - MeLightSensor lightsensor_2(11);* S( k- f9 k7 k ~% C2 I
- MeBuzzer buzzer;
4 i7 | K; y6 S( u' Q - MeLineFollowerArray linefollower(PORT_6);5 Y' C8 h; Y- ^: t4 g' d
- 8 ` H( k) b# u
- #define Error1 1+ p# ~9 t( q7 v- L3 C$ C
- #define Error2 2
( e1 {$ y) Q* G% o/ `7 e; c - #define Error3 3
% C8 F/ i& L0 o
5 C$ e% j: N9 |2 O( v' ?1 m' M% }- #define Kp 15' h! n7 D! z- l+ O e8 V5 a% Y
- #define Ki 0.15
. G9 t" v6 e. Q. B% |1 u - #define Kd 0.03
8 m# u0 c9 d! K& l$ W" b7 d - 2 [, |8 t& G, ^9 d2 t6 y; ~
- uint8_t sData;
% ?3 R/ [1 }1 f; g - uint8_t D1;
: d5 q) l1 R( Y* E - uint8_t D2;7 b7 E# N- y3 F+ c9 r2 R
- uint8_t D3;
" J9 U( K3 r+ D1 y; }/ M - uint8_t D4;9 N) t& v. D5 o5 v! ~- t
- uint8_t D5;# y1 w: Z" v$ y- A
- uint8_t D6;% b7 }3 `" X0 e" _* I8 S
- 2 k7 ^$ H6 x9 [; k( \, N8 t7 N! V
- float previous_error = 0;. N' [5 A. N2 k5 q; V5 p. I$ ~
- float integral = 0;$ y/ a! d" U7 ]- X/ t
- float derivative = 0;# \: U" ? k& j- a/ i" m3 @$ ^' ]8 \
- int Speed = 160;
! K1 V9 S/ c5 l8 k$ i - float output;0 V' O p2 F3 Y/ z8 `
- w, C" ~+ w, v5 c- byte Left;
6 Q- P2 {* c2 e$ H& E
& J2 J1 H3 G& G3 A: k- void setup()
9 z8 u8 @9 y/ |& O, ~' b' ` - {
0 U `# u6 \ `" W/ L$ u - //Set PWM 8KHz' p6 v7 Z% @5 K
- TCCR1A = _BV(WGM10);" q# P ?0 Q3 t1 z) d
- TCCR1B = _BV(CS11) | _BV(WGM12);
2 ]8 x- q# @4 t- c1 x( N- V - TCCR2A = _BV(WGM21) | _BV(WGM20);
6 r( R: u9 d* I8 c$ e# x* t3 s - TCCR2B = _BV(CS21);
" G+ v# [3 H2 m; E7 L; Y3 C - Serial.begin(9600);6 e8 i* `/ Z( J4 K7 J7 R# [1 t
- buzzer.setpin(45);
/ v+ ~, i" `" i2 F" _$ ~. V0 c' K1 v - }; \" y/ n" E, h- Q3 u
- / o& ~+ V3 W+ Z2 @" q( H
- void loop(); [# l1 x" _2 e3 w1 U" V
- {: g* l* G. G) j( s5 a: q- e. F
- while(!((lightsensor_1.read()) < (10)));% D* p1 q. S( v- b% l4 k
- buzzer.tone(1047, 500);# L; [7 z- |/ ? ]
- delay(500);
- m. a& M4 |2 {8 @6 ` - do& d9 J0 R9 k. X2 Y; B
- {# {! n: V4 P6 R4 L# M
- int Err = getErr();& P9 ^7 {1 I: s
- if(D1 == 1)" b9 v! C( C$ X5 a1 ]( m
- {
6 j( U0 T/ x0 S! k5 ^% @# e o* x - Left = 1;
6 Z! A4 B! w2 _) Z/ j g& Z9 ~ - }* ]- h+ @" }; Y8 p- z) h
- if(D6 == 1)9 t8 ] b# g- s4 v
- {0 T5 Q/ ?7 ?; W( N3 a
- Left = 0;
: q/ w% G: E9 x& b0 U; K1 ]: E - }
, D; u6 R! w3 K - if(Err == 99) U8 L& t% L# t$ p4 N& p; _
- {8 x* ^! J. y! o& Z
- if(Left == 1)
% x8 \1 w. T) ]$ M - {
7 o. D7 k7 Z9 e - Speed -= 5;; G3 X: g: O0 \* W v8 F5 r
- moto(0,Speed);
3 Z1 O9 q1 G" l; ? - do
$ s* p$ e/ T1 k; T. C - {7 F0 O! ]+ v- i& j( z* O( G
- Err = getErr();
/ J$ g3 ?& L$ q1 \5 ^/ t. } - }while((D1+D6) == 0);
# N* c1 ?! D* ? d - }
, n. L% v4 E) }- t; d- L, T0 | - else1 @' v& m. B8 _. Y9 S, \5 O ~
- {# [9 n$ p4 D3 H
- Speed -= 5;
" P- {* M. a1 b/ u* E2 o - moto(Speed,0); & u2 J# M* U) Q9 M' N/ T* A
- do
" f. y) J: p9 U" x - {6 z; b) P1 ]1 \! b- ]
- Err = getErr();
2 q) f( u- U/ v" h! z) _, C - }while((D1+D6) == 0);2 f& q3 M$ B; C( X
- }
, _1 ~: n/ j9 \6 y) M - }3 Y: O' k( G6 ~
- else8 p( _1 r+ k' F
- {' i: s0 S0 o) \, t$ l
- if((Speed < 160) & (Err < 2)) Speed+=1; 0 y( p9 e7 z8 J$ Z4 W( S
- if((Speed > 100) & (Err > 2)) Speed-=2;
# q* k( e9 t: r/ y& F% d1 m - integral = integral + Err;
' Z7 o0 z, x6 u# u# d - derivative = Err - previous_error;& e( h8 Y5 @ T
- output = Kp*Err + Ki*integral + Kd*derivative;+ n' A1 |% l. w' h
- moto(int(Speed-output),int(Speed+output));
6 \ _8 c& s7 |5 z+ j5 K - previous_error = Err;
! K7 s! U1 o& e' Y3 @ - }
4 F; u0 B# e3 U: `( a6 I& [3 z - }while(!((lightsensor_2.read()) < (10)));
) H, _# E8 m( ?5 f* W( P - moto(0,0);: |7 Z7 G* ^1 f# m7 R+ C( V& E5 q
- delay(500);9 z7 o, C' _* f5 h# X/ ]
- buzzer.tone(262, 500);
- r* K1 m$ L* t M6 z - }, K4 p7 }! H" c: r' J9 L
- H7 V( Z. T# b8 w& R" h# X; Y2 o- int getErr()/ A5 `/ ]/ H$ S. S% g; V
- {
8 \ W& v$ O4 j' U7 [" i1 i - sData = linefollower.getValue();" ~$ Q! x* M* `" ?
- D1 = ~(sData>>0)&1;% e. V3 ~- }% f9 g7 y
- D2 = ~(sData>>1)&1;2 k1 H7 u4 p' U1 m! ~; B. m
- D3 = ~(sData>>2)&1;
8 N2 Y$ X5 S% C+ R8 p' M3 @" m - D4 = ~(sData>>3)&1;$ ~) ?$ B5 M; o9 B3 s6 [
- D5 = ~(sData>>4)&1;8 b5 ^$ Y3 s. c! o F8 t; B, {0 c
- D6 = ~(sData>>5)&1;8 N4 L& L- r% _- J& O" u c. R
- int downD = D1+D2+D3+D4+D5+D6;
: s& j: _) J( J) W/ A - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);/ I2 F2 O6 A0 `0 y _( m
- if(downD == 0)6 e% A% i- V/ [; C% m, w
- {
1 w ^ ^; ~8 _% {: ]$ R9 B# |* x - return 99;
) y1 J% j2 B0 A% e5 n - }
7 Z8 E. y( M' V+ w7 j7 C3 o - else
& W* t$ P$ F8 k. ~4 Q - {
$ s! ^" ~% f, h5 y% B0 f- x1 R - return upD/downD;
6 W2 Q+ E; U1 N - }
, e( |0 E+ R# K% k$ I - }
% r$ O0 \% R3 p U/ H7 I - N9 A* E! Y, Z Q. F. C
- void moto(int Speed_L,int Speed_R)
1 ^& L3 D" r; Z+ n( M8 f - {( n* W) ^$ H: F+ Q% p8 T
- Encoder_2.setMotorPwm(Speed_L);! X7 a, f9 e( Q/ M
- Encoder_1.setMotorPwm(-Speed_R);1 A6 [0 ^7 g4 ~' }
- }
複製代碼
4 g/ Z# X7 G2 b9 a& _3 s1 m$ G" ZMeLineFollowerArray.cpp" K$ X3 e$ F' v0 \& u
- #include "MeLineFollowerArray.h"
0 g! d+ F; p& m8 x( K' _$ n' \9 @
- t+ \/ s$ I) d* f- l% W7 h- #ifdef ME_PORT_DEFINED/ q' e1 p) @" x- ]
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
- _4 a ?8 a/ W5 E2 Z3 ]; A3 | - {, S" y% U) l+ f" \( j0 b7 Q( ^
- $ \ _9 I9 _# b$ ^
- }( u! s( C9 V9 f7 ?, G2 { ]6 P
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)* a' ~1 e" w, z. r. F8 w0 J
- {( C6 k( n! O# W: z% {& h3 ]( M
- _DataPin = mePort[port].s2;! p* F' e- X7 U$ ~9 R, `6 W' O
- pinMode(_DataPin, OUTPUT);
+ K! h! ~& b8 Z$ E - digitalWrite(_DataPin, HIGH);
/ M: `1 |/ \8 U: }2 ]% o( v8 L; } - }5 H# b1 g. ]0 }7 [
- #else // ME_PORT_DEFINED
5 n) V6 k1 F0 h2 \- V - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
/ W2 D' u T% E f/ y t( ~0 g- c - {/ F4 c( O0 h4 Q) b) h+ Y
- _DataPin = pin;
4 ^6 N9 h; c. q! g. C" z, a - pinMode(_DataPin, OUTPUT);
! O& K6 v, o/ g }# F - digitalWrite(_DataPin, HIGH);
5 E1 l! g R. ]0 R% g, k - }5 Y* ^* @/ G# {9 H* G; @3 K4 A6 k. m
- #endif // ME_PORT_DEFINED' t/ y2 I$ h/ u/ q- P$ Y6 T0 _. P9 o/ B
* o4 [' b( J Z' |. ~
2 W5 _/ }( [3 B$ H* s- void MeLineFollowerArray::setpin(uint8_t pin)4 d& ~+ J, s4 \( f) y
- {
2 ~' G5 c, e1 s S! k6 ?/ s - _DataPin = pin;4 U- m+ Y$ Q4 V* |+ }' o1 d7 N
- pinMode(_DataPin, OUTPUT); : Q& D1 F2 E) t# Z/ y. f! k
- digitalWrite(_DataPin, HIGH);
- h* j6 J- s a* _, L
" }( Z( c+ C" l1 j+ x- #ifdef ME_PORT_DEFINED9 j7 ~( m) ~) W& i* X
- s2 = pin;% F, @: I( T5 ~' n j
- #endif
% ^- {$ f, I! U/ d5 w* J - }# H/ a9 `5 X$ j
- / O9 s$ \+ ?- N8 u5 a+ K* j
- uint8_t MeLineFollowerArray::getValue()
( s& g2 H' W* J - {1 L1 V2 q! @5 f6 H! h' l
- uint32_t LOW_level_read_time;
' f- C G! Y, T* B* H - uint32_t HIGH_level_read_time;
6 b2 y3 V. j2 a2 S, @, v - uint32_t time_out_flag;
0 M) M/ f, y x7 g) a - uint8_t Sensor_Data[3];' \4 h) p* p. ^" } r- g
- static uint8_t old_data = 0xff;
' |8 {3 h+ P* G* v/ t - " B# Y% X/ m9 A8 | E l
- pinMode(_DataPin, OUTPUT);5 q' S$ b$ E2 S* B4 A* i
- digitalWrite(_DataPin, LOW);9 I6 w# t/ n7 L0 O
- delayMicroseconds(980);8 S* _/ a: a5 R1 b- V8 T# @
- digitalWrite(_DataPin, HIGH);
0 V5 ~2 D' B, M - 2 s W* k; ~% d k0 o
- pinMode(_DataPin, INPUT_PULLUP);
$ m! Y: R! ?- H6 z - delayMicroseconds(10);
0 l; k- D+ }! s# U$ D% D
3 M) k' D$ b" t0 F! n% o: @5 @- time_out_flag = millis();
8 }- p D5 S+ m$ e: a" u2 U - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );% O6 Y K2 [ |+ F$ [
- 1 }! T0 }$ Q. y/ d Y5 v4 _
- LOW_level_read_time = micros();8 d8 c' B, h4 E8 z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
9 c( g; N5 V$ H8 `/ _, a2 J# q) k7 G - {
; Y. Q5 r6 J( [# s5 ` - return 0xff;
! [" {; |5 s% D. w9 |2 }- ? - }
$ [1 T* r: n- f - ! U0 c% B2 A% U$ W9 n* v0 F+ O* @
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );3 K4 ~; A0 f% V4 n& k
2 _4 H7 k" x/ F- m* C( F) l& \- HIGH_level_read_time = micros();
7 @0 b/ G$ H, I6 n0 j* E - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
, ^& V. s. z# C( r- v - - _2 l( J! V& S5 i8 F1 H
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out0 L/ \! l% n6 `
- {
' G( Y6 R( s4 ?- {) E4 Z - return 0xff;
2 o+ b5 \; Q& n. N' ~, A( W- c' L - }
6 c) C* J$ ^+ ]/ i
+ L% g; \9 R& ]3 A9 t9 x9 `- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
8 x- b( y7 i; S |& U# G - {! {+ d9 k3 G3 |) V0 b
- return 0xff;# B. @) O+ K: V9 O+ ^7 x
- }
) Z4 t+ e* _4 B! A/ s% @4 ?
8 y9 j, X2 u( e( P- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) \9 @0 M5 i4 n
- LOW_level_read_time = micros();
9 c4 D8 _% g6 Z7 I5 a- v1 j - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level4 w" w' w- i' E/ P3 P
- ' d+ J3 D. W; ^# P' v( E5 T9 \+ L
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
. N4 ` Q" S S( V6 J - {. T. [# `5 ^- w) ]$ M" m$ D
- return 0xff;
# W4 p- I8 k0 f! Q0 ?9 n - }
A, m" {2 r" E f' I( V3 H
0 |5 o3 {9 F1 m; Y- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))6 H ?8 S0 `5 V( H+ r# _
- {
8 q" N& X2 l6 B0 D - return 0xff;
7 h/ j5 y8 s( D( c/ y$ F& ` - }" }6 ^# ^% V$ X! I
G& K8 r) N& D7 F4 L- for(uint8_t k=0; k<3; k++)
! T. @& l ^7 ~ - {
# C7 A- k, l$ C! ? - Sensor_Data[k] = 0x00;8 ]1 Y1 t. {" D. J
, T3 d' _. [. c7 Q( Q- i- for(uint8_t i=0;i<8;i++)( ~6 J J" L; F6 [9 G
- {) S) @3 g( b# A% }: k1 x
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
% {) }5 R- F( {- i- K - HIGH_level_read_time = micros();2 t8 |, c8 b& `9 z
- LOW_level_read_time = micros() - LOW_level_read_time;* N6 \0 n* y. }. f
- " J2 x: U3 a+ I# o8 }$ l
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) d9 h9 B0 J* V* x' j% b
- {
$ N8 m' x+ M0 F- c; ` - return 0xff;# R; L' H0 z& R3 T: a
- }: o% ~% S3 [. i0 X" l9 v) _
- ! c8 A( g) s0 P! j8 y9 K2 ~8 V) O
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
0 ~+ o6 Y! L0 V, e6 J - LOW_level_read_time = micros();5 ~7 T4 t) E( M% r
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level# @+ K3 v9 M9 a4 u5 A4 I
- : e' @ W4 {8 H o" [
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1) H7 ]! t& ]% S4 T- b0 E
- {" B7 g9 {, {8 k/ I, X8 h' ^% P
- Sensor_Data[k] |= (0x80 >> i);
- H! r" d r) G1 y3 M v$ F - }' [# @, c% O5 Q- F. g! i0 J
- else if(HIGH_level_read_time >= 100)" A3 x4 C& G. ]0 Q5 q
- {
! t/ R' W. O* P" U - return 0xff;
0 O$ R" B# ]8 \. x N: u - }
- i9 f0 [9 ]$ ?, @8 V* u2 H
( u; Z2 W0 D/ s, v" u, H6 I- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)! Z" W$ Z% y; B% v5 n* c
- {
% _) h# i3 b! S- q - return 0xff;& l, F- V# ] V
- }
; b1 O- D0 }$ c1 O- D- P+ d, Z& x - }& v' {( x! A O& r) v9 q
- }
# t0 E3 L( }% T2 \9 R C) t
$ |0 s9 [- y+ x4 m8 H, o& {& ]- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level$ _# w' F- v( X
- HIGH_level_read_time = micros();
1 s9 F) L' e; a - LOW_level_read_time = micros() - LOW_level_read_time;7 E% U: `) |, L2 p; w( y: W) R
- 3 h% ~" f4 a4 i$ [6 s8 @- C( @
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ). d% o r6 v, v: {2 a- Y
- {
9 O5 @' {% n3 o, \, q- ^5 z$ F9 W - return 0xff;
9 j+ f9 u6 \! L! l - }3 ~" l z" d% s) T. }
- 2 \6 G% s2 G8 k( w1 p
- pinMode(_DataPin, OUTPUT);7 W5 W5 H# F a- S* Z* }3 }
- digitalWrite(_DataPin, HIGH);( B6 g4 u( U: m( ~$ D: x
- % ~5 |7 x* i/ _& N" I" F, D
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
# H' P1 u- M) ]+ h L/ Z: @ - {
: [* m! Z% P1 y/ [1 ` - old_data = Sensor_Data[0];
8 x! O+ Q; j( j7 v( w - return Sensor_Data[0];
+ f. k3 u$ V. q: b5 {9 N - }6 f' V; X1 I5 U1 q" n
- else
j9 @5 V6 m1 R$ x - {
6 J/ x+ D! a a6 r - return old_data; L( F1 ^( p# t. R# I
- }' y3 l4 A4 a) J! X( W7 e
- }
. U0 o+ `6 G+ j! r' O
複製代碼
6 {7 Q2 o$ {2 t& E# j6 m* }MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
, B* Q1 I2 c% M
% Z" u5 p, e: W$ _0 I9 q5 ] |
|