|
|
0 M) T& A, Q: _) r) ?- #include <Arduino.h>
) c Y4 g' D* a6 f! A( h- G# N& O - #include <MeAuriga.h>6 z& i: z6 a! ?& {8 D
- #include "MeLineFollowerArray.h"
+ H/ n. Y( N0 ?& _7 x
7 n0 ^. N- k1 G# v+ p& |% C+ W- MeEncoderOnBoard Encoder_1(SLOT1);
8 Y9 N* P( h5 ^! h, _4 f - MeEncoderOnBoard Encoder_2(SLOT2);
3 O, P+ s8 J3 M - MeLightSensor lightsensor_1(12);2 V" B" Z; @, g9 L7 |
- MeLightSensor lightsensor_2(11);' L0 L; J' w$ w' v
- MeBuzzer buzzer;9 C f7 j* g3 [ w
- MeLineFollowerArray linefollower(PORT_6);
' E3 I1 C) b$ C' a0 R - / F4 G/ K1 ?7 O' ?; @
- #define Error1 14 C1 u1 k% ? ?& K( @
- #define Error2 2, G) w0 S. d1 M) R; `; J
- #define Error3 3
; m* U) M3 V8 @! n9 s3 b2 \) P - 2 s# ~3 g' _1 J- _# x( u' i4 _
- #define Kp 15
+ p$ C4 M o4 L9 u0 t - #define Ki 0.15
, c/ \3 T8 a/ Q- q3 K2 t - #define Kd 0.034 v2 \* M, f l% ^
- & s. }0 [+ e* m) \1 w
- uint8_t sData;
0 c4 Y( Y( R/ I1 q' g - uint8_t D1;
0 X% g! M5 `7 ~& {; \' _- E3 ? ^ - uint8_t D2;
# |% x! Z6 u1 i$ m - uint8_t D3;0 o0 m0 T, [- i1 h( l
- uint8_t D4;; T3 b2 A% ^ A) J$ K5 O+ b8 C& K
- uint8_t D5;/ _* F; |, B4 r" h3 W( J% B
- uint8_t D6;3 D% b6 L% q- q/ X, ?6 e
- D+ A2 X) u( B2 i; N- float previous_error = 0;( b7 d: A9 K6 u4 x
- float integral = 0;+ F3 @/ L0 v- p2 d$ s2 J' D
- float derivative = 0;7 ?4 T0 ~. [6 F' a9 k) l# Q/ T7 u
- int Speed = 160;
5 ]% _! g( V6 Y( n* b - float output;( i: M0 D2 Z, d
- 2 @. ^% [3 s2 Q' f: j8 w
- byte Left;
* n1 k' p! P; A# `6 V, o3 M: S+ S
" m: o# ?0 _% j, U- void setup() ! h$ L- T% L5 ~2 q$ N# N
- {$ t5 q% ~4 y1 |: I- ]9 f
- //Set PWM 8KHz
2 R& Y2 b: T, L/ ~, k - TCCR1A = _BV(WGM10);0 ~2 R* E0 J! ^% Y6 f$ ?
- TCCR1B = _BV(CS11) | _BV(WGM12);+ t" O5 O" K9 d7 v% m: V8 `
- TCCR2A = _BV(WGM21) | _BV(WGM20);! N9 ?+ G2 g7 P/ j" Q9 L& ]
- TCCR2B = _BV(CS21);
2 ^* m' U7 A9 ?' l9 M/ a# t5 i - Serial.begin(9600);* l5 j. ]5 V( C+ P, ]
- buzzer.setpin(45);. U. X5 X7 Y* L; [
- } o; J3 Y+ c5 V7 G/ h `5 {
- ( g& l) U6 Y3 |' e! n/ i5 u" F; y1 p
- void loop()
' r6 s& |) S+ O g' R# K) F' Q - {
) |. H! C! c4 C+ ]' h' Y5 k - while(!((lightsensor_1.read()) < (10)));
2 o+ W& Y6 S8 O* Q' \0 c1 t - buzzer.tone(1047, 500);) n M9 r$ R/ S- G- K
- delay(500);; U, h, b* S: r
- do: X2 i& k( M; p4 Z8 A5 c; O" X
- {; V. c& ^1 {" V
- int Err = getErr();
( w4 f$ s/ h$ H9 J - if(D1 == 1)
9 G- O- P9 y( Y# e0 F/ ` - {
& s5 \" r% l# P& ?/ y - Left = 1;3 T3 m6 y. T) f
- }
; _: I' M E, K; I( V2 T8 W* r F! \ - if(D6 == 1)
# |3 y# t% u& X, t! S - {! y. T* l5 i: O( W0 k+ j
- Left = 0;- Z6 ~7 \; T! |" \# b
- }1 t$ K: O+ K5 h' |3 |1 R4 S
- if(Err == 99)
6 l, ?5 J/ Q" R - {
, @. \8 {9 B. b$ {) H9 v$ W R - if(Left == 1)7 B+ p4 X6 X: K
- {
7 r& t4 n9 A, a6 R! w - Speed -= 5;
, j9 X" H- Z5 U7 U, j; P5 \; A - moto(0,Speed);) M: O, R! i+ M- J7 t
- do
) e+ |$ r, ]1 M6 H, d& p7 f6 }/ w - {
9 F9 Q' _1 f; X+ Z3 M1 O/ S - Err = getErr();8 I) T7 O* H& _/ m6 k$ C1 K
- }while((D1+D6) == 0);% x1 E, [7 v2 [- A8 `5 R7 N6 {
- }
6 y5 H+ U ~( I# V" X4 Q$ \) ~0 u - else( Q3 U, O5 D: B* g6 g
- {
1 f$ g# o: @# S. x4 Z - Speed -= 5;" t2 M( ^) m" N d+ b# s
- moto(Speed,0); $ E, y: o! m x5 x, ? e
- do
1 A+ I h( E1 N' Z - {
) ^. o, A6 T) V% y A - Err = getErr();
' a; d. b. b8 a - }while((D1+D6) == 0);
4 e3 n# V3 f P$ z8 G% ? - }0 u0 P( ]# v w. @8 N6 P n$ X
- } q/ X; q( U/ E9 B- f
- else
9 i! u* X' i) Z U - {+ t+ T; Q7 |& e6 f3 y8 \$ p
- if((Speed < 160) & (Err < 2)) Speed+=1; k' v4 S5 x% O6 d' a! B$ r( f/ S7 e( i# G
- if((Speed > 100) & (Err > 2)) Speed-=2;
1 u6 H$ s3 H* s; k$ v6 B: B) v - integral = integral + Err;
$ K `' s5 o5 W- N& T& d; h - derivative = Err - previous_error;/ _- x+ {. P& m$ U
- output = Kp*Err + Ki*integral + Kd*derivative;. x5 J: ?- g% m3 m: a" d- Q s
- moto(int(Speed-output),int(Speed+output));
3 V- K5 |6 I: g+ y - previous_error = Err;: W/ e5 H# Z" ~. ~. R6 C% g" h$ C; ~) C
- }$ ~- v0 r0 g; x7 w/ G- @7 Z' ^* i
- }while(!((lightsensor_2.read()) < (10)));
8 d) }4 k: Y! a. G3 k9 G! ~ - moto(0,0); @+ j: u2 r4 R
- delay(500);
* e) \2 C3 e9 p3 y0 v- ? - buzzer.tone(262, 500);7 ~' U$ c* f1 n9 q5 e9 H
- }
8 T9 \" `! W; y& p8 e3 i - : x* l2 i! n8 J X/ P
- int getErr()
$ N% C7 N! a7 g2 r1 A* I- M% B, B - {
, w6 Y- c" R A5 V$ K - sData = linefollower.getValue();
2 F8 ]- n- `0 ^! e1 E - D1 = ~(sData>>0)&1;* l2 O9 i; k; G1 f
- D2 = ~(sData>>1)&1;
7 q7 y, g# ?6 z; Z$ g - D3 = ~(sData>>2)&1;
: [; F7 j7 j/ Z - D4 = ~(sData>>3)&1;
/ H g, A1 M! U; y% v - D5 = ~(sData>>4)&1;
3 A* N+ F9 y4 c M* k V - D6 = ~(sData>>5)&1;
) n, X' c, q7 I: }* D; _ - int downD = D1+D2+D3+D4+D5+D6;
2 c* [2 w0 ?' D - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);8 n( \4 A3 K: a9 ]8 X# F
- if(downD == 0)
3 U: P* M8 d" r& V% T- a( u - {
! K! L- ]0 b& e% ^2 o) M Q - return 99;0 u1 X; o0 ^2 k) K: H
- }
( u! B. e) R- |- Y; V; j6 N - else- X# B! A$ O: y
- {
F# x5 R; U) x# i3 \ - return upD/downD;1 A, M9 M+ P; Y+ e0 p Y3 h$ R! Z9 r
- }
/ E; P8 @! C- L8 ` - }/ Y) w& s$ Y* y) h" b( W3 W
" V) V }& U" M1 E: B. ~; p- void moto(int Speed_L,int Speed_R)
( `) |3 x' Q9 \2 r" R - {
4 P7 H6 v- a: T m, j0 U - Encoder_2.setMotorPwm(Speed_L);6 t1 X0 q5 }& q7 l
- Encoder_1.setMotorPwm(-Speed_R);/ N9 J3 r' {# S7 M6 P0 q
- }
複製代碼
4 X- _) R; {4 O( {MeLineFollowerArray.cpp! t8 @& V$ K' t$ B+ N, a
- #include "MeLineFollowerArray.h"
6 E" J& F/ h2 S - ( _: p4 `* M1 X
- #ifdef ME_PORT_DEFINED
% P1 t# D5 v3 E" n - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)3 a, ]! n" t& J5 x0 n
- {
& S- I H% ~1 Q; T% m
, @) h: T, h6 y" J5 L3 w: }- }
' d+ t' b0 U! _, b# a3 {* W4 J& O) f - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)' {( Y8 E' c; t, s3 G. q7 q4 U
- {
8 t! F. D' o7 h0 h/ B3 x) v - _DataPin = mePort[port].s2;0 D( Q2 E4 H1 M3 U+ X# y
- pinMode(_DataPin, OUTPUT);
: i1 h" H+ _' J; Q" g - digitalWrite(_DataPin, HIGH);6 w' Z9 O: [( M
- }4 d! L' M3 N* E6 y& c! ^
- #else // ME_PORT_DEFINED
2 @, B$ e: _# C - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
* C/ C" M, J0 Z1 p3 I - {# h' e% m( L: j) X0 D
- _DataPin = pin;
2 |& M4 n3 x2 l) l D - pinMode(_DataPin, OUTPUT);
, }. m! K( y' S: O - digitalWrite(_DataPin, HIGH);
* q; W7 z, k! O3 v8 e/ E/ p - }
5 K* Z) t t2 N7 x4 E& W, a) m d - #endif // ME_PORT_DEFINED; E8 G- ]6 t+ |: S
- & q& s7 ?4 G& @" a5 r% i
0 @" W9 {" u7 {, U+ t3 [! k- void MeLineFollowerArray::setpin(uint8_t pin)
# t+ X6 l) c. t8 v) u9 Z$ b - {
' b- }3 a1 J! X: M. Y; z0 j' w% b - _DataPin = pin;
9 M( f1 h. H+ v* C+ }9 ^ - pinMode(_DataPin, OUTPUT);
* m" D1 B8 v# B# H1 x ~$ p9 L - digitalWrite(_DataPin, HIGH);+ x, b$ W% f2 C3 k9 w0 j" D
: Z1 g2 M( h5 A' Z$ u- #ifdef ME_PORT_DEFINED
- z1 n- D3 I/ ]/ |& M* x: I - s2 = pin;
8 W# t) p# @6 ~# A7 Z- | - #endif
+ K i9 E9 v/ r: m - }1 @- `" Y6 v$ t$ a( o" \
" Z8 ?7 d3 T7 P* C$ J- uint8_t MeLineFollowerArray::getValue()
+ Z( L4 j" Z6 ?$ u D0 D - {
7 e2 x7 x, g8 j' v' |: A! F - uint32_t LOW_level_read_time;
4 x+ T" m9 {1 X0 i4 V - uint32_t HIGH_level_read_time;
& p) e& Q7 ~3 n* ^4 r/ }, ?. x7 t - uint32_t time_out_flag;
& H8 v, k4 H; V, n/ ]# A1 C% _ - uint8_t Sensor_Data[3];0 _& c' i: c# d( ^% z! Z* d- Q/ `
- static uint8_t old_data = 0xff;2 B3 _( T& b, v; p# I8 K
2 ?- n* F' `/ B1 z+ ?- pinMode(_DataPin, OUTPUT);6 {8 K) D# `: ?; I
- digitalWrite(_DataPin, LOW);! Q1 ^5 W+ Q- L* u7 c: c/ O
- delayMicroseconds(980);( B7 R4 p0 I$ d# [1 }
- digitalWrite(_DataPin, HIGH);
! t0 y8 [8 k c0 O9 q: }
2 ?! I9 [& _4 g: z& i: ^- pinMode(_DataPin, INPUT_PULLUP);6 p: f: ~( O+ f- z- }# j
- delayMicroseconds(10);
6 Q3 _5 [9 M' g- ^
' I8 ?+ h# |6 {" l/ d+ W- time_out_flag = millis();8 Z# x. ]4 E1 _- S! l. v
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 H) Q9 u, ]- O4 e' r. R
- , P$ L" `5 ?+ p1 |7 W2 ~/ v+ G
- LOW_level_read_time = micros(); \- P$ _, ]& I4 t8 a0 J) V
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 B2 i2 W+ k9 ]3 ^ R R( B8 g
- {
6 n' L7 f3 U2 S5 G$ o1 x7 ? - return 0xff;) y. m' I J5 Q7 Z- P
- }
; [. E; D& k' I0 Y/ [/ j
# _( c6 L: R+ e- q- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 \& J4 m1 m; y. i& E
# D( V V, Q* V- HIGH_level_read_time = micros();
% @9 Z5 B6 T& U1 }9 x% A - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
3 Z. l; {1 B& P! N5 F% a - , g9 s. T2 g0 \! M
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 h, l( G. D. V8 ]/ k2 s - {8 u" m* ~% w, d: `5 x+ r
- return 0xff;, _' {5 h2 ^4 ^: N! @' p) D
- }
7 U% ]; Y7 Z8 B ^ - . O0 y I: U8 D4 E0 `* E
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))8 ^* ]( b j/ A7 i3 w
- {8 k' \2 d6 }) L, j! r6 b
- return 0xff;
. M3 ], c: A1 b5 j( v% s% b - }
, {$ Y* _5 E6 w) t, k% B - $ i1 w2 K& g- U& h, S* j( o8 o: o
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, W" Z/ }" p5 _* Z. _6 e
- LOW_level_read_time = micros();2 C! y0 ], u( j: C
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
$ B! @, c; e6 n! l' s4 s7 q - ; Y0 [( P# g# G: U" ?2 D6 L. P
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out- w; _5 i0 q% ^$ }7 [
- {
9 h9 h8 b6 O* Q. {; \' u - return 0xff;
) I! c" i( Z" } f) q - }6 h- c' B, S4 c" l
6 B. l( d0 f+ m' b( H& l' a) E! ?- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))2 s" I2 p+ {! Q* O$ D1 f! m. T( |9 K
- {6 e( ?# G+ i7 S* i. c# J; Z7 a
- return 0xff;
3 N! A+ W. s. S$ G# m! c$ k - }$ {( [ i2 {4 s) i) j
- 7 {- r e( S0 U$ F
- for(uint8_t k=0; k<3; k++)
+ ]: @4 [4 e5 u& l- E+ F0 \ - {
- V; W! H# `$ ?% V& H; { - Sensor_Data[k] = 0x00;- u# ~5 v9 W- C* u
- + t' ]- E6 N8 B
- for(uint8_t i=0;i<8;i++)
" l% M! R ~5 T ~. Z* ^1 e - {9 m! }5 k! I- g' z9 B: m
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level( `4 D+ T- W% F& |7 R+ m
- HIGH_level_read_time = micros();
0 J7 v( \: h# @8 t1 [. j - LOW_level_read_time = micros() - LOW_level_read_time;) Z3 D3 `* e+ l' P
' C% |1 R! ~6 j# I5 W- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
* \! G/ q& A5 Z - {8 [) O- D) K' C Z4 g
- return 0xff;; d5 N# U3 t$ d
- }% p$ q) @9 t! {. @" ]1 d
; F. T. i3 c% a% O- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# J7 J; s! p% ?% q% k: M3 ^8 I9 B - LOW_level_read_time = micros();3 i* Y$ |# A- ^3 z
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
5 ` y9 S8 B. Y( u/ ` - U0 {9 b/ \3 [0 q3 D( _4 K
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1; A2 Z3 B' \! Q7 p' \( C( g
- {/ E* j4 Z. I3 C& G
- Sensor_Data[k] |= (0x80 >> i);
* ], a+ t" ^, ?0 ^/ d4 t- Q - }8 G* v9 @7 @- w! \2 j2 h( O
- else if(HIGH_level_read_time >= 100)0 |# M0 C# R, x" B: |5 h
- {
4 u- M3 j$ h1 f$ H& L/ l - return 0xff;6 J& b& M, f& W6 p- e
- }. m: Y3 m9 z, T- E1 V4 m" |4 K# r# g
4 v1 C1 n2 h Q6 L0 d- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
7 C0 ?. u* K7 ` - {' V) C0 |8 b T* e4 O2 ~; d
- return 0xff;
5 b. V* X1 M* H) }5 A: }6 X& n7 Z - }8 j5 Q0 s/ ^& {; S1 H2 H6 h2 P8 U
- }% ?) y4 w) J/ z" j( C
- }" U2 C j$ ~' V8 h) |
; S/ {1 V- S. T7 l: @; S$ J; L- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level. j& C2 U( y% D8 u9 x& _0 M( U
- HIGH_level_read_time = micros();! |+ D- k* M1 i! }* g5 N7 ~
- LOW_level_read_time = micros() - LOW_level_read_time;
* W2 x1 b, U" U9 R4 e5 A O5 P
" }5 @! F& \# n- h7 x- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ); P* _* M, |( m/ ~' C9 O i. R! A4 K
- {
. _( ]! P$ Z+ f0 n }& V2 H - return 0xff;8 d8 ]" v2 |! f+ o. } {! n9 y
- }
; @ V7 ~, G* F6 Q3 D" f! O - % M* p% g, s; o X+ z u
- pinMode(_DataPin, OUTPUT);
% L; F( r9 |+ \2 a7 `5 e& [ - digitalWrite(_DataPin, HIGH);7 o; w! f" E. J' b' P m+ D
- ! a$ q2 s# ^( c8 C" E
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))6 k, M6 d! i. Q) s+ d) ], Y
- {2 r/ ~; L" Y2 y* e) X- C: r
- old_data = Sensor_Data[0];7 b% }1 m8 v6 A1 D- n
- return Sensor_Data[0];2 O R6 d& k; y _, C1 c2 h6 q
- }
' P0 g+ }5 R7 h. b - else
& ~" V6 Q8 ?. S$ i. N - {
5 R! D% s% C5 w) ~ - return old_data;
" Y' `5 v( c$ c1 ^" E5 _ - }, S2 U, G9 t9 v( p
- }) d( ^2 |9 d3 M$ Z: g+ S. n
複製代碼
3 ~9 ?3 ~; f* }7 O! _' g: g0 p1 G: q) mMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
) w- `& T3 L" C9 y) v) h b. }' k9 J6 c5 K
|
|