|
|
7 v6 B K9 Z6 [
- #include <Arduino.h>3 b3 b5 R6 R; U) @% a
- #include <MeAuriga.h>: t- q& G' y: V* S# n. i& ^
- #include "MeLineFollowerArray.h"( f6 Q2 o; j& Y: u9 W P
- G0 Q" |" P# K# e1 }0 k- MeEncoderOnBoard Encoder_1(SLOT1);8 k8 j' x; L) b2 @2 ?
- MeEncoderOnBoard Encoder_2(SLOT2);# s/ S) E# y$ D8 l- q6 Q3 S
- MeLightSensor lightsensor_1(12);
|3 |8 f4 F( [+ } - MeLightSensor lightsensor_2(11);) D8 x. V( w6 R9 B6 }
- MeBuzzer buzzer;
3 p" U7 Q+ p( {) L - MeLineFollowerArray linefollower(PORT_6);% ~0 r& C! f+ H7 k3 _- k' e
( q+ M! c g+ b- L$ w/ ?- #define Error1 1
& U5 e' {9 H: ` - #define Error2 2! L# \) {5 c m0 f5 w
- #define Error3 3
+ y# c! g+ B* Z8 E - + q0 K8 B5 m) p6 C7 o- t
- #define Kp 15
" _9 x% X' @7 L - #define Ki 0.15" w# b* L6 b, N7 B
- #define Kd 0.03
; T3 J' d* S* K8 U' p8 f+ K& \ - 4 d& }2 Q6 p8 ^& X. K+ \4 s
- uint8_t sData;
; S7 c; c2 m; z2 `* A7 W; ~0 T* y9 D - uint8_t D1;
6 N0 x$ o$ M9 t5 |5 n$ V6 Q- K - uint8_t D2;0 r. ?, ?' f' h, Z
- uint8_t D3;
) w2 Q. ~1 l$ D I- b - uint8_t D4;
|' _" @/ i' K' ]" S - uint8_t D5;
# S' _( t, K8 F3 c - uint8_t D6;- s" `5 s6 k+ V0 B- ]
2 G' l% u4 u0 }" ]/ M" o6 ?1 ?- float previous_error = 0;- C2 W8 }! K0 g$ k9 O: Y8 S) s* G3 N
- float integral = 0;3 I7 o \. a6 c4 I1 c" u
- float derivative = 0;
: y% g. Z1 c( t$ U! I u! T" U - int Speed = 160;
' w) I1 _ W5 Q - float output;
7 n( C9 h, k: L7 C - 3 W+ S, q7 K8 r
- byte Left;; c% k0 F* I6 ?9 ?$ o+ W# ~: L! s3 ]
- ) \* B6 l( h4 M1 F% l; V6 e2 t
- void setup()
) L/ I6 R3 k( }5 B. C. Z - {+ g% i! X# z) T! w" B
- //Set PWM 8KHz0 B2 v+ d d/ d0 n: ~6 |) c4 t5 X
- TCCR1A = _BV(WGM10);
' t3 [; Z2 l. E; T9 H* i5 L/ s: v( R - TCCR1B = _BV(CS11) | _BV(WGM12);8 K" X' J0 t$ F o2 O
- TCCR2A = _BV(WGM21) | _BV(WGM20);
- c: Q9 O+ O+ A) V/ D/ ^8 m - TCCR2B = _BV(CS21);
2 S: O6 t! ]; ], k0 g! i2 z7 c; M5 \ - Serial.begin(9600);
% @( @9 L! M1 I9 I- w' R - buzzer.setpin(45);. u3 a5 C/ }8 S; ]) X! y
- }
7 X8 _% G0 m; N8 i; l1 Q7 U7 V - ( {" b, x$ E2 o: t) S
- void loop()" g' O* a' X; m- B
- {
7 q! t2 C& P; `' F# Z - while(!((lightsensor_1.read()) < (10)));
$ E9 ]! q5 ?9 Q/ N$ q- g - buzzer.tone(1047, 500);
* s9 x+ I, o& r% T - delay(500);
# q" b* V$ g% b. _: y- T - do
6 L- u, w7 h( c" w& U2 }: y! T - {1 C& a: v4 M( @ v$ ~: w: v
- int Err = getErr();& d- L/ C% X* W; O( J
- if(D1 == 1)- a1 x. A2 e. p8 A4 g
- {
( u' J$ Z. y6 s- C8 I6 s7 Q v7 z; V' y - Left = 1;4 v8 B: i: x8 U/ ?' k' `
- }
6 b5 b4 H8 B! q - if(D6 == 1)9 r( P( `0 c8 j* W& O2 o% {
- {
& G6 v, O) @( T$ S W: G9 Z - Left = 0;2 x( M3 h- j/ R& d; v9 N& x7 ?8 Z; i
- }
$ s: T# L2 T% `' p- l8 E - if(Err == 99)
- g; v$ k# N" H - {
; b; N' D$ z; |- |. D6 z0 O - if(Left == 1)
* V6 R. F4 q" i6 C. L' o - {
2 |7 j8 |7 T, v' B: J) w5 |. U - Speed -= 5;( Q4 h i, v, y X- Y
- moto(0,Speed);
: z5 j h- L7 r' R9 s' z U) F4 Q - do
1 I1 y3 x3 i2 ?8 m: z# @ - {# j# _2 S. V) [9 c% S8 @4 u" E0 j4 _
- Err = getErr();
/ _/ r, s1 q x. c0 ? C - }while((D1+D6) == 0);8 ^) b/ I3 j) H3 \
- }
; v' p3 G% l# @2 |7 ] - else/ i7 x9 t e1 k7 O, F/ E7 r/ F' ^
- {% _9 ]! I* r8 c. ^- r" _
- Speed -= 5;# {; O; F( |$ S9 R% B" \- {+ [
- moto(Speed,0); + A: Y. D) I; O) s! P4 _" O
- do8 U: B+ ~5 I, S5 W) F T9 K/ p
- {7 i0 b# v4 g1 Z1 C' P, N
- Err = getErr();
8 P5 V: ?7 a* X( _: M5 Q( L3 d - }while((D1+D6) == 0);8 H. j' s t/ F% z: m; n
- }4 S! n( z9 ~. B
- }& Q% Y/ g0 v* X' |: J m
- else+ q* C& {- ? w2 \- T
- {4 D S) D# `- f7 r1 b
- if((Speed < 160) & (Err < 2)) Speed+=1;
, D# W# a* J1 @; _3 o2 i - if((Speed > 100) & (Err > 2)) Speed-=2; 5 ^$ D! d i8 ^1 Z% j% R$ ~
- integral = integral + Err;
' C8 `" i" ~0 W. I1 U+ y9 } - derivative = Err - previous_error; ~" g1 W9 |( d% f
- output = Kp*Err + Ki*integral + Kd*derivative;
5 [% Z) z9 t# U/ l - moto(int(Speed-output),int(Speed+output));
" \' b6 i/ L5 D. R: E4 d. ^9 h f5 p, H - previous_error = Err;
2 j7 u3 e- G' T; y9 R - }
: m' \( a7 `8 ?% L) v8 N$ z& v - }while(!((lightsensor_2.read()) < (10)));
! U9 [# }8 B. ?6 A% U, |# ^ - moto(0,0);7 q* c/ e1 P! X4 M* u' w
- delay(500);6 t' K6 o0 M+ I' {
- buzzer.tone(262, 500);$ L. x4 |2 q0 [6 D
- }. ]( D) h5 `/ H
- 1 V0 W9 Q6 G" d' k! t" v
- int getErr()
$ b( ]+ K# T: ~( J9 E9 l8 n5 F1 S - {
9 A2 t7 C' A9 E) a2 o4 i; r8 {$ L - sData = linefollower.getValue();3 y+ a9 K( h" H& f0 E
- D1 = ~(sData>>0)&1;9 [- W6 Z! T) U
- D2 = ~(sData>>1)&1;
) @7 I* E/ z2 j& t. _' U4 F. q5 a3 y1 ~ - D3 = ~(sData>>2)&1;! e5 a, a- Y% o4 ^. Z. X, s# U
- D4 = ~(sData>>3)&1;. L* C7 `' S5 ^; X0 ?7 U9 {
- D5 = ~(sData>>4)&1;
/ [* C, X' k; s - D6 = ~(sData>>5)&1; u. x! \% C/ i( G* |5 f. j
- int downD = D1+D2+D3+D4+D5+D6;
3 @% y9 g' k5 t& c5 y6 O - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);, Z( h" Z* Q2 } P
- if(downD == 0)
. N7 z+ s( U) K5 k - {& ?6 n/ I' Z9 r) ]9 K0 d2 L
- return 99;' U+ J) r) [& r" ~8 A* `
- }
& j7 r4 ` I1 M) H5 e0 U& ` - else
; t' C! z) R6 ~8 a6 h - {$ s/ d$ |$ A, ]+ V
- return upD/downD;
! Y" B& @5 g! H7 ?7 o& C& D - }
" C! |- _; t$ F! Q Y* h - }9 M$ ^# M/ ~4 o9 y
- + r6 b* ^+ C% [
- void moto(int Speed_L,int Speed_R)
* A/ q# Z. g# G- G' j" H: U - {
6 z. w( A2 Z. ]; ]% S - Encoder_2.setMotorPwm(Speed_L);
* L- B" y9 w% t4 X# m - Encoder_1.setMotorPwm(-Speed_R);
% ?; \: r* j9 }1 u# ^ - }
複製代碼 6 C& G8 g! E9 F; E# m0 {$ s8 P
MeLineFollowerArray.cpp* T8 r$ S' ~6 h8 e, j
- #include "MeLineFollowerArray.h"( j* l' J& G i' ?
- 8 D3 i! Q' n. }' }% ^
- #ifdef ME_PORT_DEFINED
9 _2 s% ^5 a$ B* r$ k7 }; f' C/ j+ I - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0). W' g& v0 E# ]: j+ @0 Q
- {
C( w4 n3 v3 b% ?1 J# H! M
& M8 G2 Y$ d9 Y. u. Q0 h9 I- }) u( @; G$ y4 V' a( r: `- a) X
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
0 W6 i$ v. p! b& F. o; l6 R - {; W2 g% C5 p5 W0 c+ K$ H4 X+ E: \
- _DataPin = mePort[port].s2;; X$ Q" w- p2 W
- pinMode(_DataPin, OUTPUT);
" ~4 i9 d/ ?1 d7 m2 I0 j - digitalWrite(_DataPin, HIGH);
0 t% J% u' P/ C; N - }
' T9 y! B# Z" ?4 s% U - #else // ME_PORT_DEFINED2 i9 E+ b5 H& z/ \; S9 A
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)" p/ `8 _; `, }) Q
- {
+ P4 k9 w6 E# t+ _& u5 J0 ~ - _DataPin = pin;/ A! v. P2 U7 N
- pinMode(_DataPin, OUTPUT);
9 M2 Y9 s$ m3 k - digitalWrite(_DataPin, HIGH);
3 ?1 y. Q3 @0 y( @1 P - }5 z9 U+ a/ {5 J0 X3 m& N8 M6 }) I
- #endif // ME_PORT_DEFINED
8 K0 m( t2 P0 c/ V! h; s% O - ; U' f/ u9 D; y( S9 v; W; Q% f' l
$ |/ K1 G& N' n- void MeLineFollowerArray::setpin(uint8_t pin)/ A' L+ f) |- g) r- |3 `! S& W
- { f9 z" C( ]! E1 m1 |) [8 O, T
- _DataPin = pin;% b& R: k: _" L: N8 @3 U
- pinMode(_DataPin, OUTPUT); + \8 m# [ ]. g9 P2 J5 Y& K
- digitalWrite(_DataPin, HIGH);: z- l. T* o& w3 V- i: |# N! A) d5 ?( o4 H
/ m) [6 p2 i4 A) q& L7 S- #ifdef ME_PORT_DEFINED
/ R; _: j/ y- K! B - s2 = pin;
4 J: L: p& ]# h6 n. r/ J - #endif% ?3 y* k) y. |) W
- }
* q) n- b P k% u - % F1 A* L/ F; w% o8 {2 k5 {
- uint8_t MeLineFollowerArray::getValue() ^" X( ~- o6 R$ I- H
- {
2 Z. u; {/ z7 Q! e8 [$ {7 _ - uint32_t LOW_level_read_time;' A# b* R7 i# }2 N9 w3 K1 a
- uint32_t HIGH_level_read_time;
; d% L7 X) Z+ B+ p2 U: n2 H - uint32_t time_out_flag;
" r/ i/ e: B, B" \0 W - uint8_t Sensor_Data[3]; m% |* W5 [5 U! F* F) s* c
- static uint8_t old_data = 0xff;
" w& j' B4 l; R3 L - $ u% q* D7 Y Q, m
- pinMode(_DataPin, OUTPUT);$ `. ~/ v' i8 m/ Y ~
- digitalWrite(_DataPin, LOW);! q" K/ y9 ~% |2 F
- delayMicroseconds(980);
; \7 w. K& D0 `: |3 B! i - digitalWrite(_DataPin, HIGH);# y* U: v& c9 s, N3 o7 B# }4 _) Y
- $ [' B" f1 e& Y$ F" y! ]. t8 W
- pinMode(_DataPin, INPUT_PULLUP);2 E- d& ?& {5 v R# u
- delayMicroseconds(10);6 g& b$ U8 x5 G$ h
9 D+ l; }* z v, t- time_out_flag = millis();
# r/ ^8 ]: E$ W - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 \( N% z+ X3 w, m
2 B) h* r" p$ A K: @) d- LOW_level_read_time = micros();
+ z1 r! g$ {# z. I* o - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ ~. p1 a/ h+ s" F2 X
- {3 D) a- g, e/ I! c) @/ h; b
- return 0xff;
& _1 u5 N8 W. ^# w4 J - }/ n' o, ]/ E5 P5 m. y! H, I2 k
- 0 h0 A5 F3 X1 ]
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; G" f/ w$ h3 x0 D* N7 N- y
& A2 F2 w0 A4 U& l- HIGH_level_read_time = micros();
/ c$ k) u# A% t0 S. }5 w - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level' M* V8 p U2 U4 z& v# Z$ t6 I
! Q3 g4 n% u# q3 w# A9 c4 p- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out+ T* z5 S x+ L) Z- y9 b
- {% S5 g9 H8 v8 U
- return 0xff;6 ~' [! [* k- p& }4 G3 d6 j
- }7 D# S: K! t$ E Q7 M0 B& q0 }) m
- 9 ~. N) n2 F! i; T+ q
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))$ {7 O8 s# G+ B2 t
- {
, n& d" G* I6 B7 s3 `3 M - return 0xff;
4 B* H& W2 X1 y) z - }4 _/ f- U3 I. X9 Z- J m% `
- r4 F, N' g2 _4 Z2 j! L- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" u) L- h) N2 ^ q; q
- LOW_level_read_time = micros();# W; ~5 {* Q2 [1 y
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
- ~2 M7 r3 g, Q$ C8 c - $ c8 T, Y) A2 e( i: w7 B
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
% Q8 [% l: {8 p/ g, A - {
9 T9 E" ]3 N& C. _& ^ - return 0xff;% k. S7 M& I6 v' W/ s O' u* B
- }
: j( _( d. ?( ~( }6 p3 P3 @" u1 g9 e - ( }, \# R* P4 C! m H. o0 D7 ]
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))2 ^ O! L! {! J1 a& u
- {0 C! ?: q2 \% z% I+ j1 b% v! B4 E6 k
- return 0xff;
: T0 k. Q6 m0 N4 J - }% u. t2 p! h. L: [+ |' v
- 6 \2 d3 t u0 m# Q) |) ^8 P
- for(uint8_t k=0; k<3; k++)
1 x( q2 U) M/ e( f+ @# J* k7 } - {' c2 b: t6 r8 z
- Sensor_Data[k] = 0x00;
, G: O: a! v! j4 `' V1 R8 u
" e8 b3 q2 u5 D: c+ J: D- for(uint8_t i=0;i<8;i++)
4 K# B& f6 m2 T - {! m! z8 N1 t& c+ ?: l
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
* L% F: s5 ?- Z- |! v - HIGH_level_read_time = micros();
$ X* }, G+ C, `8 ~2 E0 E5 a - LOW_level_read_time = micros() - LOW_level_read_time;
- h& d# p; c. t* ~0 ^ - 7 Y: k* o9 J: ]% G' t7 i i
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
- G+ k% t% a( K1 G% V: B - {
# u$ k; O5 M H& o! L2 J" d/ ? - return 0xff;! G" _* s+ o3 d Q
- }
; l) S/ j, Q; l4 i3 t+ h - " d/ ?, W' _. F- H
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ ` z: y; A; i8 q- G5 K5 m - LOW_level_read_time = micros();0 n9 ]( _& K$ ]2 y3 D
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
8 B: z% k1 D0 h+ f% A0 M" S" ?. V - * f0 W$ c' a1 B& p5 x w( n6 S; Y
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
' x9 S2 G3 |5 H( g8 R$ m - {$ y# a+ F L8 B& w: m) E
- Sensor_Data[k] |= (0x80 >> i);
( F; t/ G. u; Y w - }$ J. R. A) e- W3 s
- else if(HIGH_level_read_time >= 100)
1 e% J+ Y" Z. x* _% W3 [% A - {
0 W: r3 g9 @7 K/ z: u. Q0 T - return 0xff;
& C" ^4 G, S- f; i - }
* y4 @8 L7 n/ X4 {3 W4 T9 Y' B
. ?: }1 p; r& u+ u' ?- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out). N, x7 H. v# R# ?& u
- {
+ S! |- M4 X3 J/ x - return 0xff;2 Z% f U$ m. [6 B
- }
8 B+ C5 r% v- F) \4 P - }& g1 r, S2 Z" n. H" v
- }
# q$ q1 L5 l2 Z) {" E - ! z9 b. B* r8 E0 [7 ~
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level, |1 ^6 d0 a# V0 H) s* s3 j
- HIGH_level_read_time = micros();% }+ H3 C9 H1 g. c! _; N
- LOW_level_read_time = micros() - LOW_level_read_time;! l2 q; x! T2 r4 j
+ d& u9 _# R+ M6 ~8 _4 C2 j8 p- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
0 N \/ t" q. ^8 W$ h) e; ?9 {- ] - {, S0 L9 B1 Y9 z% N( {, V- J6 u
- return 0xff;
4 V# }: t0 y7 c - }5 h+ p5 ^7 m. ^; }9 @7 a: Z$ C
- * C$ G$ \$ _& g7 Q
- pinMode(_DataPin, OUTPUT);
. r# z' v; f$ |9 r; q - digitalWrite(_DataPin, HIGH);
; h1 W9 F9 E4 u8 L4 x& @
& p5 x% K) K: F6 \- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))3 E. Y, z7 C" y0 b) M
- {
- y. _4 p, e' Z' ^ - old_data = Sensor_Data[0];$ ]' {8 q7 I- i
- return Sensor_Data[0];) Y9 {% z5 T/ Z. o
- }6 s0 H* J3 U" S/ p$ m n, t
- else
. ~' E& T! o- Q8 S - {. P+ {+ c# \$ {, x: \; q
- return old_data;4 ` D) Z8 r9 q
- }7 l: m8 a* ]! \4 V5 V
- }) {4 B4 E8 X% Z+ c% D2 R
複製代碼 . j9 N5 Y3 [7 s* q, R* w
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
/ b& c; j+ \7 f; Z$ v" [, O( N1 v8 S# T' w8 V+ K
|
|