|
|
$ N+ `7 x$ z" W6 Y$ C+ \: L9 X' Q- #include <Arduino.h>
4 Y2 T4 } ]9 K" X5 f - #include <MeAuriga.h>7 s7 W% M( W6 ]7 V' W& ~
- #include "MeLineFollowerArray.h"
3 H0 m2 C) O3 V! C) I+ h9 B - # N7 q% E, Q# X: L# _8 Z
- MeEncoderOnBoard Encoder_1(SLOT1);& ?$ e4 x3 P" L& G3 W# J# Z" B, J
- MeEncoderOnBoard Encoder_2(SLOT2);$ Z d) g5 H$ m- n
- MeLightSensor lightsensor_1(12);
- R& }, d* g+ F/ C) C+ _ - MeLightSensor lightsensor_2(11);
/ ^5 q6 F3 B6 ?& d6 g0 a6 h3 m - MeBuzzer buzzer;
$ Z' ?* r8 \4 _ - MeLineFollowerArray linefollower(PORT_6);$ L0 g* G! A+ B1 s r8 o1 _% _
- 6 y1 a0 B# b8 z# L% d8 B; B
- #define Error1 1
7 d# k; e, z4 M- h/ l% U, c6 ^ - #define Error2 2
8 R% Q' L8 V/ w" M7 b - #define Error3 3
& y7 M3 Y1 L2 R% _% @+ Q$ l; A1 | - ! U7 e4 e: h9 Q8 I, E4 z* J! q
- #define Kp 15
9 \& C) T) }) ^& d - #define Ki 0.15/ O/ c8 h* ]7 Z
- #define Kd 0.03
) @1 d- c* \0 y4 M) z/ D3 \
' K0 F3 W- I' b9 x4 e- uint8_t sData;* L5 e; R+ v# C e$ p3 Q
- uint8_t D1;! W1 N& {/ L1 ?4 L) F$ r- Y; ]
- uint8_t D2;' i5 S- T- Y+ W2 B
- uint8_t D3;
_" a* N" F0 a* @ - uint8_t D4;
% W- G L: l7 p8 l5 r - uint8_t D5;
* `; w/ I% ]+ O. @ - uint8_t D6;
! F1 ]( M! i9 [3 r" H( F
7 f3 `/ p+ J! J4 V- float previous_error = 0;/ }" ? l. c1 a" A6 Q. G! h0 F
- float integral = 0;
% [! h6 u. N/ v# ]8 \# ^) s1 D - float derivative = 0;! P2 Y- C7 u; x
- int Speed = 160;
' t4 D' _. i4 z8 _, ~! L3 r( A( g% @ - float output;
: ~) v5 l9 J, }8 l; e' @
- t0 L& O4 @4 i5 ]6 |- D% W- byte Left;2 D% }1 Q+ M, e \1 |3 M
- # U, F+ L6 b9 k4 T
- void setup() 0 }3 R( O' w% Z( A( P
- {9 @4 h1 ~! [2 l9 M/ t5 k. {
- //Set PWM 8KHz0 f% q" J! G* X: ?- K
- TCCR1A = _BV(WGM10);
* ~7 R q& h! \3 i - TCCR1B = _BV(CS11) | _BV(WGM12); t: ~# L. `' f
- TCCR2A = _BV(WGM21) | _BV(WGM20);
2 \8 }" M0 _5 {4 F - TCCR2B = _BV(CS21);; V5 s1 ?9 m/ X6 N
- Serial.begin(9600);' Z$ I7 x3 s) ]2 k M5 `
- buzzer.setpin(45);1 l0 y& p: m" Q/ q* ]# p
- }
7 d! ^. q, G! ^3 f* I
1 b9 c' A4 K' p" w- void loop()" k& }/ s" b. P3 m( |1 W3 F& W% c
- {
# |& x5 n5 W6 j' ?8 b+ `$ B - while(!((lightsensor_1.read()) < (10)));& x; a: \& T" S$ {
- buzzer.tone(1047, 500);
3 G! a* W5 \6 Q4 O/ p3 X# P# i8 x - delay(500);3 A$ r' Y: K2 V: ~5 F
- do
. j8 q- r# F; c X$ b; u - {- `. X; Z0 q/ ?' F H0 ^
- int Err = getErr();5 d. G4 l9 ]5 z/ n. m$ e; `9 Y9 Z' Z
- if(D1 == 1)! z2 n' }& _& U9 u+ P
- {
2 J0 r h! y }, S% h6 I - Left = 1;' I l- {' p" j* H2 q2 z0 b
- }
! }& N+ P7 l; S9 A, Q - if(D6 == 1)
. T4 M8 h+ j4 a {+ t9 T - {
6 g8 E9 C5 z8 Q/ m - Left = 0;
7 W0 y# R5 Y9 K5 x2 p& N8 m - }
( ~3 D* g+ m: X6 L: F - if(Err == 99)
b/ l! q* w S7 n# ] - {
9 Q0 b1 E6 \- l$ K) t; i1 q7 N - if(Left == 1)1 v* A- k# L( w. ^, i
- {
$ {# Z1 B4 a$ d0 T4 d - Speed -= 5;
* S& x# ]4 v6 A& ~+ _# w" I - moto(0,Speed);6 Y* ?8 m$ z: ]4 c* J
- do3 f5 z1 ~) n" n
- {
: {( I9 D7 u a) k: A - Err = getErr();
7 j/ k; \; ?1 N. ~$ d& e' n! ^ - }while((D1+D6) == 0);
" z. N" N: c( w0 O J - }
7 _6 v0 U6 A; }, O* f" i! O - else
6 o2 C4 g4 O3 V. o1 C# l( J - {
3 V0 e$ Y9 p7 F! j% b - Speed -= 5;' d# U ]7 l; a9 M1 [
- moto(Speed,0); D' Y. \3 d" }: Z* b. X$ ]% `
- do
( k# Y1 Y6 _" f0 _4 c - {
5 e. ~) }- A, }: O1 ` - Err = getErr();
4 j7 W2 x- z p- Y4 _2 K$ o& {" p - }while((D1+D6) == 0);
, ?! T9 s2 k9 C' g - }( a: o( U& U" @5 ?
- }
" G; z+ s O4 @% v, M - else+ k4 Z. L# Q$ {8 `9 C* L
- {: a/ v+ t' e+ {2 L# L' ]9 A" @
- if((Speed < 160) & (Err < 2)) Speed+=1; 3 F; W( _" F8 e* k" \1 J2 r
- if((Speed > 100) & (Err > 2)) Speed-=2;
1 u9 D/ d5 H& r; j- }2 u - integral = integral + Err;% L T5 {" f: A; d% J
- derivative = Err - previous_error;1 [8 A) Q4 p" l% F; ]5 |0 p5 _0 ~
- output = Kp*Err + Ki*integral + Kd*derivative;
2 m' X- _ \+ u - moto(int(Speed-output),int(Speed+output));) a0 Z3 k$ c U5 M0 N1 H! W. F- S
- previous_error = Err;3 z P* W) y$ ~+ X8 D& p
- }: N( ~6 u9 n( q& q9 o/ I! W
- }while(!((lightsensor_2.read()) < (10)));& h9 J; c9 A( C, F8 ?1 `
- moto(0,0);
+ C) {- _' K, [* x( n0 L; K" _8 Z' Y - delay(500);
5 E. c* [# \7 L9 A3 ? - buzzer.tone(262, 500);
: R' P" T, z! H' {) [' d - }
+ r. ~) r# h4 c2 l0 K9 n8 | - 7 u8 m+ U# k l) ?$ D
- int getErr()
* q3 |) f! m! L) T9 A" X: K - { 2 ?, e5 a9 Z! E+ u! F
- sData = linefollower.getValue(); _0 N* ^2 i# G2 d; L* v- `0 ?
- D1 = ~(sData>>0)&1;5 ^& `# N% V6 K
- D2 = ~(sData>>1)&1;
* R+ H) N/ [; z( ] o/ [9 G: m! T - D3 = ~(sData>>2)&1;8 ^* H: k: h3 N |1 i3 o, x( _3 G8 J
- D4 = ~(sData>>3)&1;
- E. x& ^% k$ Q& F7 S - D5 = ~(sData>>4)&1;( u. k9 }: A- c1 D9 N3 o
- D6 = ~(sData>>5)&1;
/ `, _8 C: ]# ~4 b& G - int downD = D1+D2+D3+D4+D5+D6;/ A/ E2 K/ C1 r$ \
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);) _ L7 @5 c, l7 v) h# | o
- if(downD == 0); P; \9 o4 V& w' z% E0 _
- {1 ?# c6 k0 g% J2 b# ^
- return 99; d2 p5 g: |, L t
- }4 I' K/ \6 M! @9 K: G! @5 E
- else
3 Y& w3 Z: E7 E* `( l8 _! H - {
( i! b- ~8 @4 k7 h - return upD/downD;" I1 w% f$ `& S8 z. K- K/ `
- }
9 l, ?9 p; W" ]; g2 O0 {2 b9 k - }" s: x* N V/ D. w ~ C# E; J
( I _! b+ e$ @4 A8 _- void moto(int Speed_L,int Speed_R)
1 D2 Y2 N2 |' c6 f5 h - {
$ _/ p! m9 P- H3 L& j+ ` - Encoder_2.setMotorPwm(Speed_L);& e1 V, l; q# ^( {( A2 [8 j/ w8 p0 v' d
- Encoder_1.setMotorPwm(-Speed_R);
) x. x$ I+ p a1 | - }
複製代碼 ; \" A) ^/ u) Q' q" q
MeLineFollowerArray.cpp( A9 A% d# z. X) O
- #include "MeLineFollowerArray.h" x1 O% C; b+ V5 _! M4 _* ]. B
- $ \* o& K' R" t4 z! p! d3 u
- #ifdef ME_PORT_DEFINED+ N2 ]! N% n5 b" |6 n0 G$ B
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)5 G7 y M4 X" m T5 | B' X
- {5 o: O6 F8 g+ u# _8 V- y6 j
- ; N$ j) p H; T( F1 z
- }5 q% T0 \5 R, U9 ~. S
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)( r- Q/ }3 p7 \: c2 f
- {
0 s S" D1 n6 U- e) ^6 J& g - _DataPin = mePort[port].s2;0 x; V: n, H) }2 _
- pinMode(_DataPin, OUTPUT); ' Y( ^6 s# T, x$ }3 C6 q% f+ u9 H" N
- digitalWrite(_DataPin, HIGH);" |: ~$ s+ H" \& _. X- s9 @
- }5 d" G# ^2 ~. R4 Q% b/ E
- #else // ME_PORT_DEFINED, Y$ @- e5 X" g) x5 k# l
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)4 H+ b: @8 H7 g( P1 r5 y0 b5 x
- {
) d+ ]7 G9 j, t5 c! m E. v - _DataPin = pin;
W; Y/ T+ b7 }9 O' ^ U - pinMode(_DataPin, OUTPUT); 1 g$ I: X. P1 N w1 ~: L3 ]* g
- digitalWrite(_DataPin, HIGH);
0 o$ g) @# L% H* }/ s% I - }7 l5 }. ^& i, a$ R. Q/ C
- #endif // ME_PORT_DEFINED
+ ~4 g. K M$ V( p
' Z& [1 V8 S" {( X' G5 f
% b; L& j1 N; Y2 i3 b- void MeLineFollowerArray::setpin(uint8_t pin)- B; G6 h: \8 x1 |. w, q! g
- {6 c+ q& T- D: r& n5 p2 i
- _DataPin = pin;- ]3 \2 R' m' ^* m q* b3 i
- pinMode(_DataPin, OUTPUT); : b& H" y" G! |; i
- digitalWrite(_DataPin, HIGH);# K0 Z, y4 v' q' z }4 `6 D( A
- 4 f3 t+ B/ u( U
- #ifdef ME_PORT_DEFINED2 p# a7 g( F5 h: W4 a7 q, |0 K. e
- s2 = pin;
3 O! l! s, `/ a9 N) _% P! j - #endif
' K4 a4 D- o" O2 O+ E - }$ o4 I. I. B5 x' v; E3 w1 S
- , X2 o6 ]0 N1 r5 a" J
- uint8_t MeLineFollowerArray::getValue()
8 x& Z. \9 y6 H: r9 t - {0 u% G, W% T: f1 E# f: @
- uint32_t LOW_level_read_time;) J/ \7 p/ s8 }) q* X' H8 w
- uint32_t HIGH_level_read_time;
2 T) O: V% C8 F - uint32_t time_out_flag;' h$ M. R9 @% }$ C
- uint8_t Sensor_Data[3];/ Z! D1 |7 k( c1 v5 n0 U: l
- static uint8_t old_data = 0xff;
6 S \# t: @/ R6 L" d. f, H - 7 c! V. n* g' e% {5 u
- pinMode(_DataPin, OUTPUT);
3 ]2 D. c6 _. C7 z a- q4 d. e - digitalWrite(_DataPin, LOW);7 K& T, S# m8 `* V
- delayMicroseconds(980);
- Z) ]& u7 t( l0 R' v2 g! G: o - digitalWrite(_DataPin, HIGH);
% L0 z$ ~% Y8 C& W0 k0 w - , D. x$ R8 U3 Y( W
- pinMode(_DataPin, INPUT_PULLUP);4 V! y5 P/ c7 o6 f, b3 x* p2 d
- delayMicroseconds(10);/ f" P& `2 b% I" o n: n# X# w5 F
- , k( K6 A: P% i, s
- time_out_flag = millis();
: N7 X, C4 d+ G" }# ^+ Q8 n - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 `; L) x) \ R
. v% r% I+ ^8 I+ G5 R- X- LOW_level_read_time = micros();' w1 h& Z& g/ l" }, G/ x
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 x' S- D0 G' Y# g7 L/ i - {
8 q' [7 p2 |) k- d* q7 E - return 0xff;# M. Q) \. K3 A: x1 ^. n5 {
- }
6 \ |* H* b/ ^ - ' b9 ]* y. N7 d$ C- ~/ W. z( Z# c+ s
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
' {* @- |3 N3 K: n - $ X* |0 [ c" @2 v4 V% b& Z
- HIGH_level_read_time = micros();
( n8 p. v5 f( S. O - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
/ U+ j5 @: q) ^% [# o, F' N3 a - 8 { H5 B) ]3 I" m; u; S
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
/ y9 t1 d! W- c* n) g - {
1 ]3 i, E( C/ l. j - return 0xff;
0 C* g! l2 O7 O' M) }& I2 h) o \ - }
% p$ k$ A. _- i3 D
" e, d! w2 o7 v( t1 n% z- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
W% B: |3 E% u/ R2 j9 ` - {2 K F6 B0 ^0 n
- return 0xff;+ k$ n1 B7 S# J) ?% @- v
- }7 W7 W0 }( _) ?- e* |, q
- |2 `# a. H8 i$ Z9 L- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 C( f& c# i/ G
- LOW_level_read_time = micros();$ i. z4 T! F, i2 R1 u1 J
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
# \, P% w+ G* v: T+ F
$ X" [7 ?9 D) k; f$ A9 t- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
4 R- k9 E ~: f8 D - {2 X/ z5 t J8 P8 ~6 J- r
- return 0xff;* Z) c1 g/ S2 O" g) ?, f2 I( i, J
- }
) h) R) b6 Y# R2 m) _ - 5 c$ _4 w, h( n. f' n% W
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
8 [4 [$ o! h3 i# A% | - {
7 D1 C, o+ ~; s5 a1 s - return 0xff;6 \& S5 y% h" D* C8 q$ Y6 f
- }4 F6 t3 _8 `' |& P
" I% u' k/ u1 q* c2 y. K, z, `- for(uint8_t k=0; k<3; k++)
k; b5 u. y; i# b& J& L' k - {6 E# E) }% ?2 q, r. H8 V
- Sensor_Data[k] = 0x00;/ O9 ~0 q3 \+ G. K& G
, l% U' S8 a- |: t. ^1 ]' ^/ P1 s: U- for(uint8_t i=0;i<8;i++)
$ c2 [2 t! l- W( {9 Q9 ~. g - {; k) L8 m( w" U2 k
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level7 J# T* t- U# f8 i+ B/ u
- HIGH_level_read_time = micros();+ h0 v8 k1 `& B) ^6 T+ w8 L6 [
- LOW_level_read_time = micros() - LOW_level_read_time;6 V0 q2 O% e2 b6 j3 f
( u% ^5 G$ T8 |2 `( l) F0 y9 @! e- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
! Q+ [. R3 f* B# j# }, N9 B" C: { - {6 q( H% L' V. A, b' p) N
- return 0xff;
7 W2 K: e5 f% I# l* k/ t7 l% P' U - }" T9 n5 ?. c4 T1 r" j" c
- 5 R/ \( q3 @ s! Q0 @! o
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) U, ^+ _* U8 B" v/ C4 S' I# R; L
- LOW_level_read_time = micros(); X5 o1 A& u0 N. w
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level2 y1 K' Q3 p4 p8 d) x
7 Z8 N8 f. J2 c) `- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 18 a9 g9 j4 m; y' Q. x8 K
- {
" |! O7 O5 ^- C& |, N - Sensor_Data[k] |= (0x80 >> i);
( Y& \8 s# M$ i. Y4 u - }6 r1 }/ t( T2 D B% D. h5 J
- else if(HIGH_level_read_time >= 100)
) c3 r9 E1 R2 j$ ^( M5 @8 p! ] - {
8 R0 A' p7 y" |6 |9 Y5 ]5 c7 d2 z - return 0xff;2 H- B& {4 p& w) \: [% r
- }
( W! Y* Q( h- r) ]) W( ~' f) e - ?, @3 M! ?6 ]. {! x5 z) D
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
& X7 b: E" }3 @( W* A/ x, t - {
$ z& @# f$ E: d0 j: \+ p - return 0xff;* ~% W- Z1 i9 ^4 t; K- K' B
- }
1 d' ?/ } p; A! E, W% b& I- r! G - }' I# I2 H$ R. R! G2 K, H8 o
- }
7 z0 Z% o0 z" W% @( V" @; R6 d
$ {4 N- B7 i* @- X- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level# l' I, W7 k: n9 e; c4 \
- HIGH_level_read_time = micros();6 [, w8 L$ ~: p* |5 ]3 f+ N
- LOW_level_read_time = micros() - LOW_level_read_time;8 y- ]6 C, ^! b0 t' U5 `; U
/ ~* C3 q4 @! k! k, n- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
1 ~; I1 G5 w. J; K7 A, ^ - {& M9 I4 G) S# w
- return 0xff;
/ J* E) v0 d! |* o0 m, z - }- E7 H3 S% G, E! m5 x5 h
- ; } _( C8 `5 f
- pinMode(_DataPin, OUTPUT);
`) x2 K2 R B* V% D* G$ J7 } - digitalWrite(_DataPin, HIGH);) f, X$ v8 \% l" _1 c9 E
- & b7 }' A/ F9 B. \& F' M0 X% }
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
+ O4 @! }4 B' [ J - {
6 S3 v- f0 _6 p I' p8 l l% u4 A - old_data = Sensor_Data[0];
! I) I) V% p; l" ?( n7 u+ p - return Sensor_Data[0];
2 B* a, m/ t9 x! \- P7 u6 S5 Z - }
1 `' E5 x0 y4 U - else
( D( R' w$ n! L% e6 w - {
$ h7 _& \; K) q: O - return old_data;
: @5 \/ I* H( E7 \' T& E! i4 j - }
; e( S% w. R' l( c) q/ ?8 z- ` - }/ G( @/ A) f+ i5 E/ N; j* |
複製代碼
% j" f" e8 J( }. ZMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
+ {( F+ c c! P* {# Q" \
! h. _$ }9 r; Q n9 W; ]) e5 G" y9 [, T |
|