|
" }, y3 v7 \7 S
- #include <Arduino.h>: g7 y+ a. ^$ n
- #include <MeAuriga.h>
. J; W* w* g2 ^! o. ^ - #include "MeLineFollowerArray.h"
" k- e6 D j8 ]! n
3 y& g$ M) r9 r4 l( @" I% t3 i5 i- MeEncoderOnBoard Encoder_1(SLOT1);$ }: B) p, u. n4 e: D" d, V# Z! E* _
- MeEncoderOnBoard Encoder_2(SLOT2);1 W* \, d0 C U$ L0 ~6 n- P
- MeLightSensor lightsensor_1(12);* O6 o- u/ j9 N
- MeLightSensor lightsensor_2(11);% _* S, f% O `: D
- MeBuzzer buzzer;
+ x4 s& y" j% t - MeLineFollowerArray linefollower(PORT_6);9 B4 Y) z& C2 C" s- c$ r4 {7 X
- + {2 L, T9 j: u- W
- #define Error1 1
0 A$ G1 n! @% K F) u: V* ?' s - #define Error2 2
2 q0 M; _4 f/ T. }$ g- u - #define Error3 3
' I. F: {5 U6 i! e1 K* y
, G3 |% O: U' ~5 G- #define Kp 15
: \9 X1 _' k2 I. } - #define Ki 0.15: B- c( V- E) h* K4 ~- o$ C0 h
- #define Kd 0.03
. z5 e. ?9 d0 E- v - 1 h c+ H% R8 Y' }% U% [9 H
- uint8_t sData;
# R( p% M* k% t% X - uint8_t D1;2 [" D3 d) ?+ ]
- uint8_t D2;
! Y8 L% O$ [0 J- z% C! O8 I; t. ] - uint8_t D3;
# W8 @. t% w& \. _ - uint8_t D4;9 q$ i( p7 F- B! K/ |+ w
- uint8_t D5;, ~, x1 y( [; d
- uint8_t D6;
0 i, ?: f3 P) k! B% V9 r, A
% J5 d, L8 ~ t- float previous_error = 0;
1 G* J" z1 T6 H - float integral = 0;2 Y9 w% z; M% u! ?" I9 S
- float derivative = 0;
" p: I1 P `1 r: f0 c; \ - int Speed = 160;
# e* c( ]+ z5 y o! r - float output;$ h6 L, g. e' M! Z& q. G% I9 Z1 }
- 6 V: \% s/ G2 R5 r, L) [$ |! C
- byte Left;0 z8 d- Z* n3 I3 D# L: W5 d& e" ?
- Q, h' _9 I* _/ U
- void setup() ! T7 r( ]" }( `1 W
- {. C6 x9 m& j8 d+ m: o% x* w. O) X
- //Set PWM 8KHz1 s. {; o/ F3 I
- TCCR1A = _BV(WGM10);
! Q% U8 Q2 ^' f* o: B - TCCR1B = _BV(CS11) | _BV(WGM12);
+ f- D( f D: d) O - TCCR2A = _BV(WGM21) | _BV(WGM20);2 D( g7 c8 {( }- F
- TCCR2B = _BV(CS21);
1 x' s, g, h) _ - Serial.begin(9600);$ W9 R& w1 m/ z( L: k
- buzzer.setpin(45);
9 @& h% M2 _2 J - }' y }8 `# [* q6 I( m8 [
- + q) o) w# `7 ?" x. e7 w* C
- void loop()# t7 w8 H3 G" K# q/ U
- {2 D4 H. P9 |$ u) r: {- K, O3 g2 [
- while(!((lightsensor_1.read()) < (10)));( }) {9 q: }4 g. u- C1 l
- buzzer.tone(1047, 500);8 x% q$ I+ @, Z# k+ f3 Q" r# y
- delay(500);
; Z1 N. V$ u7 s) D; k5 ?8 D4 x - do
" F* I$ w, d z+ @ - {/ D% o+ Q- X5 a( Z3 s) [
- int Err = getErr();
/ a' A5 Q) g; V# a - if(D1 == 1)& W2 p/ E* `" B! H: P- @
- {5 @' r- ? G+ Q; u( c6 r
- Left = 1;
9 f( k" y0 C) l3 G. e( x: X - }4 K' g/ X& X, w* p* `8 o; q
- if(D6 == 1)
V# D: l/ v! A1 q! R - {
# h( u% D% {! C; p# H/ T - Left = 0;
2 ?% |. D5 H! Y6 Y2 E- c - }
6 m. `, J2 D8 k6 l' P0 W! y, V - if(Err == 99)! ~! M+ X; A, Y l
- {
1 E6 L a$ _# j* Z/ c: ]$ y - if(Left == 1)5 m- H( S4 L8 s/ J- E! Q% F
- {! |, l* t* G0 P7 J9 @
- Speed -= 5;. l( n7 v& }0 L$ T! J! Q
- moto(0,Speed);1 C4 S+ R. ~- b. n! }1 K( I$ x+ B
- do
( B3 {* H/ p) m( I! G' E- ] - {) H g2 h$ F2 h5 x' r' f
- Err = getErr();
2 Q8 g- C- J; f4 M, p6 S! ?% g - }while((D1+D6) == 0);- U1 \3 z. J q$ c) ?- [; E
- }" u2 Y- F* T: u0 U
- else4 x7 Z" o2 }/ n! t" L0 `, N7 F7 }6 z; L
- { `& N- P# X3 K3 J* s
- Speed -= 5;; u6 ^+ O1 J5 Y& J
- moto(Speed,0);
+ U T1 [4 ~3 f x, J - do
7 B2 O. ]$ p& A# @) _ - {2 x4 \5 O6 Y m* V3 _2 P, {4 k
- Err = getErr();$ F; {* _ A/ w5 o- {) q3 D
- }while((D1+D6) == 0);$ f+ Z9 M# S9 Y( v4 i; W8 o5 x D
- }
) O2 W: O' L8 a# O9 _. I/ S - }) c- ^8 _- E; ^+ L
- else
; p- y+ H# |( }2 C! ?& z9 O% K - {
+ A- f1 @) a* y - if((Speed < 160) & (Err < 2)) Speed+=1; 3 L) Q: L% h, @% ]# ]0 ^6 s3 x
- if((Speed > 100) & (Err > 2)) Speed-=2;
" b* ^) L f' L* M+ n- A - integral = integral + Err;, e) a) e9 g. [: I
- derivative = Err - previous_error;
a! K: ~/ r5 L6 w - output = Kp*Err + Ki*integral + Kd*derivative;/ I. m" f- H$ \6 Q0 [- o/ ^
- moto(int(Speed-output),int(Speed+output));
7 d2 V6 q3 V, i. Q6 ` i - previous_error = Err;2 z2 a/ U& E" ~% I1 p
- }
$ D( Y" } N( v3 P - }while(!((lightsensor_2.read()) < (10)));3 \' r* Y' X, r6 F( `
- moto(0,0);& C8 T3 n. k! C ?7 }" P, B, Z
- delay(500);( U' Y. b" @2 R
- buzzer.tone(262, 500);; c- a# W+ E( f. a+ R x/ J- j
- }
4 t. r3 A% W8 h+ N) b( ] - 9 _$ {# r( R* I0 u5 ~/ r. f
- int getErr()
( o3 \3 W2 y( N4 o# z$ m - { ! T% W( z) b9 n% n8 u8 n4 ^- {- m
- sData = linefollower.getValue();
4 r F) C1 _& x5 {: W; @6 j - D1 = ~(sData>>0)&1;
! V: O' F& {: f1 K T4 ` - D2 = ~(sData>>1)&1;3 Z' f# S2 \* }' _$ n6 b/ @
- D3 = ~(sData>>2)&1;
% |' d3 X: s# k- ?$ N - D4 = ~(sData>>3)&1;
: r+ W6 [ s$ b: X" w5 d" Z - D5 = ~(sData>>4)&1;; H0 T- h0 V* a; ^. m0 K. e' C# \
- D6 = ~(sData>>5)&1;+ a1 D, h% O% M, U
- int downD = D1+D2+D3+D4+D5+D6;% \& j: q& _' {# L
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);: y& }9 C! L2 Y
- if(downD == 0)- x8 L7 w# Z; B7 e, ]) u0 G, m" c
- {
/ }, _4 R' k: n' Z; y - return 99;
4 ~$ y$ U. R, h$ I& Y - }
! E* c: G' u0 n+ T6 d" w - else
0 h7 Y" O" U0 [ - {
$ E# h; x% n5 A) s& p* \ - return upD/downD;
1 J ~" Z6 {4 V+ M6 t+ _1 ~2 j - }$ g1 o* E( p, s# _% R0 z6 ]
- }6 N4 V5 Z6 F N' b$ j; g
- 1 g0 Y; E1 D {! b" r! L
- void moto(int Speed_L,int Speed_R)! s1 H- F' c& S; P$ O
- {9 d: O" e% O& {1 a5 V; P) W& e
- Encoder_2.setMotorPwm(Speed_L);. X! e$ a2 T: `0 {' w# L
- Encoder_1.setMotorPwm(-Speed_R);. q9 k* C& M$ E; v( v6 e
- }
複製代碼 / `- e3 ^- ~* X. b
MeLineFollowerArray.cpp9 |0 j1 Y- U9 [: P7 u7 p2 q6 M
- #include "MeLineFollowerArray.h"
! I# Y/ c' G8 z7 h( P! m% z - 6 H8 F& E9 H" Q4 z; M
- #ifdef ME_PORT_DEFINED& L8 h/ {* A4 R/ u$ W4 y# N
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)& Z4 }! |4 t7 j8 ]8 a6 {
- {# _/ V0 D1 P7 l; I4 F5 u
5 N; Q/ o; O+ k' k% [: G- z- }
, {! r- X X( x' B3 s - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)8 h+ Q! |& {& W
- {1 p L8 {4 l% w0 U
- _DataPin = mePort[port].s2;
1 f/ M$ s" p5 Z& ]9 s4 L. x" R5 g - pinMode(_DataPin, OUTPUT);
" Z5 k! ~- q$ W - digitalWrite(_DataPin, HIGH);2 O6 j+ o( G2 m! Q
- }
& F) r; i( R5 p" e' E {' P, F# B - #else // ME_PORT_DEFINED
+ W: o v. F' J* Y! p - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)' F2 Z6 c, r0 R0 e$ V% `& w9 N% R8 t6 J
- {3 m- [+ H' W9 h: ]% T, v% z
- _DataPin = pin;
O1 P) Z" H P/ I6 { - pinMode(_DataPin, OUTPUT);
6 O' n+ t. k0 u% C$ t - digitalWrite(_DataPin, HIGH);
7 C$ e# _8 |. Z. r4 T) K+ s - }
! F9 P3 Z: Q8 L0 u2 k6 t0 A$ Q - #endif // ME_PORT_DEFINED! ~/ V* @$ z* d
$ D0 e% W8 l9 Z) ]1 y8 T4 N/ `
% U0 r7 d! x& g u# e4 J- void MeLineFollowerArray::setpin(uint8_t pin)$ k% _$ h6 [! S6 A
- {/ F V( l* D: E5 d2 `
- _DataPin = pin;3 C+ b' ~) z( w6 D' ^6 w A5 P
- pinMode(_DataPin, OUTPUT);
7 b$ G9 j3 o7 ^8 ^, C2 ?- T - digitalWrite(_DataPin, HIGH);# G: Z# C0 w6 w, @
: c; j1 z! B9 H( M! d7 Y; r- #ifdef ME_PORT_DEFINED: I1 o6 ~8 U6 |* B; @7 ^
- s2 = pin;
6 s O, G! F3 w' M- v* _& Q - #endif( H6 i& B# M2 \
- }
+ s2 E( d8 i5 v8 h
9 v7 n9 s7 ]: H, P& i- uint8_t MeLineFollowerArray::getValue()
* a) J2 ?* l1 ~; N! n - {
2 z! K7 L( e1 C6 c) f W - uint32_t LOW_level_read_time;
# Y' \' W+ d2 R. E6 e% L - uint32_t HIGH_level_read_time;3 ~6 Y" C9 H! N7 i
- uint32_t time_out_flag;
W! Z# ~# v: s% w - uint8_t Sensor_Data[3];
4 z3 G- |, ^0 {6 I - static uint8_t old_data = 0xff;; f0 c' n( I3 J. E
- 8 }) y6 R. `9 M" M& |% J: b
- pinMode(_DataPin, OUTPUT);
# R# m/ a3 a7 D) V3 E' K4 E; w - digitalWrite(_DataPin, LOW);
& y5 t% B: q0 G0 {. v* ]) c - delayMicroseconds(980);
( z9 S& M n5 d$ f" y0 V, q7 Z - digitalWrite(_DataPin, HIGH);
4 A/ | _. R l' z - 5 f1 c% d! o+ {: p8 B! ^4 G8 P
- pinMode(_DataPin, INPUT_PULLUP);- \# u6 V% J: V9 O+ B
- delayMicroseconds(10);
d% p! K. l9 G. a, u1 ~2 w
1 z( L) N i! R( b+ o- time_out_flag = millis();
* K; n: H* Q6 p* [ - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
" o& @2 h$ B5 t6 R0 e. F- R
* B- t1 O3 l. Q3 d o- LOW_level_read_time = micros();' n, }3 G! n) `- g+ D
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out1 b- m ^# F2 X4 x+ N
- {3 D7 y% H, X$ s2 {
- return 0xff; R" T) X, F+ ?2 a l
- }& L4 k* L; N! B- E* M
5 i* U( t: S) u2 l; s- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ T [9 X7 I2 r4 Z3 T1 B - / U5 i2 w9 c& i
- HIGH_level_read_time = micros();9 {: H3 B) C3 v3 |
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level8 k$ R% j8 Z: O" K* b# z7 O7 x
- f/ u5 V: Q1 n7 g* \$ P8 k
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out% x0 r: q: H% P8 {. N9 A
- {
. ], i& e& O/ ^ - return 0xff;
2 z' U- ?" z$ m/ T. X" @8 o2 i - }0 h: t4 G3 V ?! C
- * K1 P. @4 A1 a9 A
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
7 R& @9 p+ v6 }; g1 w* } - {( [) g- E+ u9 S
- return 0xff;8 Q: \( C7 C o& s3 C5 _7 m% O
- }% E- b; N* x4 k1 [- I8 P4 Y6 w
- : B6 h8 X. N- p3 C) ? T( i7 V
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
[+ v& W- q4 h6 d% u7 k - LOW_level_read_time = micros();
: X6 M+ S, V; A7 p5 {) G9 M3 r - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
! N* ]4 b# ~/ w# s! u - 8 j9 t- O4 [8 A- c! |+ e4 v
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
7 I5 U/ l8 i; a! u; i - {
; N- ]; P+ F4 G; ~4 x% N. Z - return 0xff;
2 a2 P1 p9 u& D - }
7 Z, o9 }, \4 G! u - # _4 j' O/ ~$ D6 n
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)): P" j8 \! F: g. v
- {
# g8 V( ^4 m* u) v& |' h - return 0xff;: \2 `: ~2 E. C; e, D6 X1 `
- }1 V$ d3 h- y3 A/ G3 {% a
- $ i2 P+ g7 c- }$ V& } p: W3 l8 \
- for(uint8_t k=0; k<3; k++)
5 t8 H0 j0 r7 X6 g6 T, g( }" L+ R - {* p5 x; K6 V T# N( I4 J7 j
- Sensor_Data[k] = 0x00;
' F! `/ W# R& D7 [ ]1 @
& Q: B' p3 z, F- for(uint8_t i=0;i<8;i++)& i& u! \5 o5 l. F
- {( n4 A: b9 ~; o2 q% w) `! u3 F+ @
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
( D* R4 W0 d w N0 k - HIGH_level_read_time = micros();! C% A5 q: O9 x/ J
- LOW_level_read_time = micros() - LOW_level_read_time;
' Z) p4 N' g) q3 l. J4 @
' a& V6 b, x& x6 y- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )5 g2 B8 P2 D' u7 P" p
- {. M" B1 X2 t/ s& _1 G9 |3 h
- return 0xff;
) p! I, M# A: R+ i - }/ T% \- J) P# F
- $ h2 e5 G: E; v% s
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, q( D( v5 e8 M# J5 @
- LOW_level_read_time = micros();# {! S$ z. f8 I; `5 W, e+ j
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level. W* F* o. C0 Q- I! F8 m
- ; ?* T' l6 k3 R, f" A3 ?
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1% ~. M9 p8 U/ f+ D7 p
- {
2 z8 N; Q8 L7 e9 }. F E: q, i - Sensor_Data[k] |= (0x80 >> i);4 t& b8 O0 t: o0 G! {- ]( {
- }
. d' W# s8 j9 _9 P; @1 U% [' ? - else if(HIGH_level_read_time >= 100)5 [% ] |, ~: D0 _) M
- {
+ }) `' I, @1 r; M - return 0xff;
@2 u& h) ^. a( I) k - }7 f$ S1 ^2 ]8 K7 e
- $ M5 M3 A# W$ Y5 k
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
, q, M1 q- y# ? - {7 u Y3 g- ^( I6 A. s+ m1 W
- return 0xff;) P2 O" g2 }& }; @; k& t* N" [3 f" A
- }
- K! L' L' [9 ?6 z; |# H - }
, w* W- V. l' O; C - }
9 K7 o) x0 R. `. r - 9 u, Z' p" \" P5 Z- F
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level9 }5 o3 x, R9 Y3 b- l# t
- HIGH_level_read_time = micros();
- q8 q, h# J. X% @( ] - LOW_level_read_time = micros() - LOW_level_read_time;
+ M( I! Z$ G7 r6 _
) x. n; b* @% E3 L" G- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
1 Y% T5 q& ]8 [' C - {
0 }9 S$ t$ d4 e2 }, d$ m: F1 t* ] - return 0xff;+ `4 y$ s# x4 K
- }6 i8 l% L7 P! G* r0 B8 c
. v% Q8 I. b4 o7 u% H/ d7 G- pinMode(_DataPin, OUTPUT);
: z2 d ] I+ u% H1 y! ?4 r& z/ r3 { - digitalWrite(_DataPin, HIGH);
' D/ X5 n% L! [
5 k7 f6 l5 n8 U; L- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
7 B; V* b9 C' u C3 Z& K( \ - {
0 U" h$ `' U( _9 _9 I - old_data = Sensor_Data[0];
+ g2 [1 y6 [8 O - return Sensor_Data[0];( e3 J" B- Z/ w/ ~
- }
) O- f' Y) ~; E I - else: N9 q" V- H8 [
- {3 h# u( ~. g! Z
- return old_data;
8 \8 \- u& b3 ~0 E - } d- P0 e7 I! I; o: {* `. W4 d% i
- }
, _: s; t0 s+ P) E. L& W) e; W
複製代碼 - U, ]" b7 Y2 o% [/ ?. c5 a/ K
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
! I5 s9 z/ j( ]: L9 z; R" l$ n' ^& O8 [, ^) D$ J8 B% F9 e
|
|