|
|
: r$ \! O: [1 r4 ~' I# k! A' f- #include <Arduino.h>) F) m' u- b) k% i3 W) H
- #include <MeAuriga.h>
# Q4 z3 ^* f: h' r8 S - #include "MeLineFollowerArray.h"
2 Z0 _" z& v2 q9 R9 ?; O6 U - ) `8 C, z2 z0 z8 G' L2 _6 k; t
- MeEncoderOnBoard Encoder_1(SLOT1);
7 o' a. \, R, E, g8 L1 a9 _/ t |# X - MeEncoderOnBoard Encoder_2(SLOT2);: c! i7 i, A4 Q C1 r: m5 F
- MeLightSensor lightsensor_1(12);
+ t9 |) i9 l; @ - MeLightSensor lightsensor_2(11);
- C( ?, Q8 |- R* O4 p e - MeBuzzer buzzer;0 @5 H E. i, S
- MeLineFollowerArray linefollower(PORT_6);+ m5 d! F7 Y' r% h* P) Y U. E
1 t1 w3 Q% G0 t9 a- #define Error1 1
5 ^* S9 k. e* \ - #define Error2 2
, y2 ~0 I" r; [4 `% F+ o9 v. _ - #define Error3 3 G: H# E7 k4 l* Y; L& k% n6 @" z
- ! X" s H" y+ X3 a1 W
- #define Kp 159 I8 ^2 O" B4 w& }9 Q% o% b6 P8 M- ?5 f
- #define Ki 0.15
" N# D7 _8 f7 F1 j2 K - #define Kd 0.03
) x' }2 ?9 v3 d8 x9 O - ; C" `0 b) Y/ K. ], o5 D$ z; d( D4 M. \. Q
- uint8_t sData;
& ^* L" S' {( S# s9 ?$ k+ l - uint8_t D1;, P& }, _5 j0 b7 [1 y
- uint8_t D2;
, `# l: _0 I) K - uint8_t D3;/ g6 m9 I( ]+ `" k+ L# L R0 u0 m
- uint8_t D4;
( A) b& ^5 _" a1 j) S. } - uint8_t D5;
z0 Z. [/ Q2 G% k1 |7 ? - uint8_t D6;
3 ]2 `8 G3 s$ i
5 L5 J9 M, e+ {8 ^6 C d* `1 t- float previous_error = 0;1 n9 r6 i- t' T" ^
- float integral = 0;0 `& w6 E5 B7 a, G+ {8 J: c8 j+ Q
- float derivative = 0;( l& I+ Q# U' ^" q2 B/ \
- int Speed = 160;
2 g6 L- J, u' x - float output;
' E' C1 ~6 M4 F: j+ P - ( h9 f; F+ N) p. F- M
- byte Left;
" H% D, o1 {9 X V. K& [# H
% a. _# C. P8 H% v2 ^- void setup() 3 k! V8 c* x v' F, \8 S
- { T) ^( l* _, S6 k* n4 u) r/ O
- //Set PWM 8KHz' g" ~! y' k0 j( u2 Y9 ], O
- TCCR1A = _BV(WGM10);; p+ J( S( f7 g4 _8 V$ q
- TCCR1B = _BV(CS11) | _BV(WGM12);
( f/ T6 I ] X& z/ r0 T& H4 X M - TCCR2A = _BV(WGM21) | _BV(WGM20);
1 o R, y0 S7 B i) {5 k. a - TCCR2B = _BV(CS21);
& v. m5 E+ W f4 P0 f a - Serial.begin(9600);1 Z+ {. c6 d: S+ a! {" }
- buzzer.setpin(45);- [- l- j) \4 E. ?: s
- }
- S" ~+ e7 c& M0 X7 `$ R - * K4 ^8 O- _8 Z5 T
- void loop()3 s9 `, q5 ~) g, i& e- V
- {
1 X! p1 C8 i+ n9 X4 D - while(!((lightsensor_1.read()) < (10)));
' Z# \5 j- U& w" W - buzzer.tone(1047, 500);. N9 j2 Y! y# J
- delay(500); T+ D, j! ^1 S$ y" k: I
- do8 J' h: @; g2 {" g( j5 Q3 \
- {, p5 z% n0 L+ @ d1 W2 g) e' e
- int Err = getErr();% @9 V' |& e; ` J6 ]
- if(D1 == 1)
- [" a" {! r4 u( z$ N- _ - {/ b) N' ~: l* J! |. x
- Left = 1;% H% d: G& S4 s% E, Z
- }
# e- e2 V" a. @( B - if(D6 == 1)
/ h7 Z$ G1 d7 u* L4 b6 j W, g - {
$ j. t n. Q+ A( o - Left = 0;7 U' ?' p" F3 U2 k# M0 {. y
- }" D4 q$ r7 p2 F5 L4 X( P
- if(Err == 99)' j9 v* F+ r: V% o! ~* C
- {
5 o2 Z; a Y. v4 v - if(Left == 1)& o& J+ f) K" s l4 W- X M+ n8 W$ G
- {
7 H# S u3 {# S t - Speed -= 5;+ \1 V- c" X w" t) [
- moto(0,Speed);
: e# z: W$ J5 ?& |9 |8 e* d - do+ a) |2 }: J9 T% @; i; o
- {$ B4 [' Q; d2 V: l( x; M' w
- Err = getErr();1 n" q% z! F+ R$ b6 ^
- }while((D1+D6) == 0);2 G* v4 D% ~! K+ U. s9 X0 p+ V% {7 q
- }
6 {5 n/ c$ C$ s4 `+ S7 u - else& q# q- x# e) l1 I6 H6 |
- {7 J; `4 G& s$ k
- Speed -= 5;/ V0 N- K6 q2 v( X
- moto(Speed,0);
# K$ K3 P& B: }) r6 l) J+ _0 [ - do/ p# h+ F1 R4 z, I8 s# A0 `
- {
! O C$ M$ ?, P; b - Err = getErr();
# u) V+ R( |1 f0 n. A9 u - }while((D1+D6) == 0);
1 R+ N) B; d8 `! ?5 B y3 E - }
( n. t( c0 O, f( u) I, I$ Q - }8 p- s9 R% e& A p( K( T
- else, O% J& y. [, j1 d! H7 ~$ |" ], n
- {& x1 l \8 G0 x8 C) G
- if((Speed < 160) & (Err < 2)) Speed+=1;
, H8 F' j# Z% B7 R# U - if((Speed > 100) & (Err > 2)) Speed-=2;
! E3 D t* [9 @ `1 ]0 O) K - integral = integral + Err;
& h& ~( ^" r& T, S! L# z7 q - derivative = Err - previous_error;3 T0 E# y* D! f3 ], k' ~+ w
- output = Kp*Err + Ki*integral + Kd*derivative;
- U7 E3 g2 e7 _$ [5 L1 t. x3 R - moto(int(Speed-output),int(Speed+output));
6 l8 s" O8 i% t! M$ j7 V7 W - previous_error = Err;4 ^: O- H: R* p) X
- }
; r0 q4 D5 m2 K. i' X - }while(!((lightsensor_2.read()) < (10)));! R- v& Z3 K4 c o& h! \0 W$ L
- moto(0,0);4 L6 W- }* w. W. P
- delay(500);
^4 j; g7 m! A* M3 D) V - buzzer.tone(262, 500);5 H8 m$ ^2 \1 ~5 x, b
- }
. L4 g7 d: d3 ?( I
X2 [" t; X) N- int getErr()
/ L7 b5 a9 K5 Z - {
9 t# l8 ^% i* H* j - sData = linefollower.getValue();
3 [) {$ @/ E* V+ }/ L' g; E - D1 = ~(sData>>0)&1;" h( b7 k5 @4 q, u% G4 |% {6 j2 ?
- D2 = ~(sData>>1)&1;4 q' ]$ t! t. _7 c0 y) G' A- f: a
- D3 = ~(sData>>2)&1; V! q! n1 }1 q$ m' D3 @$ K+ w, t
- D4 = ~(sData>>3)&1;/ ]0 F1 l5 g5 O5 z; V* o) z
- D5 = ~(sData>>4)&1;
" b8 k. |; g/ ], h* y - D6 = ~(sData>>5)&1;6 c: }3 u3 K5 X: P
- int downD = D1+D2+D3+D4+D5+D6;: z9 o v8 v8 ] R8 P3 W2 y# _, N8 {9 h
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
% H" x# ]) r& p+ {/ _2 n& X8 x - if(downD == 0)
9 k/ U6 E/ ^# Z9 n; Y9 w, q, e4 f$ B' f - {) }3 Z2 t1 ~2 e6 [7 c* g
- return 99;" b) t; L- L7 F0 X
- }
/ ^+ O5 C( y$ Q Y0 t* C* ^, q - else' p( }, m( |5 S. {' p( ]2 M& ]. u
- {8 a* D3 U% I4 V) f6 }- e8 e
- return upD/downD;
; V# w% [2 ?; u4 P& V+ b - }
1 L4 z3 H' D' h3 Z9 t# } - }
; S5 C U: L+ I, M1 X
) ^# ]$ C& v0 ^' z. p, b- void moto(int Speed_L,int Speed_R)
. i5 s# V B) W1 _, n/ C5 z1 \ - {
3 W& f3 ?& o3 t' Y; I1 x, m: k( n/ r - Encoder_2.setMotorPwm(Speed_L);$ h, S4 {0 {+ J2 B7 B
- Encoder_1.setMotorPwm(-Speed_R);
2 x5 w8 Y& G8 J2 K4 P - }
複製代碼
! d. X7 x9 g: E* j3 c& a2 o* MMeLineFollowerArray.cpp
! t% b( U% x8 T; ?- #include "MeLineFollowerArray.h"
a7 _! B; U# H l9 x- \- W* E
- Z/ G) n3 A. p2 p5 X- #ifdef ME_PORT_DEFINED4 w" c6 S; X2 `6 d( `9 V* D
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
+ _- C: ]7 s# Z; l( ` - {
; m7 f+ G Q7 ] - . H- Z* r. x7 R4 ?) \7 _; k
- }
( ^7 f4 S/ P, c& @5 M9 j, R8 G( Y' v) S - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)- m6 _$ E0 v4 F' t+ d/ x; O
- {
) c) s* z1 i+ U/ g h4 u - _DataPin = mePort[port].s2;5 S: ?3 D' O) C3 n9 V7 d
- pinMode(_DataPin, OUTPUT); / D% [* g, T4 I/ Q3 S$ Q: J; C
- digitalWrite(_DataPin, HIGH);* P" M: |1 @3 W0 h2 b& R( x$ b
- }! Q: S- z$ Y" T+ _: F
- #else // ME_PORT_DEFINED8 ] f/ s) c# [; l0 d, @1 M& s
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
+ _/ g& k5 ^* O! ]. j8 D1 ]" C - {3 r+ f* N6 l8 o) G# V( Z" ]9 Y
- _DataPin = pin;% g5 L+ C3 n( L! g4 N4 z
- pinMode(_DataPin, OUTPUT);
0 t2 r- n U0 T K) j5 ~$ P! I - digitalWrite(_DataPin, HIGH);! b3 E2 _9 U: c& p. \1 P
- }
- ]& L( _6 K0 c/ i - #endif // ME_PORT_DEFINED
: [" A: S( r, J% z0 m. d
; F) f, O9 T0 _' F- 9 t- r1 Z) z" Z
- void MeLineFollowerArray::setpin(uint8_t pin)9 J1 k2 A5 j+ _: S6 E
- {' r& A4 k- }7 [$ k' v% C
- _DataPin = pin;
1 Q" d/ ^: n @) J* ^ - pinMode(_DataPin, OUTPUT);
+ P$ z' ~4 P7 G% z( H8 G9 n - digitalWrite(_DataPin, HIGH);
0 u0 q: h: I2 C5 } - 6 G% l$ U# [+ a* E( f9 m
- #ifdef ME_PORT_DEFINED
9 `1 P/ R& M+ I6 Y- t - s2 = pin;7 u3 l( I0 I# Y( m8 M
- #endif
6 ~9 c; Q L1 G3 k* p1 a; P - }& ]8 e9 n. l+ n8 Q& P2 X
- / _1 I9 C# K0 E5 h3 b* `
- uint8_t MeLineFollowerArray::getValue()# y% U7 X. V8 q4 n: R- [( }
- {
9 s' S6 E+ ~# k - uint32_t LOW_level_read_time;
# W* {/ C: K5 V* \ - uint32_t HIGH_level_read_time;9 x: A6 v1 B6 f6 A% x( [
- uint32_t time_out_flag;5 ^- n, g0 o( [2 X* N4 f; N
- uint8_t Sensor_Data[3];
8 ?* {4 M: ]# z- q' C M - static uint8_t old_data = 0xff;& u3 D5 x1 |% ?+ B. a
- & J6 _$ N5 `$ c; p ? \; ~
- pinMode(_DataPin, OUTPUT);4 c1 y) ?* _9 T$ e! e% F' C
- digitalWrite(_DataPin, LOW);5 N; y3 H- N( J6 Z7 R3 m$ ]
- delayMicroseconds(980);' q! y) ]2 K" x7 l8 c% q" c
- digitalWrite(_DataPin, HIGH);
% M0 J) U# k7 g; P- J- F0 |/ v - , D3 H# D+ e6 Y" c* g* J! \, b! @6 V
- pinMode(_DataPin, INPUT_PULLUP);) i* I, n1 L; H( q, v* ?8 K
- delayMicroseconds(10);
2 w y/ D4 ~. x5 h- s5 B) t8 D j E
: r9 @; w+ T+ A7 X; E) b" b- time_out_flag = millis();1 i/ R9 s5 k% B7 B1 q2 T
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" B4 Y9 o2 j+ I9 T1 o1 m/ }3 d
" C! X% |+ N3 u7 I- LOW_level_read_time = micros();6 n0 R, S$ O& d, z3 P
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
4 U8 U) {8 p3 s! p' ^3 ?) F* X/ G& H+ ]( { - {
0 g3 D5 e3 B, s. E9 C - return 0xff;
' ~1 N/ g5 Y( r& H- y8 w) ?1 X/ d - }8 S @+ ^( y0 O' S
- ( O$ Y% t1 o: d/ x
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); \8 M5 v* O8 B; N# N, a
9 k" X# P9 c0 d, w7 E5 d: I- HIGH_level_read_time = micros();
% ` w* J# Y; d v0 d* q& } - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level( L* _2 N% `/ }* ?" o
! F* g* N( |8 G& m( j7 E; q- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 \1 x+ M* l( i: A9 f# u - {
- W+ V8 Q& L3 g W& L3 m' |% i - return 0xff;
1 U7 f3 R! z& j' M( Y$ j. O1 S - }
, A) H# l; e0 a; v# d3 O - 9 L& v# e% z: b, t5 m. H4 s! ?# E
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))7 ~ a, O* y9 [8 k6 t7 o
- {
8 k4 S6 r% o' ~# W - return 0xff;, F9 l, L* O$ c1 K% o' t3 B
- }1 t2 f- r5 p8 h y6 S* r T( M! ~) u8 x
- ! R' A) o z, W# ~5 l5 |
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 S3 o( v1 {4 v - LOW_level_read_time = micros();
1 I/ k- V; [; `8 }2 P4 q* v" g - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
! h+ t/ Q6 V8 T- F% n5 I" ?2 [ - , f- ?9 |% p# Z' E
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
5 m5 e6 l4 F5 o0 N' W - {
/ P6 z' m5 P1 c - return 0xff;6 N& k% e! S4 h
- }3 c: q8 [& y0 \ m8 N8 J* z
& a7 f! E: d% A! S" r9 M$ D- s" K: x- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
; [5 d. t( T' v1 W. A- {! n - {
# h6 p9 P7 T! j; ~5 s - return 0xff;
* k- F0 t8 h& s$ S T; q5 |# z2 ? - }& R8 y4 k7 W2 _7 Z" t7 y% @) z% Y( t2 o
8 ]# M0 {, H& T9 t! {- for(uint8_t k=0; k<3; k++)5 h3 q6 q4 B% N
- {
0 t$ ?8 b. z$ O( m" X - Sensor_Data[k] = 0x00;9 V2 c% T$ G) i; c
- + ~/ o4 u7 D6 }) g! A( m
- for(uint8_t i=0;i<8;i++)( T7 o { Q. f' F' ~6 \% P' ]" j
- {
; y+ l: m% A" d( Z! t0 |, y& e/ e4 I - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level W: z& y8 l* n. ^+ b- ?
- HIGH_level_read_time = micros();
( x! ^- V \' x, K/ ?0 K$ C0 U8 j, L# [ - LOW_level_read_time = micros() - LOW_level_read_time;
! T6 i. F8 K) ~ p/ O
& j7 K# h8 R; }5 k) }1 a( _- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )# {; A' n4 r+ P3 `# `8 i% y6 m: j
- {
) ^7 ?5 ~; [2 }- L9 c6 P& y* { - return 0xff;
- K ?0 o6 n' \' @, ^8 O D/ y - }% z' G5 U8 c0 p. O& O6 q: r7 T
4 T. l, ]- i9 y( v- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) {/ G5 Y! v- f6 @
- LOW_level_read_time = micros();1 \3 J9 l, d7 ]; D0 R& u8 f% L
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level+ g2 j( J2 F8 w6 R I
$ c7 W. U( `- g; [ G4 G- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1# G3 q' A# n* a/ q; E* s
- {
( t7 V! L) n# }6 g - Sensor_Data[k] |= (0x80 >> i);5 c Y8 @( C T- c3 {' G
- }) `1 b8 }7 E5 L" }" Q( R; y
- else if(HIGH_level_read_time >= 100)
. R6 z: {2 E5 b% U - {
1 w& S$ y! p' ~/ ~' z - return 0xff;
5 x8 g `* b* h( \& W- P" I - } l$ H6 n7 M& w- `
9 x& E2 F; n& I, G, ~( \. {1 R- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out): X& z# W2 X' Z% H. i5 M- U* ]
- {" o+ J$ ]; P: B' U
- return 0xff;
1 ~4 h8 U; c, ]; t% J - }
8 i X S! s) {8 Y - }/ G1 g. H$ T. o6 a
- }
, [2 }# g5 Y4 u9 c8 Y. z - $ j" h' F* ]7 y! Z4 y5 `
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level9 v T+ V. l0 i7 I+ t
- HIGH_level_read_time = micros();8 A' c( y/ b3 }5 F" g
- LOW_level_read_time = micros() - LOW_level_read_time;
, F. X6 U2 X1 }3 i2 ~1 B; o - $ j' S. T. ^1 _3 m
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )7 T. T7 ]1 U) ~7 D
- {
( t' G, b$ M- d$ u6 ^ - return 0xff;4 P6 e6 z$ g* T3 R- \
- }
* Z5 F+ n# `0 W" q" T
$ E) A+ ?+ |- \' w; [- pinMode(_DataPin, OUTPUT);* Z) ?# a( m2 Z* ^' i* R1 {5 q
- digitalWrite(_DataPin, HIGH);& @1 @6 b# T$ k. Z% B" H
- ( b: R) Y& [# m! ^1 O/ s# B
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
* f& \& L3 R1 x$ n) H1 D& @ - {
, X C/ v3 Q: e. n* e - old_data = Sensor_Data[0];
5 T5 }0 e; ?0 k* b# H - return Sensor_Data[0];
* w( }1 R* G7 W# z - }- Q% C; i! h) X0 W5 L' u3 N
- else: e$ F7 C% F# m
- {
9 x8 K. }$ s3 i4 W7 _' X - return old_data;
, x1 T; y- O0 R$ Y) d - }1 \ }; Q3 f6 H2 x# _
- }
! M t, K9 N. p' N5 R, H6 v
複製代碼
& y; x; E: W3 y0 W3 v1 E- UMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
+ t! k- Y; e7 Y. Z8 ?( P6 p' x
: s6 N4 S9 d+ T1 ~, Q |
|