|
|
4 W& B$ `' L/ o9 i- #include <Arduino.h>
# \7 [ ?5 N, I, O( B n - #include <MeAuriga.h>
6 ^$ i+ ]' ]* n& a - #include "MeLineFollowerArray.h"
# c& g9 r/ M9 Y i" ^; u - $ N, A- d# }' ~: ?- c
- MeEncoderOnBoard Encoder_1(SLOT1);; N) F( k5 o4 J5 i ?3 O3 `9 @4 @
- MeEncoderOnBoard Encoder_2(SLOT2);
) C0 r% n: @6 E' Q" _: D - MeLightSensor lightsensor_1(12);: c1 b/ ^8 V8 F! f8 m2 r7 z
- MeLightSensor lightsensor_2(11);
7 I s/ l$ @. K' m/ O" l8 P' R - MeBuzzer buzzer;
0 _8 N7 Q/ W# L* ?; z8 d - MeLineFollowerArray linefollower(PORT_6);7 r- o9 M4 U3 e- @
- . r7 s% E) R3 X1 W, Y' Q, h& L
- #define Error1 1
7 q8 e7 U5 f4 a/ P - #define Error2 2+ k k) f/ x& w1 z
- #define Error3 3
" r" a- M! {$ |4 }8 @5 g6 `3 I' o
: Y( x$ R) m0 y7 |+ m- #define Kp 15
* g; J5 m0 ?/ ` - #define Ki 0.156 g+ ]: U9 i: T, j t% A1 q
- #define Kd 0.03
4 j: s6 O- x6 w3 _ - / ]! S% O( [$ g+ {9 b
- uint8_t sData;7 u* R$ r3 K4 R% m
- uint8_t D1;
( P0 b+ r) Y( S4 n7 y/ C - uint8_t D2; u" a+ _2 k! x) z
- uint8_t D3;
% Y6 D; ~$ W* I. C - uint8_t D4;' x" C: S0 s( o/ }6 P5 k! k
- uint8_t D5;
6 c, P4 h. m6 s9 Z& ? - uint8_t D6;
F1 V$ k5 ?! s$ Z# W8 _1 ? - 4 |% M" s- ~8 w& v" L$ g% C4 y0 Y
- float previous_error = 0;1 Q# J, K9 V k. V9 ^
- float integral = 0;- V! H( \. Y5 q3 o
- float derivative = 0;. k1 U: H+ L/ c; K6 x
- int Speed = 160;
; L1 L9 @" U9 @" s& G5 b - float output;
7 D% i0 i V s8 F$ ~ - \2 O# r0 U; k' r' A! n- Z
- byte Left;
+ D+ [( V) T+ V: H9 \9 `( C - * ~" q# n- v9 D
- void setup() # q7 E3 h: c1 U/ D6 p/ s6 q/ X! F
- {% T' W" [3 K- c3 ]. S' \/ G
- //Set PWM 8KHz
9 D9 E6 s1 W5 Z( H" O& b. } - TCCR1A = _BV(WGM10);
X% E/ y; {' E4 G! b - TCCR1B = _BV(CS11) | _BV(WGM12);
0 u* m( j9 i& w6 { - TCCR2A = _BV(WGM21) | _BV(WGM20);9 s& Q5 u5 \3 Y8 `
- TCCR2B = _BV(CS21);( m' ]6 x$ }* B/ i
- Serial.begin(9600);6 k. O" E- r- g, E3 z( n: A
- buzzer.setpin(45);2 b, R7 _$ J8 w
- }/ B! v! s0 Q; J
- ; [& L8 p) _5 _' `$ y7 x3 ^9 q
- void loop(): p; `- j. F6 j: L/ [: H
- {7 o- E/ `6 J8 h8 b
- while(!((lightsensor_1.read()) < (10)));: P M7 E1 {# [' M. f6 o7 Q4 ~
- buzzer.tone(1047, 500);& T/ w$ @3 Q, N7 a$ \) r$ m$ |
- delay(500);
9 f* T- L! Z0 d; |' o0 v6 X - do5 D/ k- W+ Y3 Q- @5 Q
- {
. n8 d6 S9 k) J% U4 L/ w - int Err = getErr();
9 f+ z. x+ K. X7 }4 H4 f$ E7 U - if(D1 == 1)
# w1 L2 v# ^' `5 N" L: `) ?/ M - {0 B$ k# q) l' e4 ?& b4 G+ c
- Left = 1;7 k. Q: F0 B+ u, n
- }
9 q x" n3 h0 d" c - if(D6 == 1)
$ H7 Z% C) }( S& x* A. G7 P$ l - {
- t6 O5 C8 l1 c6 ~2 s - Left = 0;' s) ? R p7 {$ _) v, Y( k
- }* K; [# [# F% x
- if(Err == 99)
- J9 A, M& o* H - {
_' e4 s& C z1 a - if(Left == 1)
7 d2 }( F# @! f+ A - {
5 S- i) K$ x; y3 k- ] - Speed -= 5;! g; n* L3 [' P, ?
- moto(0,Speed);
2 A& Z* ]5 \6 y: q5 R0 j - do
; \7 g' p* C1 d# \" l4 Z8 F9 t - {
^! K' X; M( Q( r" w c& q - Err = getErr();
6 |8 `+ p8 j. `0 z+ A - }while((D1+D6) == 0);; E! S% S4 l4 n: j
- }) Q! I+ P# V% x
- else
" ~* n6 f. Z7 Y; P& G7 P' H) O5 n - {: v; ?# E% C, A" F5 _+ T
- Speed -= 5;
4 |+ p: _2 H$ X - moto(Speed,0);
1 F: A7 _! z* i - do
/ Y l$ ?* R& A8 P% _5 i/ I) L - {1 {/ B3 r2 [8 g
- Err = getErr();
7 t& z* c: e! r - }while((D1+D6) == 0);: ^6 L1 v5 N, ]* y/ o
- }% {3 v/ M' a6 O! N/ d. s
- }
& e8 d% B! k- k2 I% ` - else
@# J* W2 g \( s( i9 B9 s- q2 v - {5 i4 b8 V- i, Q
- if((Speed < 160) & (Err < 2)) Speed+=1; - E' b1 P9 T3 c3 K3 F
- if((Speed > 100) & (Err > 2)) Speed-=2; 1 s8 N! G) ^; W4 d
- integral = integral + Err;
4 K1 W, H8 d% |* d - derivative = Err - previous_error;
1 A1 [6 k' v( Z2 Q* R8 A - output = Kp*Err + Ki*integral + Kd*derivative;
7 ]; r0 k4 ^. ]: n1 b6 I - moto(int(Speed-output),int(Speed+output));' | _3 P/ D# l: @% ^' H7 |- y
- previous_error = Err;
9 b9 _6 k1 E7 T" {* Z. D6 n. w - }
6 E' f" V( y: a7 e6 m r$ J - }while(!((lightsensor_2.read()) < (10)));
5 p5 C! Q& [, j4 F' k - moto(0,0);, K7 F: @! W; n2 t
- delay(500);
; n8 }$ {; e5 [ O0 z) ] - buzzer.tone(262, 500);
+ I% G" O* c& J( O( ]2 V - }
( G& [% F. y( q$ J7 [/ @) a - ) b* e1 _4 I5 [+ K2 f
- int getErr()# ]# t2 L6 q' I$ Z, O6 S) @ M
- {
1 \+ x" ` k$ ~ - sData = linefollower.getValue();
E: ?2 n- O8 e' @ - D1 = ~(sData>>0)&1;* m3 l7 m; t9 k6 o9 h+ b3 x1 k3 @
- D2 = ~(sData>>1)&1;
* x) s$ y9 f+ H# {4 t% j - D3 = ~(sData>>2)&1;& W- {! v" o- \, S$ r
- D4 = ~(sData>>3)&1;+ s# E& ], W; p* R* w
- D5 = ~(sData>>4)&1;
4 U; ]* ~+ \. a$ y" U9 [7 v - D6 = ~(sData>>5)&1;) u& D) g5 [8 N6 h5 z0 m
- int downD = D1+D2+D3+D4+D5+D6;% v7 K, V0 P- A' L8 N3 ?7 A& X
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);: R! @ {9 I7 m4 N
- if(downD == 0)
4 R0 g# n0 @& C4 ^6 c - {
% {) J6 g6 d5 g, _2 n( z - return 99;
* r( v& f6 ]/ d( G- d, P" e" o - }
/ H. Z# g0 ]6 ~6 S/ f; w - else( k- T' Q7 c# u* a: B0 i: ?
- {
8 @3 m! s& X0 v; C* o3 K" J3 g - return upD/downD;
4 J/ D9 d0 J6 {* ]: a$ k& u' y - }
: b( Z( Q* f3 i. A# l8 N% @ - } R: ~1 j; a0 W$ M2 q9 t$ w
- 8 x" D: S2 V' t8 W8 D
- void moto(int Speed_L,int Speed_R)0 `2 }* Q3 C' ~5 L$ }
- {- |8 |* Y! I. K. ]% _* {. v( u# N
- Encoder_2.setMotorPwm(Speed_L);, h* S. X# Q" ^- a+ D3 w2 n- q8 b
- Encoder_1.setMotorPwm(-Speed_R);, K G! E! `* L1 C8 p, `* t
- }
複製代碼
% F; X1 R" D$ @3 _ m2 t& J& ^MeLineFollowerArray.cpp7 B, _; r& N0 k/ j& B/ t3 n
- #include "MeLineFollowerArray.h"
) Q" S) I# Y) Z: _2 D% y! a, m - / J# c. e" v2 I2 L* D& ]
- #ifdef ME_PORT_DEFINED
1 C/ C" n: J. r) n - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
; y, u1 ]8 P' h5 X0 n2 w - {7 l- B3 O+ F) B3 Q# N4 g7 s C4 i( Y
) F# b$ L5 b4 T- X+ k( Q& |$ c- }
" ?( l9 E1 S3 d% O - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
) C9 I5 k8 e+ b- D H( G1 ] - {
$ E+ N! V' P, H3 k - _DataPin = mePort[port].s2;! r3 d; k" J8 `: G2 s- @
- pinMode(_DataPin, OUTPUT);
# Y5 s- L4 c3 w7 U/ d- s - digitalWrite(_DataPin, HIGH);
* b$ c( ?. q9 r$ ?+ o/ ^$ {' `, d - }
: b( N" m1 o. t v - #else // ME_PORT_DEFINED9 U; L4 e1 X/ a+ `+ f2 o
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
2 c, T5 A9 d3 |: u3 U2 B - {3 ^9 W( A5 l1 I7 _) h) p
- _DataPin = pin;7 d' A/ E+ N+ t/ Y6 E/ w# e
- pinMode(_DataPin, OUTPUT);
+ ]8 o7 |$ i7 j1 a - digitalWrite(_DataPin, HIGH);% L, C# p$ k" M7 d b
- }6 ~3 ^9 j z' ?4 t: U4 j) p( ?9 i
- #endif // ME_PORT_DEFINED7 X& B* m9 H# H
# J$ h+ C, i9 @( N* S
9 e/ d, ?8 k0 j5 v- void MeLineFollowerArray::setpin(uint8_t pin)
; s; J1 ^2 q& _+ l Z - {
0 D8 r- |9 M' r2 g! B - _DataPin = pin;- ]$ x( r3 z( {* a# k7 m- d
- pinMode(_DataPin, OUTPUT);
) A/ f2 l6 X0 z5 q, t& h5 ? - digitalWrite(_DataPin, HIGH);
R; v- g& ?( K1 M) W7 o
8 \" F: f$ P$ ?- #ifdef ME_PORT_DEFINED
+ P! p9 y1 H5 X$ p+ t' Q - s2 = pin;
5 B8 _0 j) b; C9 b: F9 b' L - #endif
, p* i( M; f9 U: { - }
* N4 a% X4 N- @3 c/ [( e. v" g - $ P; ^ ]( D9 C
- uint8_t MeLineFollowerArray::getValue()" ?+ c7 l1 }5 {, a. u$ Y$ a
- {; W$ r5 f( F$ r' E7 B- | `' i }
- uint32_t LOW_level_read_time;
; G- U7 u, F4 V& m8 r, z! B - uint32_t HIGH_level_read_time;" E [! |6 d* [& H* o; T3 w
- uint32_t time_out_flag;: K8 f7 c: G R+ _9 x7 k( j- R
- uint8_t Sensor_Data[3];: l7 q1 g5 F! k: O/ b* Y: [
- static uint8_t old_data = 0xff;
3 X1 v2 p9 L" W - % k* i: m0 a1 h3 t4 ]- m: k
- pinMode(_DataPin, OUTPUT);
* _+ ]: K2 i' p, ^4 s" f - digitalWrite(_DataPin, LOW);
7 m# [* \1 U5 \ - delayMicroseconds(980);- t9 w* k5 B6 z0 Q
- digitalWrite(_DataPin, HIGH);
% f1 d0 }$ @( ?2 N - 6 |- J/ y6 B$ {5 M
- pinMode(_DataPin, INPUT_PULLUP);# ?8 f; L2 J- }( ^5 }
- delayMicroseconds(10);- p! ~0 `! T I; S9 J5 l% K/ O
$ v" g3 ~. {6 @/ T3 e& m0 r# g- time_out_flag = millis();) m( ?* J& ^% A) N
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );! H2 Z# l1 N5 \ E, |2 N
- 1 L" @6 h8 l3 g7 Z1 V
- LOW_level_read_time = micros();# z1 p# m. s/ Z- _& u
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out7 {3 G( @: H# L# V/ u1 K5 j Q
- {$ g! C) B7 J) X5 X. ]
- return 0xff;2 w- `- u" {) E* a0 U
- }
& n$ `, x0 b0 o& k7 i: [ - 6 ?, h& M' d0 ?5 B8 Q+ g _! N. i& B5 |* d
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( h$ u P% H3 ]
- % r- Q' c6 b5 @$ A! ~
- HIGH_level_read_time = micros();0 B/ c3 W- \# T
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level8 [ x, P2 M1 J& D& @
- " c& f: X1 Q# D m' a: T
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 A& }# [/ x% B R0 A( \& v( G7 ~ - {7 r) `, u2 E/ F8 a- u5 V
- return 0xff;
. l2 x5 K+ p5 L0 s' f - }
7 r9 S; ]6 N; w7 m - K$ ^+ E7 D- C8 n2 ]' L( d Y+ l- j* o
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
# Z1 i6 H7 _" I3 c# j - {0 r$ S6 B9 R9 X% A" P4 w
- return 0xff;
8 C' W) I) A1 s% U - }
9 L1 z; c! s3 v# U7 G. S! x
" i2 z! _: q: r s% ~. @- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. J' Y/ O0 q8 V/ a6 J# ~ - LOW_level_read_time = micros(); v y# P. w% ]0 u( B
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
7 l/ _7 z5 y6 `0 m4 i
; d. f$ m7 z' ]9 u8 Y9 M4 m- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
, b2 P3 Y$ q; E: N+ E. z6 j - {$ }3 \( C9 V' g) E8 f
- return 0xff;
( d! ?7 G/ G1 K - }
0 m7 y. W* v% v8 z- H1 b& Y' v - ) W' K6 ^$ W( e
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
5 @$ R$ v$ ]0 w' [; a) A& k V. X4 y - {
2 R8 B9 @$ ?* J& L$ o* [+ n - return 0xff;
* Y S/ z7 R" \4 @! J - }. {+ ]3 A$ M- Z% q
# X% H. }3 I9 L; _8 J- for(uint8_t k=0; k<3; k++)
% \) t- M! I0 K6 D) r: @0 U5 O - {
/ W5 N5 b( W0 q2 l' J f: q - Sensor_Data[k] = 0x00;
. u6 {' Q6 j, C( a5 o Q, |
& G S! t6 A4 _3 h4 `- for(uint8_t i=0;i<8;i++)
& ^, t2 F0 u y - {+ S: f. _& g9 v5 t' {+ b. E
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level" l/ q5 @% n, H
- HIGH_level_read_time = micros();. `6 _ ]7 K! j7 W* K, ~ J; k/ p
- LOW_level_read_time = micros() - LOW_level_read_time;
' U5 E2 Y: e9 p. y - 1 O5 K) y. t( {
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )* F+ V- D* h# M1 e( q
- {7 J4 N7 h" x/ M
- return 0xff;
! `& K( e% k3 ~( d7 d. y) B( W - }
- n6 {2 e9 f& X3 C" b - 0 F3 ]4 Y S" C. G) c! j9 l
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ `7 w4 x z, o9 r
- LOW_level_read_time = micros();
. I$ U$ S- {" d- H+ t( m3 F- _9 X - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
+ P0 W K$ r) s# F1 V* S4 k. R8 e0 `
$ ?% X1 j0 }$ W) c4 x- f3 P- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 14 [4 o: ?0 t1 }3 l% P, u
- {+ ~! ~/ V0 D! x- P$ m) b% W9 V
- Sensor_Data[k] |= (0x80 >> i);
5 C: c a/ c' v' J) W - }
+ t: O6 e5 z* N0 {1 A2 n; w, l8 Z - else if(HIGH_level_read_time >= 100)5 d/ o; g n6 s! P2 _* s* T# _
- {4 Y) s3 G" y' p% V3 \
- return 0xff;
# M# W8 _" v5 F+ ]& k( Q8 O4 b B- _3 S - }
% N* L0 _5 Y) H) o9 t' r - 9 l- [5 d' ?& r0 i
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)+ u W! {% F Y: f: v; k- k/ A
- {* L% O& P5 C6 ]6 b9 y
- return 0xff;- c" i, m) C0 h
- }
6 f5 u9 `" v; `8 z5 C1 O: X - }8 D- w7 v8 ^0 K, Q# W- W
- }% t. j2 @: { {" }7 J
5 K9 m6 u; a6 Z1 S- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level4 Z- i( |3 j7 s) V n
- HIGH_level_read_time = micros();
5 c( u: A- _! O' X8 d+ D, e - LOW_level_read_time = micros() - LOW_level_read_time;
- u( w* r6 e+ ^ - 7 ], ]& ^0 i% x
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
8 {" o Z1 j" `7 v' Y- z0 } - {. ^) b+ K" |& B1 @& p
- return 0xff;4 }: D7 Y: X+ D9 h) T
- }
/ L9 a) d5 G5 ~' F! \ v1 O: B! h: D - 7 T8 @5 L& H3 X- C- K
- pinMode(_DataPin, OUTPUT);
4 n0 @; B0 {' H - digitalWrite(_DataPin, HIGH);4 s o& m5 Z9 c
- 4 Y. I9 A7 X5 T
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
8 S% Z+ [8 ] ^7 A( }/ w - {
' R' p1 [$ t9 d9 Y1 k/ j - old_data = Sensor_Data[0];. y& J1 @8 \/ q# t$ _* ~
- return Sensor_Data[0];
+ k( ~7 D. e! B/ L U5 u - }( x, [7 D- ^! N t% V. ?
- else
$ ~8 U8 L5 b3 v& N7 l d/ c0 ` - {# d: l0 c( c9 g. d/ v
- return old_data;# N, A8 \) f7 Q3 t) N* s" h
- }
# r- q" ]6 x B h9 m- Y; `, u1 i8 W - }
# t. @/ X9 J5 v0 d! E7 j0 A; D
複製代碼 / d" G5 x4 x# I3 [
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
" i: H1 Q/ n% [, x% X4 e1 Q5 w8 @) m) ]; W
|
|