|
: i. f7 z. a8 X
- #include <Arduino.h>& d) b v7 E- x) E; l* e, q2 ^3 [, n
- #include <MeAuriga.h>$ J/ k. a o- q
- #include "MeLineFollowerArray.h"9 N( R2 ?: R$ p
4 S p2 @& W0 \" C- MeEncoderOnBoard Encoder_1(SLOT1);
2 O& C7 x$ d+ P3 C! j1 R! F - MeEncoderOnBoard Encoder_2(SLOT2);8 W8 F+ B' e) O9 i k
- MeLightSensor lightsensor_1(12);
% @2 \( O5 N. u - MeLightSensor lightsensor_2(11);
. W; B/ a$ {6 ]5 q, }2 O - MeBuzzer buzzer;! X2 y6 P+ e( m5 ]" b$ h2 E" \
- MeLineFollowerArray linefollower(PORT_6);
# t: S6 C- x+ l9 \7 G" \ - $ x. g$ Q1 o8 J
- #define Error1 1! N0 ^, d* t4 [* L3 M
- #define Error2 2% g2 ?8 u# w; K y5 w( f) ?
- #define Error3 3. o/ i: W7 R8 [. o; n- n
- 3 ^; C, g/ ~3 q% H
- #define Kp 15
6 y5 [. i, d$ } r: Q0 L2 ? - #define Ki 0.15
. z+ a; W2 v0 y - #define Kd 0.03
, [$ w$ L0 i) i# r( }
: M4 P Q! H9 t% e) r2 w- ?- uint8_t sData;
# y0 g* C) Q+ e. c7 e - uint8_t D1;
. X" S: ]# k$ K/ E2 G - uint8_t D2;4 M3 m( ^- f% [; n! }1 z N
- uint8_t D3; t3 Z" A: C1 N% N
- uint8_t D4;
+ W4 I9 U% [6 k* `+ p1 t; _ - uint8_t D5;* z- h7 t8 Y; z" O* d6 p
- uint8_t D6;4 [+ y7 Z. p3 A7 S3 [- G1 F
5 q7 i4 u8 g' {) x9 N, P7 B, K- float previous_error = 0;3 C' c7 n5 r/ q* J! B
- float integral = 0;
! N6 b( i5 T( M - float derivative = 0;
) Q: Z, r2 x* O" i/ Y! { - int Speed = 160;
' F: i& ?) P5 f% r$ e - float output;
9 I/ u7 ]6 G5 C6 T. V - ) h2 X* v6 p `0 n' t
- byte Left; @9 p5 m" C0 q! q2 i8 Y$ ]$ v
- ) x, _' a3 S, G$ {$ t# c* D, c
- void setup()
# H# X# U9 W* J5 R - {: P5 Z8 B8 M2 q0 h
- //Set PWM 8KHz W1 y: U1 g0 i/ c
- TCCR1A = _BV(WGM10);
# U( R2 o( Q1 t C7 _ - TCCR1B = _BV(CS11) | _BV(WGM12);: j4 v7 R$ Z+ C# t
- TCCR2A = _BV(WGM21) | _BV(WGM20);
! t6 c& ?; Q! v s& e- t O - TCCR2B = _BV(CS21);# V& r4 M/ ^9 d' T
- Serial.begin(9600);, I* C2 i0 Y g. \' h# N- _6 U
- buzzer.setpin(45);' t, T$ @! y3 a
- }
. `; M" p( D7 t4 f - # Z4 a" P8 K. Q8 P L; U
- void loop()% R" r1 r: F2 [* ^
- {1 e9 Q7 `6 ^# a0 Z- }0 }
- while(!((lightsensor_1.read()) < (10)));" n2 H% o- y F* i) C! N
- buzzer.tone(1047, 500);
) v% i7 |) d& o$ @( |4 b+ l - delay(500);
% i; `! v) C2 E+ O4 \ - do
/ m, A9 }! B3 ]) U" }2 G5 z+ \8 B - {
2 M( A) D. W M2 Q - int Err = getErr();3 z$ ] c' p) l4 ?: T: d
- if(D1 == 1)
! h' c9 D9 }, ^7 T: k3 L8 p - {
. Q7 N& p y# b. ]8 D! i5 W& {, V - Left = 1;' {' t+ z Q4 k# y1 h4 H* L
- }1 o2 x0 C5 t5 T, x6 c [' Z
- if(D6 == 1): t2 b- `/ ^5 q! d) C
- {
: O) d0 ?. y- f - Left = 0;
6 Y% K8 [7 [* V: h4 @& N2 w - }
8 b9 _$ Z# A' `: i7 P5 ~* m - if(Err == 99)
; T( Q3 _- ?% a& S - {
, f1 z- a* r; \7 [ i$ W - if(Left == 1)! a [- R1 r+ |% Q8 h& D$ r
- {' I: A$ A9 u+ g) |
- Speed -= 5;* j- R k, [. A
- moto(0,Speed);
7 \2 |+ P x" s - do
$ |. I% A5 b8 a3 W1 P - {* |6 C% `- o3 }
- Err = getErr();
; t# X% W5 \6 a7 `3 n - }while((D1+D6) == 0);
' U6 K1 q" j9 u! C& F1 W" \/ {( ^ - }( C2 {! m8 d0 e8 y) V1 Q: H
- else2 }" w" Z# e: \. W1 E( E; b0 q' P
- {
+ i- y! x/ k$ b - Speed -= 5;
9 K# E P; l- s# D+ Z - moto(Speed,0);
* W# n' }- d1 l; L - do
# d- \" p8 c, f - {
* j* S* ]* h& v& g+ t" K l - Err = getErr();
1 [- L- L. d) r - }while((D1+D6) == 0);3 u% E! w9 p2 R$ x/ P
- }8 C# R1 d4 h Y: j
- }
% s8 T6 h2 V" ^' |% n# U - else
5 z1 g! f/ X& e/ p0 c - {2 @; N% j8 T& {/ {
- if((Speed < 160) & (Err < 2)) Speed+=1;
# r: s6 N0 ~1 R. B - if((Speed > 100) & (Err > 2)) Speed-=2;
9 g) T* _2 G' W3 n' z" | - integral = integral + Err;
& g4 L. m9 \" |4 Q3 f - derivative = Err - previous_error;6 ~& S- o# |+ i% H* K! F
- output = Kp*Err + Ki*integral + Kd*derivative;) f9 M8 O( N8 q1 t9 J' K) N6 {# G
- moto(int(Speed-output),int(Speed+output));
& r3 c1 t" |; q+ \0 g' _4 p - previous_error = Err;7 \2 ]5 x) [+ W" k( n
- }
2 _3 @5 {& H x2 k0 O - }while(!((lightsensor_2.read()) < (10)));9 w4 d$ k8 I3 ?+ Y. {' s$ w) t# Y. f1 q
- moto(0,0);
" M3 o2 q1 Z$ F& o - delay(500);- R4 b H" g4 i; ]7 ~! w& e
- buzzer.tone(262, 500);
! z% r- M( M* o f0 b" o - }# @7 p# A) l$ G4 l; B7 J3 Q
- : a7 a0 P+ z6 ~) H0 m/ X8 Z( C3 ^
- int getErr()' F8 y3 T- v' E& B- u' G
- {
- E% Q1 Z! R: X4 E - sData = linefollower.getValue();4 V6 j% |: b |: s+ o6 F
- D1 = ~(sData>>0)&1;& O7 C' U b2 y% l2 F8 B# o- F. Y
- D2 = ~(sData>>1)&1;
, p& H& [, r: _5 V0 P - D3 = ~(sData>>2)&1;
4 v9 U( I# E$ x3 F! y! D* \# ] - D4 = ~(sData>>3)&1;
# C: n1 h n$ C; a - D5 = ~(sData>>4)&1;
! f( Z9 N, {4 f: J* i$ k7 U9 J/ h - D6 = ~(sData>>5)&1;3 }: s9 \2 [: K# m) }
- int downD = D1+D2+D3+D4+D5+D6;
5 W/ x, u- h# i8 I& v - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
* T' N' ?2 u' J. l) {4 {# i" d. X - if(downD == 0)
* r# B9 v, X; S, W6 l0 M& X2 L - {
; [. F* F* k6 F v/ U8 @. p - return 99;3 j; \2 l9 z. r& r- `6 h; I
- }
+ ?8 W/ R4 i' ^ - else: ?8 B) c& V D' a* p
- {
6 B" R# D Y- ~0 U/ b @ - return upD/downD;
$ N: H4 d1 r: \8 K - }
: ]. O0 C+ P" A+ W; u2 D( L/ G - }; w& i; N$ c# B' U% M* ^& P& f
1 J0 j* ^, T A1 b- void moto(int Speed_L,int Speed_R)
1 i3 p* ^$ N! j: b5 Q - {# M& Q% s3 y$ {- S* L- t7 y
- Encoder_2.setMotorPwm(Speed_L);
2 Z* K- ^, d8 |# R. r - Encoder_1.setMotorPwm(-Speed_R);
8 c, @# G6 j( J9 T - }
複製代碼
( l Y% O. u4 l6 ^3 jMeLineFollowerArray.cpp, `9 C2 t% N+ k
- #include "MeLineFollowerArray.h"
% V" D0 e8 n, V) {
* s+ g$ t$ ~9 k% s- #ifdef ME_PORT_DEFINED
1 f( S' O }! T) H+ d/ `7 V - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)1 `5 A' B6 E3 X8 A4 E
- {
2 J, s* V( z( \. c& _
) M. {+ I- [+ G R3 r- }
) Z+ V/ L# x" u$ ^& i - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)& K$ e$ z0 c' ^4 u6 R7 `
- {6 H# O' V. O5 U8 u- q9 ?2 y
- _DataPin = mePort[port].s2;+ a2 a* }( g1 T) m& C
- pinMode(_DataPin, OUTPUT);
! g W4 ]! C. b) _ - digitalWrite(_DataPin, HIGH);3 @6 j) V+ V& f, L# H6 D
- }
: G' U- d% f1 n' a ^! W - #else // ME_PORT_DEFINED* U/ ^4 x$ V0 }5 ]& h
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)8 C0 l+ E9 u& `2 K$ |
- {+ k8 k7 b* I V8 Y+ q
- _DataPin = pin;3 Z0 P3 i4 N% X2 p0 j
- pinMode(_DataPin, OUTPUT);
- h4 d3 B3 s2 i: K; f8 N - digitalWrite(_DataPin, HIGH);4 H1 U* l& d, h3 s) O1 R/ b; h; }3 x
- }: J. f- R# H/ x$ o# [; {! x% K2 o+ ?, K
- #endif // ME_PORT_DEFINED4 J1 F t7 h: e. Q
' l; X Z* F% K% X( q! n
+ P! i" R" d; R0 Q2 f3 s; a9 o- void MeLineFollowerArray::setpin(uint8_t pin)
- O+ }5 p0 I0 R - {
. @. }/ Z- X% F ^. h - _DataPin = pin;
0 S) ^0 i) M. j" [7 a) A# ] - pinMode(_DataPin, OUTPUT);
' A4 C. c' H# Q( S4 V" \" I# i - digitalWrite(_DataPin, HIGH);
5 O* p% R( ^- P5 t% L p
( C4 ?# K! Y6 o. A5 s$ K- #ifdef ME_PORT_DEFINED
' a; L+ a" Y5 F) T - s2 = pin;
6 ~! S' ~% O+ p/ m+ b9 c7 J - #endif2 f. m3 B4 y, I" V* S
- }
" p6 V+ U+ y* i7 T$ U# g - % e' e2 ^$ f& |! p3 F: ?% B2 R
- uint8_t MeLineFollowerArray::getValue()
# n6 d- B7 G l - {" ]2 F, Z. U0 t9 B0 E
- uint32_t LOW_level_read_time;4 {% A. M* s; W6 ~ K
- uint32_t HIGH_level_read_time; N$ ~# ~% n& L. x% }
- uint32_t time_out_flag;8 A2 `8 O& I2 S$ I
- uint8_t Sensor_Data[3];
+ r0 N y; Q/ m! |2 e D - static uint8_t old_data = 0xff;$ x6 k% y/ H1 ^9 J2 i
- 9 C$ |# A' x4 B
- pinMode(_DataPin, OUTPUT);& ]' M: W' [; f f2 z
- digitalWrite(_DataPin, LOW);2 L+ C7 w- C. Q5 k3 N. f2 s3 s
- delayMicroseconds(980);2 D, t7 m' v0 Q2 Z7 n" h. I0 Z
- digitalWrite(_DataPin, HIGH);* Z) L. L1 n7 f- W
- & a" e( s1 [/ ?. l: z5 Z
- pinMode(_DataPin, INPUT_PULLUP);
. n& P M7 d) Z2 D' Q# J - delayMicroseconds(10);# Z: _# i. \, G1 l, V8 e; V* F
5 @3 Y. ~" q8 r0 M- time_out_flag = millis();
5 B, [5 B* x3 z. E g' a6 E3 D3 ? - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! q8 T0 j6 J7 R( _9 H" W, l - 7 E/ s" B% r, {& ]1 |3 _
- LOW_level_read_time = micros();
" J* A7 E5 N- q3 A - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
7 u2 b7 D$ `7 Z - {
; x! F7 n. K u - return 0xff;, M7 ^" W. a' i1 r
- }* j9 W9 u) x* [7 e+ e7 r1 J
8 w* y4 ]) |6 R6 L: o L- S$ d- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 t2 {& W( k$ \% u) Y" D2 x' n! g! o
4 G6 w2 i/ h) }$ ]3 H$ R8 t# Q- HIGH_level_read_time = micros();
g p& H! C5 G# X i- I0 T) s: b3 o - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level; B+ C0 Y* X5 z& Y. M- c: U
* L* T0 E$ S* z+ I0 r f/ B3 X- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
" C6 U; S) f; Y7 a! p - {, e8 |, u5 t% y9 D' V% `. x ^& f: }
- return 0xff;
2 ?7 @8 Q% O) v1 Q& Z0 Q3 p$ Q) f - }
% w5 H6 P7 `# r! H0 r
& y8 p; d' ]& P3 q7 ` \- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
- O& ?/ s1 ?% |! X: ^) x' Y1 @9 \8 E - {; A' X+ f9 J7 c. m/ V1 |
- return 0xff;
8 u) m. P8 m, D$ Y) A) ` - }
. Q9 j( n5 H8 J
: J; U- Q0 T8 x- c0 j- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- q" S0 L! d) S; s! b8 }+ S! ` - LOW_level_read_time = micros();
" q- _# U$ ~! G7 \ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
8 g) M( l9 g. @$ j8 V3 f - 8 K7 n, `1 I, `' g% U4 P$ J: J
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 |0 K* C; b8 V" f1 j - {
( V. _# r p0 B8 P7 s - return 0xff;
/ p3 d* h7 `# g( _# R1 L - }
0 f: A/ X4 M t - * V% m' D2 u# S$ a" m$ o
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
, c6 x+ t! p8 E5 |" ? - {4 [' t" ?# j! ]% L8 ?
- return 0xff;2 v8 y; i7 f. y3 Q3 E7 O
- }1 R7 I, ]' ~& L! \/ n% {* p, z
- 6 r( B( d9 E) x* c: P9 h/ x
- for(uint8_t k=0; k<3; k++)
# d1 M& @8 y# Y8 ^: S$ [ - {$ @! D6 a* N! |7 d3 V
- Sensor_Data[k] = 0x00;
/ i u# k" M8 v2 A8 H3 O
8 I& y$ @# B6 K C- ?5 c6 Z9 K- for(uint8_t i=0;i<8;i++)' @3 z$ ] ]/ x5 q9 Y" k
- {
4 S( w6 s/ X# x5 M, x# g" B0 ? - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level) b& z1 i* D8 i0 Z( ?2 r
- HIGH_level_read_time = micros();0 R( K8 o1 X$ }! ~0 b
- LOW_level_read_time = micros() - LOW_level_read_time;+ T6 R* `: x4 c9 `5 R/ ?; n
; t# X% Y" x) D3 m- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
/ _' S% r$ m, l# B+ j* ^ - {
& y+ N- B; m1 j% ?& O1 }; ~ - return 0xff;5 e6 @8 c1 {; M- V* Z' A1 R1 Z
- }0 t6 s1 e6 C8 ~) v
- + v' W8 X9 [0 W7 m+ h) A- x$ M
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );3 b C! y! }( ^' p: Q4 V1 j
- LOW_level_read_time = micros();
3 v1 z$ l! S& M1 p - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level8 _% w5 C# M* J
- & B# z. D- I Z( K5 `
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 11 s* G+ S5 ]1 ?7 F' _: k
- {
3 `3 z/ Q" F' b$ G" @+ G - Sensor_Data[k] |= (0x80 >> i);' u% U- i, f9 m3 K
- }
/ ?( r* d, T, J; [5 m - else if(HIGH_level_read_time >= 100)+ l+ C% o, T; o* _
- {
3 t- f# T% ~5 e) ]8 T" e$ Q - return 0xff;3 }5 x, x2 B. t
- }
, h1 y# V7 P) G" Y$ v1 i' [! O
# u2 r. W. T- K! l0 f- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
. [7 S- ^' |; u0 ]) i' A% v$ s - {
1 b5 c( [$ o2 L2 A: J6 y9 `# Y - return 0xff;
8 w$ N1 Q# z9 r1 u+ N - }7 x+ k( ]' j, a( s k9 ~
- }
k( o0 B# x N$ P# @4 E - }
3 d* q3 P6 p6 t - 3 I7 y% I5 K5 i- ?- x5 a# a F
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level/ H1 k0 ?% c" ]3 X
- HIGH_level_read_time = micros();& C/ @5 r& `+ d$ H/ z9 S7 A2 O
- LOW_level_read_time = micros() - LOW_level_read_time;
1 c( Z9 y. n3 o5 D) m' a3 ]
4 c6 k7 P, A- ]6 Z- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )4 P6 G K$ J" d9 N1 M5 o
- {
4 ^& J" I) T; Z7 T$ k - return 0xff;7 x' E& T8 a" A2 E1 G
- }
: e& k" J8 }. n, {+ @& a) [' c - 3 v$ P2 R; V, W: l
- pinMode(_DataPin, OUTPUT);
! @: }6 _; M- V, |% @ - digitalWrite(_DataPin, HIGH);$ x/ @ o, e. @6 Y# T
9 o" s* `6 y/ A9 B! N" t5 V- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
) t# F8 {$ ^% [; |. d - {9 k( ?4 C: h5 e+ K
- old_data = Sensor_Data[0];/ G8 x, b9 A' u7 S: n
- return Sensor_Data[0];/ Q5 i7 m* x$ {0 q5 M& H
- }
+ u" m1 n4 F- B0 Y8 x; \ - else; \' |2 v _" P p9 G
- {; n1 W u( t0 c* Y0 I
- return old_data;
: @, E, S T% D - }6 _! @- y0 O5 A6 [* ]
- }' ^6 M' M! O) i2 F) n
複製代碼 `/ X) C% C; B
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
- i( O0 c% p+ F: L# u( q( s
9 B* X/ y }/ A8 y
|
|