|
|
2 V& m2 e, c) A) g f3 Y! [- #include <Arduino.h>& \# U5 O) z! K0 B' t' S# T
- #include <MeAuriga.h>
$ A1 L c7 A5 d/ J+ l) a1 ^# \ - #include "MeLineFollowerArray.h"* W0 R& M5 U* G1 R0 M% M _
- 6 ]0 C: ` D7 z( A
- MeEncoderOnBoard Encoder_1(SLOT1);
" ~+ s' e4 Y ?) r4 @ - MeEncoderOnBoard Encoder_2(SLOT2);4 k5 A- k! X" H. p; \, ?, G4 A
- MeLightSensor lightsensor_1(12);
) m: L. J4 v, @5 g r) p$ [# Y0 k1 a3 n - MeLightSensor lightsensor_2(11);* S" h4 z9 t+ ]* ]/ \) g! U1 S, I/ P
- MeBuzzer buzzer;( E. X2 B! [; _$ |. N
- MeLineFollowerArray linefollower(PORT_6);5 b, q, F5 p1 `. _7 F, b
8 n B( }2 o3 Y8 ^ I' W! f; P0 a- #define Error1 1
- H& q! |0 d- S& [; Z8 t - #define Error2 2/ V% M* k. |: x: D/ J
- #define Error3 3% P+ @0 B' C( n* Z
; Z9 ?& v9 P$ x! l: N, ^- #define Kp 159 ^" T) _0 A3 B6 F5 M" c+ b4 ^
- #define Ki 0.15# l: t6 ^/ K1 w6 D4 `- p( F
- #define Kd 0.03
/ B6 b) o N3 U6 U; s) N1 H
. U% Z2 \( M) g/ C+ \+ ~+ f% `! ^- uint8_t sData;
/ D: ]! {# G# n& g% D - uint8_t D1;
' ^ Q, u8 [% C- E; v - uint8_t D2;
# Z& }8 o3 ~; b. h - uint8_t D3;
% s9 k9 K8 H5 x! |! D) k - uint8_t D4;( C( V: D# X8 i8 f4 S1 O
- uint8_t D5;
, V0 d9 C* i+ J- U2 T3 J2 l - uint8_t D6;
6 @8 o( P+ U7 O: r } - : l5 g% ?: y: C; i# k% Y( ~
- float previous_error = 0;
2 K, `# Z2 t. m0 g3 T% v - float integral = 0;1 M, ~$ c+ y+ N" a, K
- float derivative = 0;
, {! n. W1 O v$ m& B: z; i - int Speed = 160;
$ i! T# i/ ~( K) C - float output;
2 S3 W. d# N, u! z) [3 G8 S- B4 _- b7 ^9 C) X
. j0 X" E1 h" O. E6 M- byte Left;
# v* A! h+ D5 T5 \ - & e! H) h4 \) e1 R% Y' x
- void setup() ! G* [5 R7 g( `0 F1 P/ a3 q% j) r! |
- {
; z) p7 Y* W/ D/ F - //Set PWM 8KHz
5 `& Z9 e6 Y1 _6 G* a - TCCR1A = _BV(WGM10);
- {: K& S; t* x) p* c2 o5 x - TCCR1B = _BV(CS11) | _BV(WGM12);6 t) c: |5 m; ^, ]; |: G+ m
- TCCR2A = _BV(WGM21) | _BV(WGM20);
/ D& p; p! w& I1 m8 J( ~9 L9 ? - TCCR2B = _BV(CS21);
- n% R! O, L0 {/ P) t2 k - Serial.begin(9600);
. G, t7 Z+ {+ k6 I& ?+ g% Q2 d - buzzer.setpin(45);
7 V; v" R, R; t" m - }
$ t/ j& o" W5 o) R9 H+ e
6 }4 o8 f( A# J0 s- void loop()
3 j( A+ [! u& @; F* q - {
C: a$ A" J l" `$ f8 W6 X - while(!((lightsensor_1.read()) < (10)));- @# O. m3 T2 I0 {
- buzzer.tone(1047, 500);4 d- q3 i* k! `+ _
- delay(500);
' @, f7 @& [4 T) } - do
' Y- ~2 w/ Z' V* w0 k. | - {
7 S% ?. B/ Z8 U6 D - int Err = getErr();0 F5 M6 N; E3 v) ?9 O! \
- if(D1 == 1) H' Z8 n- \! H% U2 A# K0 h
- {2 ?$ v4 O+ \$ {8 t/ G+ F6 B* p. o
- Left = 1;
* C" m0 {# I( T2 I) |+ X - }
8 \2 Q6 T) D n/ @$ A/ m - if(D6 == 1), S! O: B, ]- Y) L
- {2 C3 x3 K$ R0 C |, n- H+ J {. M3 L
- Left = 0;3 `- i5 ^- p9 j# h
- }
0 t; R K9 U/ u H - if(Err == 99)* {, h& \; |& j. x2 P! n4 Z$ K
- {0 W: }, ?4 W5 r! b7 _0 j
- if(Left == 1)/ h) ^' |8 v, f
- {, k: E. G0 L& h0 s( W
- Speed -= 5;: A+ {& D% e; T+ x
- moto(0,Speed);
/ T) P/ N& T% i% o! U - do
, J( B6 G9 [, _+ s3 }0 K - {
' S" }- t; j5 S' e& A - Err = getErr();: s+ O4 A% l# p
- }while((D1+D6) == 0);6 R7 w1 K- V! E5 C6 [
- }# M" E M7 z+ ]
- else
4 W& b) p: M3 l. M - {
& Q) i. X9 O1 u2 E: S, U - Speed -= 5;
C, w5 }, |( H, I, v6 E! n - moto(Speed,0); 9 p' _/ r* s7 Z( E# D/ t7 ~% s8 H
- do
3 v8 w: c8 ~7 Y% \* G - {" b- Z; k- a$ Z$ `3 Z) j4 L2 j
- Err = getErr();. {( V& R2 _) `# c8 x
- }while((D1+D6) == 0);
, [- V3 Z# d8 u- _$ L, k% h - }
: w/ _& a2 M/ F - }
1 h6 C! l2 c8 j - else
3 |; {/ E0 }& \( |5 F - {% C3 g7 Z, E& x8 d7 N) h$ @
- if((Speed < 160) & (Err < 2)) Speed+=1; T2 u; M8 K/ U3 \* A8 [; B: \
- if((Speed > 100) & (Err > 2)) Speed-=2; ' n7 ]+ _! ]( N, v5 G
- integral = integral + Err;+ F. W" s5 F& X$ t% E- w3 Q
- derivative = Err - previous_error;0 O% |0 O/ W, K" h
- output = Kp*Err + Ki*integral + Kd*derivative;. O/ f- I2 ]& z/ ^
- moto(int(Speed-output),int(Speed+output));
0 ~8 V. f9 h' S5 Z- }6 u - previous_error = Err;" v T! h9 R7 w& |4 B
- }
7 z( e) p! E& t) w/ s; @1 e - }while(!((lightsensor_2.read()) < (10)));
! j |$ F7 T- q" V/ `! a - moto(0,0);; N' T& k9 l/ |8 d# Q" n, t" l, p
- delay(500);
) Z _$ I" v0 y9 V6 r - buzzer.tone(262, 500);
$ x# h6 R* W9 b( n+ V { - }1 x/ ^/ v! K! P" n
- 5 t# p3 @' v0 s/ u) t9 q% O
- int getErr(). o3 p; K( B- v
- { ' h2 X. V$ r2 m! q; ]
- sData = linefollower.getValue();
( e1 G! {5 i0 T - D1 = ~(sData>>0)&1;+ J) a9 X/ v5 @/ P& P6 ?
- D2 = ~(sData>>1)&1;: q' B- }% H! I6 J7 P0 p; b" _
- D3 = ~(sData>>2)&1;6 H" m1 V8 m$ x+ W
- D4 = ~(sData>>3)&1;5 E) x! B$ p9 w, d
- D5 = ~(sData>>4)&1;
2 M* z% }; Z: x; y1 n0 w - D6 = ~(sData>>5)&1;. C" u4 m, H/ F
- int downD = D1+D2+D3+D4+D5+D6;/ h2 I0 c6 t9 y4 q; E
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);0 S) d& a8 c0 {6 {2 z( A
- if(downD == 0)/ _" v* |2 J; H% L/ f
- {- D+ |5 |! G! G5 P3 r2 i
- return 99;
8 Y2 f# O2 j( G) m - }
6 n) M9 q6 E" J5 I7 y3 `6 | - else) R9 A! `8 k2 i* z7 Y0 v
- {
, c& {9 I: X: o" f* Y - return upD/downD;
' a" {% h6 t0 R: A8 g - }1 n7 |7 N. c. i/ P1 h
- }
9 ^+ _" q3 B3 i5 ]0 G
7 d# S+ Y9 ]: ?- Y. f- void moto(int Speed_L,int Speed_R)
+ s) k3 p) Y( F5 I - {" s; b! F2 ~+ D" }# l) v
- Encoder_2.setMotorPwm(Speed_L);
+ X* ?+ ~, O; h7 v+ {$ O - Encoder_1.setMotorPwm(-Speed_R);
* c1 }' {4 [# \6 w- B0 k* Q- N - }
複製代碼
0 V& N% U# ?+ G3 M7 w+ YMeLineFollowerArray.cpp: Q$ B5 [+ c( R8 H$ z% ?
- #include "MeLineFollowerArray.h"6 n1 J8 H' j2 l; ~2 x
* Y5 a/ P& b0 I& K a, o- j- #ifdef ME_PORT_DEFINED
4 {4 j' X( r& V! h$ C - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)0 {* h% u# L. t8 E$ a9 b4 j; T% U! K
- {$ t+ \* r/ @0 n( ?5 s' d. k4 [6 ?
- , @, D# q9 {4 E1 a' {
- }- e C7 N% m! L4 E
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)0 P2 @$ u- n9 V# Y0 P3 u! D. n: C2 h' i
- {
Q1 t$ J7 N( R! T2 H$ b5 S h - _DataPin = mePort[port].s2;
4 n V6 ?, U/ y3 c - pinMode(_DataPin, OUTPUT);
* n$ |+ h T# G9 u% p3 S - digitalWrite(_DataPin, HIGH);
, e" f) I T$ ~% d& ~( } - }+ G, c8 s& H% U& z0 N% z& ~
- #else // ME_PORT_DEFINED* _: k1 R0 `* s. m# T' X7 v
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
, x( {4 Q8 ?: ~( ]: E2 E; [5 X - {( S9 y3 v% `3 S1 b1 V# S
- _DataPin = pin;( T$ `6 w: |, I: G% w8 H
- pinMode(_DataPin, OUTPUT);
, ~1 c: a& C- A5 S8 P7 d: s - digitalWrite(_DataPin, HIGH);
4 t/ I* h& l" `- B2 S A - }
+ H, A: ~1 o1 [2 P3 `. w$ m, [ - #endif // ME_PORT_DEFINED3 \* k) v- P0 F+ `+ p( q
- 2 V$ _2 U/ \$ l1 W' i8 l6 M1 L/ b
- . U( ? N2 r7 L- K" _1 ?2 D6 ^
- void MeLineFollowerArray::setpin(uint8_t pin)
3 |' G- S- @, D# p1 E# `1 M0 H# _$ B - {0 Q: A9 H7 |) ^( C
- _DataPin = pin;7 E( z$ c; F2 R& f: _1 D" H
- pinMode(_DataPin, OUTPUT); " A9 J# h* t; y* ? K& v4 Z
- digitalWrite(_DataPin, HIGH);8 q5 j# r2 ` J* V6 M
- 3 f( f4 j( D' t1 @. x- b$ @
- #ifdef ME_PORT_DEFINED: j p- c7 F8 y6 r
- s2 = pin;( i; e4 \5 b( \
- #endif$ M$ Q2 K& v* g9 W/ Z# U
- }
3 u3 G6 c4 c& {/ Z% q7 P! E - , h0 R3 x, n; @" \" k0 e6 ]) _
- uint8_t MeLineFollowerArray::getValue()( {- [( ]: [. u# _+ l
- {6 x* C: {: B/ h: O! x
- uint32_t LOW_level_read_time;
, S" m8 W8 ]# @$ K; ? - uint32_t HIGH_level_read_time; {3 U- Q# H; I6 p+ V/ K0 `+ j0 b- C3 a+ J
- uint32_t time_out_flag;) G2 g: ^+ Q. c K0 t$ f4 Q
- uint8_t Sensor_Data[3];0 o! @$ ?# C( d8 @
- static uint8_t old_data = 0xff;! I4 [" ^. [# c7 _+ h/ }% r) a8 [
- 9 ] Q0 R. y$ |5 n# ?) G# @3 ^
- pinMode(_DataPin, OUTPUT);
% Q2 F3 @; N% n" q" {9 C - digitalWrite(_DataPin, LOW);
% S3 X: c/ h) }9 f% ~8 N - delayMicroseconds(980);6 O# {, m7 I$ b( W
- digitalWrite(_DataPin, HIGH);% J$ `5 y; G/ a% l0 V! s: i2 t
- 8 |* b3 ^0 p. w1 P W7 }
- pinMode(_DataPin, INPUT_PULLUP);0 z n; F) X( z+ t3 |9 S5 Y
- delayMicroseconds(10);+ L) B5 }! J4 y
- ( {1 [% j* Y, h5 B
- time_out_flag = millis();
) `1 ^- ?( }3 n* Z$ a - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 _2 D; S: ~6 _4 C1 g
- - @. U9 m- R5 j* g" \
- LOW_level_read_time = micros();7 H# S( n) N: w5 j; @. B! \
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ T0 ^' ]* {2 l' B) X! ?5 G% H# K+ s# h
- {
- F9 \! q' {) Y+ A9 O3 l8 X- w! D - return 0xff;$ K3 D4 ~, Z9 ~- q0 _
- }7 U& k( |% P+ m9 N) ^ L
- # V- h% R0 D6 a, Z( {6 _- f% ^- z9 y$ V
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. |2 a9 ~# w! w6 P, S
: W0 U1 t0 M: u7 |' C- HIGH_level_read_time = micros();
" K% }6 y+ y9 [3 y5 B0 a - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level9 F7 ]) E- f/ p2 u# ]$ P
- 2 G& W/ b6 o1 f
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
% g8 i1 y6 m% c% U' ~ - {( J5 K9 S! w" F2 l8 k
- return 0xff;$ K O8 ]3 Z# F. N+ U9 u
- }
: E+ K2 b( U- n- d - ]' i1 X# a5 l v# O& S/ U+ ? K6 v& R
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)); `) E* T# S$ A2 n; o3 _2 f a; C5 j2 u7 q
- {6 v& M3 t) v4 p9 ?0 Z. v
- return 0xff;8 {+ g* Z4 X, o" E
- }
/ B0 w. `- X; U5 v8 a0 e* H - 3 }8 s3 v* E- {! _7 U9 P
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% w) O7 ?% {2 g7 n# \ - LOW_level_read_time = micros();9 Y! u- U7 X$ _# j. ?
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
" X g' d! j) N: r. S8 L/ B
* W3 f$ g$ X+ {# J1 |- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out; B) b) I# \( O' F
- {
# s) I; c! v* P" e( q( [ - return 0xff;0 L3 A) H! v' |! E
- }
U( Y% T1 f* F3 q# n
* _. l- D; O4 S6 D' [- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))1 r& n1 [1 ]9 e5 h9 n1 K
- {/ ]. g* ]( M0 V! A- P5 Y2 q f
- return 0xff;5 x i2 h! k6 [) z) w+ j; f
- }
/ s9 e, |, _7 M: Z7 d2 I5 g" y
! s- ?/ J' \0 T8 D- c- for(uint8_t k=0; k<3; k++)( x3 o( r6 N; J& [- x* B
- {
2 e9 ?3 Y0 v9 [% O% {5 E) a - Sensor_Data[k] = 0x00;
( j+ [6 a2 U+ w; j: w - 3 j: b& R# g, T! ]! n; k) G) p
- for(uint8_t i=0;i<8;i++)
% B4 k4 }/ W. B% n2 p( x - {. k. K% e7 q5 X2 x" O+ _" f
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
! c$ u+ C6 G* W6 K- n - HIGH_level_read_time = micros();+ u* L {8 _' Q! l! n8 z9 f8 l
- LOW_level_read_time = micros() - LOW_level_read_time;
4 a' ^. W5 `* x( m) H
0 Y1 N6 F" n) g% A- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )7 W* s, d2 i/ R9 p0 [8 H
- {2 X) h' L0 u3 n5 f F6 u, s
- return 0xff;
# S: T* J! O6 `3 W* {( O - }
+ `3 ` O$ |; }- M$ X6 p - 8 J3 O9 U g! l+ _" C
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); }% | ]* W1 d/ ~# O9 x! `6 d
- LOW_level_read_time = micros();
2 V" S4 A8 W+ v- y- W* x0 c) s - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
: R* l: K. T/ a% M! d G* ^ - ; o8 m0 E4 W7 ?
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
) d1 E, m* \* t2 j& h" j8 o - {4 { l4 E E; L& o( o" ^1 L
- Sensor_Data[k] |= (0x80 >> i);% b9 n6 Z2 o2 E9 |8 O0 \8 S2 V
- }! ~' }9 J+ @ g# @
- else if(HIGH_level_read_time >= 100)
0 L) t7 }0 G V4 O. i1 M - {
3 Y% a6 }1 z. N6 D - return 0xff;
5 c1 z* o; i# {& \0 p& h - }
6 ~9 v8 d5 e" j- ] - . t' K, h" J. A! |# t
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
: ]" `; H0 f( e3 i! ~7 W - {
$ J, s; M* w5 g1 ` P& T* D - return 0xff;/ U8 E4 W* e' T# b- H
- }* E; r- p. s7 e1 }' u( E
- }
+ D9 N& d& J+ g4 l5 ~8 ?4 ^ - }
" ~! @# n6 S# H1 M
1 O) @. t$ p) U1 ?. L- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
! X- n3 D( m+ N0 c - HIGH_level_read_time = micros();
6 S9 [9 s0 V& R' c3 W C7 i0 s4 K - LOW_level_read_time = micros() - LOW_level_read_time;
8 X+ w7 K' `4 D) o0 l - 2 ?9 x# m2 O- d. K" ~7 t
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
5 R1 I) z8 b2 ^' ?1 d# t - {
/ f' p0 V8 P6 m6 u - return 0xff;: A) j G, [) O- ~: Q( a& _5 U' h
- }
! L5 ?$ r' [' M; l* h6 D2 X" A) A
2 M2 R! n9 f& t# k. O0 [0 s8 B, m- pinMode(_DataPin, OUTPUT);" H, O+ x) l0 y! b) y0 \ n
- digitalWrite(_DataPin, HIGH);
. m! [2 w1 `! o# w: V+ d6 s9 P+ [+ Q5 j
3 _7 u0 l' m$ I: U: w- o- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0]))) O; B; }- G+ f i
- {0 S% W4 ?& q1 X( G1 G
- old_data = Sensor_Data[0];" L: T2 ?% ?6 j; p' ^2 k8 X
- return Sensor_Data[0];" Q% x" a9 k0 S) G: I7 K
- }. q2 I7 q4 k- ~" L1 {* h
- else+ e+ }- o% z2 |* [4 y
- {1 E1 N1 ? q) y
- return old_data;- \8 \& B9 C2 i3 e5 V4 h
- }6 Y/ e" c4 r1 ]
- }. c7 ^, l8 Q) p+ x
複製代碼 * s3 W$ F4 F; {
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
+ t$ d4 ~, T2 F8 @6 w
2 l4 T3 F& ]+ q v+ U! a# ^
|
|