|
|
; c0 K7 M P' ]) V+ c$ T* l: z- #include <Arduino.h>
# u' i4 H8 Z" J; b4 X: ? - #include <MeAuriga.h>& }( D. }) h# D5 q! O4 r
- #include "MeLineFollowerArray.h" t! K: B8 \& l. {% U& l, e% E
+ k) t7 q. W0 m) }$ s! W/ N0 z- MeEncoderOnBoard Encoder_1(SLOT1);! R; I9 W- P5 F, M
- MeEncoderOnBoard Encoder_2(SLOT2);
3 I* s# c" l; \1 X$ N. B6 _ - MeLightSensor lightsensor_1(12);. D3 E9 K0 R) L
- MeLightSensor lightsensor_2(11);% y+ j/ {* r6 }8 Y2 U1 g8 l! O# N
- MeBuzzer buzzer;
" M- M! h/ o, I2 M1 I/ @4 y- V - MeLineFollowerArray linefollower(PORT_6);# u( l" D. T. c3 x; D: x5 F) K
5 O2 ?1 A$ f4 U$ A6 C+ z% y' C( V0 _- #define Error1 1- D* w1 I' f' a u, o, U& O$ B4 p
- #define Error2 2$ ^; ?: {1 g3 V* z
- #define Error3 3; ~( ? V, a j. w( Z8 Z$ S/ W+ i. }
- % U4 _7 d) h/ V/ }
- #define Kp 15 \; {% \1 t% k# w$ b q
- #define Ki 0.150 W! h( a3 N1 {
- #define Kd 0.03& j/ ^8 i$ ?% K# o8 c8 e u
; g0 g& V& I1 C5 o- uint8_t sData;
. G3 s" C3 O9 Z+ _: n - uint8_t D1;: e L3 L* u8 b ^; x
- uint8_t D2;3 i$ d q, ~6 b* `% V/ e: \
- uint8_t D3;
5 K& [& R* R! Z# I - uint8_t D4;, N. [( H0 Y& u! g, {4 f
- uint8_t D5;
7 d5 h) S3 v9 g; |; Z$ x) i2 z: E* j - uint8_t D6;, A( o; M* o0 ^) z. A" m/ n8 m/ [
- 3 S/ x6 w/ M/ E; i9 f ^
- float previous_error = 0;/ U) S& q- Y9 y
- float integral = 0;! S$ i- {/ l+ u' t
- float derivative = 0;
3 w S! |" ?* K& E - int Speed = 160;
% l [% U; e' v - float output;( K$ u' n& L0 {! G
( Q; T+ b3 _; I2 P, Y8 [- p- byte Left;" W0 Y: X8 S2 E$ {
' \4 R% i4 [' s- x: K/ I+ p! I- void setup() ; m+ n* Q8 A1 T6 U; n
- {, S) R3 ^- p% ?, f
- //Set PWM 8KHz8 x* ?& ]1 S2 B: G7 z
- TCCR1A = _BV(WGM10);% \9 F) }4 C8 ^
- TCCR1B = _BV(CS11) | _BV(WGM12);0 E8 n/ @; k4 s! e/ ^7 J
- TCCR2A = _BV(WGM21) | _BV(WGM20);4 J1 k+ o$ b' D" W6 r; Q
- TCCR2B = _BV(CS21);
9 W; p+ n# @6 U1 Q, Q0 @ E - Serial.begin(9600);) r8 P5 G1 a* i; V. h
- buzzer.setpin(45);
- _% R; Z: s g5 Z( j% i" ~ K8 n - }/ g$ X. t. e& m0 I ?+ _; b
4 ~& B4 W }" [+ C4 y- void loop()
% ~% b8 E: R' v% u - {
0 G; G }( \" f2 H# I# G - while(!((lightsensor_1.read()) < (10)));$ q8 q4 W5 s6 L4 ]8 ]
- buzzer.tone(1047, 500);
! B; t/ m* J5 n' j - delay(500);6 ]) n. r. l6 }' M
- do
9 l! p% Q& ?/ D: I - {
% O9 Q& z3 i& }( w6 I - int Err = getErr();
9 v9 ]1 m% j+ i: |0 X) Q# W8 Q - if(D1 == 1)- z: x1 l" Y1 L; ]2 r' h& W1 e
- {
# d# x! e W3 Y: @) y - Left = 1;8 ?: `; _- D% G9 \" Z/ @/ G
- }; d$ f# F: N# L+ {/ n" i3 |0 ^4 n9 I
- if(D6 == 1) v4 o+ j e# c) ~+ }1 b5 y3 _
- {3 n7 h, N4 }8 M
- Left = 0;$ g8 U4 _4 \- |0 z; ^, p
- }3 z7 I5 R! r. u; Q
- if(Err == 99)
4 S9 O4 h3 U! z0 p. c! Y) o - {0 f( {/ G( `/ s2 T6 D4 J# A5 Y- u5 h
- if(Left == 1)0 U5 x( M3 [4 u4 X9 [5 @) D" h4 d
- {
- P( L- e! W2 ?7 f - Speed -= 5;
+ b, K, P: C |- N q - moto(0,Speed);
' c" z* r1 G0 m5 V - do9 F) u# s E( I _: o, e# @: v
- {/ g# A! G6 y9 F; C* Q9 T5 V1 h. N
- Err = getErr();
" e/ f! M7 @# s. Q0 z7 N - }while((D1+D6) == 0);( }7 v2 P x9 [. I/ L( {4 \ l* z
- }0 p/ U$ _* j6 W9 b+ l/ A; |
- else
* x2 V( n" n) R# [ - {
$ ^7 J. Y8 k" t. P# X7 g2 O - Speed -= 5;' T: b1 E) W! O) i7 c; W4 M1 S
- moto(Speed,0); 9 X; L' o# ~ \( F, N+ \. ?
- do
, ]# E4 R3 A* k5 r1 `0 t' P - {
$ U' e; x' `0 C' x - Err = getErr();
+ t4 V4 _% V' K - }while((D1+D6) == 0);# h7 F4 R/ l) u- {4 p$ p O
- }
. k3 { G( `8 E; G b3 O4 n" ? - } Z5 `5 T/ f' u
- else
' q+ ^+ Q6 c' i. r8 h% c/ ~0 Z4 |" @ - {
( R& R# K7 R6 l3 J6 s0 V - if((Speed < 160) & (Err < 2)) Speed+=1; ) x( K; y, L" b" o k1 X; R
- if((Speed > 100) & (Err > 2)) Speed-=2; / O4 ~ S5 t0 @$ I, E
- integral = integral + Err;, Z! i3 G& D8 t3 x
- derivative = Err - previous_error;
P- @ c: x2 T2 c7 e - output = Kp*Err + Ki*integral + Kd*derivative;
- V7 q' N3 e4 i8 } - moto(int(Speed-output),int(Speed+output));: v1 V& O2 O9 @: o& M+ I8 q
- previous_error = Err;$ @9 A% i( q0 t# @ S1 C
- }
: f0 [! u$ A5 p# ^* J - }while(!((lightsensor_2.read()) < (10)));
9 }3 N& v9 U. D2 f2 J# x - moto(0,0);3 Y4 y2 e; {8 J* f2 t) ~
- delay(500);
0 f9 m' @5 G! e: w/ m& L6 c& G - buzzer.tone(262, 500);
' i6 G! [ Q1 E) J; K) k - }
& i3 P+ P& s. N$ I) J* l2 e - - [' m" v4 O+ m2 }: G
- int getErr() q' @; W& b( w5 M+ Q" L' k2 o& u
- { " G% \( I% \. ~: L& S
- sData = linefollower.getValue();) f( t- M( N- ~% Y2 l
- D1 = ~(sData>>0)&1;. U* m6 `' S) C1 E7 R
- D2 = ~(sData>>1)&1;
% O: Y) R: s {4 [0 t' i - D3 = ~(sData>>2)&1;
: K' F/ [' c% f0 [9 E( s1 J - D4 = ~(sData>>3)&1;2 D+ A0 x( e3 w7 E) V& J
- D5 = ~(sData>>4)&1;
5 a2 R* P! j# Y, D - D6 = ~(sData>>5)&1;: A6 |& O4 U7 z1 N9 f" @
- int downD = D1+D2+D3+D4+D5+D6;! k3 F6 i5 t% o& e7 D/ k6 Y7 M
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
( d6 T( [( V5 U' ^& F1 B/ c - if(downD == 0)0 K# }, y4 L) h( F3 o
- {# D7 d! U. l" \ H P8 o
- return 99; V# ]* R7 A1 N/ w
- }
( [" x* Q" T( o6 m9 s - else7 y% E& O6 x& b6 O' D# J: {- Z
- {% g$ i! ~. g' E0 y0 U2 e
- return upD/downD;) d9 ~! C" y2 d. p8 M
- }
; E* @1 Z- O9 O0 J- y" l$ N - }9 M! z( W0 b0 U$ ^8 K/ c# ?
4 }6 U+ ~9 m8 L8 ?/ s. [; Z- void moto(int Speed_L,int Speed_R)$ m/ S% Z* N/ K
- {
$ ]; M' U+ M% j: R \/ O3 D - Encoder_2.setMotorPwm(Speed_L);0 M1 J) ?4 f& y7 h' ]' S! a6 \
- Encoder_1.setMotorPwm(-Speed_R);0 b, C7 F5 L* t- @ u c8 q
- }
複製代碼
6 T! a3 j9 _' q" P$ Y# oMeLineFollowerArray.cpp, r# K: z( [2 h. A
- #include "MeLineFollowerArray.h"
& n0 I ?5 `0 e
0 C" O# q3 p/ {. m. @- #ifdef ME_PORT_DEFINED
+ ?' D( U3 t+ y$ I - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
7 q. K0 i6 @3 |8 G; M - {
M. j3 Y# F; y9 \ - 1 c$ t: M, f: ~" E
- }% Z0 B3 y( r& J) ^' @' Q$ E" u
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)5 L0 q1 O$ [$ H5 ? A1 D: N9 P
- {
% |! P M. w5 m, v - _DataPin = mePort[port].s2;5 m7 Y/ |4 ?, w, Y Z' r" f" x5 ^/ Y
- pinMode(_DataPin, OUTPUT);
( W4 N# A$ E9 r6 j; ]' U) O1 A8 R* E - digitalWrite(_DataPin, HIGH);
- V X7 ^' X; @+ `1 c - }
2 l- p; w: F* S1 j# X0 t; e4 |' f - #else // ME_PORT_DEFINED
1 G( a/ L1 |( b. L* G - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin): ^7 s, l: b+ h2 ]# `
- {) B$ J, h- [8 q& t* p% ]
- _DataPin = pin;
. v& [+ K& O# q% F, k! l( s3 a - pinMode(_DataPin, OUTPUT);
v$ A% p1 O3 I; f) K: ?7 B0 L - digitalWrite(_DataPin, HIGH);
$ K% y* F6 c2 |- y0 i2 _; v7 \ - }
" x2 w0 S( @5 t2 Z0 o - #endif // ME_PORT_DEFINED
6 K+ `& l# O/ c% N4 \& A - " \' W+ g1 z. c, J( X, r9 w$ L
- " o6 F4 b/ \! D% _& o
- void MeLineFollowerArray::setpin(uint8_t pin)
7 f1 z3 `3 k# ^, p* z - {
8 H. D* S5 e* X7 i$ N - _DataPin = pin;5 x) E& H7 W$ Z& r4 L6 V
- pinMode(_DataPin, OUTPUT); ( g e B) f; e* F# ]" `2 Z+ x
- digitalWrite(_DataPin, HIGH);( E1 ?2 A" V3 S9 A8 X, z) S
+ F4 F% Y! S3 h& ^9 X- #ifdef ME_PORT_DEFINED, K, ~2 t9 c( j/ y+ c1 g
- s2 = pin;
9 n; r3 n5 R( N" Q# ^ - #endif
/ s5 i, k- H5 Q - }
* y, h. q, F2 y/ {3 m, j: c - 8 x9 Y, \3 k% {* v( n \) k$ p' ]
- uint8_t MeLineFollowerArray::getValue()
" r5 N) m! t, q/ a5 M+ G% W - {
; \ s; b) d* A* W- q+ P/ | - uint32_t LOW_level_read_time;
' ^! `, T- V/ X3 Y, V" z; d - uint32_t HIGH_level_read_time;# ~: `5 z7 G& @" V; f
- uint32_t time_out_flag;
, k0 r9 |) K" l7 `2 Z5 ? - uint8_t Sensor_Data[3];7 p: {1 C) a' X$ B. v1 S9 q
- static uint8_t old_data = 0xff;
6 r( x! ?" y4 I% Y
9 o! `6 R1 J, p* N( X: ?# ^" f- pinMode(_DataPin, OUTPUT);
0 x7 }' W+ k. | - digitalWrite(_DataPin, LOW);
0 _- B; u* H& B - delayMicroseconds(980);. z9 v4 V/ A; N7 k. K* j$ o
- digitalWrite(_DataPin, HIGH);
& G+ c" W" i9 Z' \2 b4 T
3 g- m' f% e g$ J% D- pinMode(_DataPin, INPUT_PULLUP);
. O# g. R" Y$ Y; D, K& I C) a' J! Q - delayMicroseconds(10);
+ b5 j# H. p2 L( R1 H7 F' ^
- A a5 y* Q/ q+ A0 I# r- }# X- time_out_flag = millis();9 _* v! s% l+ C0 g# g8 q
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- q' T0 d8 J, {
: l+ A+ [% {' s: P% F; g; v- LOW_level_read_time = micros();
% K8 f( B6 O7 G$ X - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 P& a* e5 N/ ? - {9 s* E2 y2 ^5 N2 q% y
- return 0xff;6 y m, B% D- f# c* Z, u
- }, l% i- N/ G% N, B3 D/ c# Q
1 B$ W2 R7 N# E( t+ a5 U. x4 t- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
) n3 U( ]- m7 i" q& G - % ^. o- b$ k2 p' ^3 a# J' G8 o1 ]1 n
- HIGH_level_read_time = micros();' B4 O: b+ X* X
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level* d0 Z7 {0 _+ W. |( O/ |
# W1 {- y l0 K+ u" E6 `, G- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
% u8 ?+ a! p, z - {+ M" X: K3 ?/ l1 ]# N! ^
- return 0xff;8 F5 [4 |1 W$ T5 W' O |+ N
- }! \3 G9 Z& @7 `) I9 I4 u* }5 x% I
8 |2 w% V; |1 `& G/ H" k& d% m- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
2 x* u& h2 L- R( ?9 ~) l9 g - {8 u, {, d; P6 L5 g, J3 z% l
- return 0xff;1 c( \2 W1 b h; U9 q; A r: w6 X8 h
- }
( [0 E+ G; P. V& X3 V
8 n3 ?8 t2 J% y: e" r0 O, r- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 D; `/ b5 K& Y4 @7 {3 y - LOW_level_read_time = micros();
% _% }; u: h+ r - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level$ A3 f$ K% d& k- J
' T0 k9 s0 k: h- _0 D# e& Q- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 m; x) j9 N! `: u# p( P) y. h, T - {, W; K* z9 { o
- return 0xff;
* F( L8 b6 G5 S. r, W - }
# [7 I; o% {/ H" h- ~2 Z - $ a+ D4 m9 L0 R8 x, y
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
3 F% E5 E; \( X* p" R; y - {
- V/ x( a m! u9 Q- b { - return 0xff;
+ h& T! g( R- ~" `6 { - }4 A' |# @" ?* i) X
- ' M% Q% h0 t( Z, w
- for(uint8_t k=0; k<3; k++)$ i3 U" X0 V9 k3 a& w
- {/ i5 s" v8 s( o
- Sensor_Data[k] = 0x00;
5 M, ~' I. r S+ X* r
! w+ u( W d- J+ d7 m- for(uint8_t i=0;i<8;i++)9 p- }8 `% i8 b0 w' ]4 q# ]
- {
( u$ G* b1 H5 ]+ [3 t: N- i; ? - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
! ^/ l' K. t9 z6 z3 |9 @ - HIGH_level_read_time = micros();
^5 B+ ~" ]' T- n. q/ ? Y0 K - LOW_level_read_time = micros() - LOW_level_read_time;; u3 M' r o; X2 e, n- v% O
& {& G* _* o6 l5 V( j- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
; S- v9 d: E' C. b, u& _+ g - {. l0 i% ? u5 |% J* s9 y
- return 0xff;
# Y/ R3 W! @% T) L- U4 ~ - }
# m5 |; L. x3 R+ ]+ A. d* d# m - % ?- L7 l7 |' c" o
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( C6 Q1 r. Y' B* a: O! _
- LOW_level_read_time = micros();
2 `2 I) n& ~9 M+ T; r& b8 p - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
" n! k* k, ^' ^, e; d - - V9 P% C6 ~8 Z: r4 i( o" I
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
9 ?* T( J+ s0 |% o6 y6 t - {
4 G% i# `2 V1 [3 X1 O2 J - Sensor_Data[k] |= (0x80 >> i);/ |( q% _& A% P- @/ q5 v1 ~$ i' U' e
- }
+ m$ @' f5 m( s# {* ` - else if(HIGH_level_read_time >= 100), \. B' _, j7 G
- {
$ ^' w) w- A+ R+ E6 c: _' b: l, h - return 0xff;& b; B' E \* ?/ n
- }" D3 m2 m6 r) _
- / T J5 f1 l9 r6 z5 F
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
2 c, i/ w1 g( b; \3 J1 v5 H - {% b5 i* M7 i7 b$ j5 \- J' s* Y
- return 0xff;
: W& D* a6 z) P2 G& S2 P1 Q1 B/ E - }7 C& w$ F0 k: \# F# o3 K
- }; s* T2 }$ V4 T( b2 k# C; N
- }
* d8 c( w- u8 i& H1 n
. l$ H/ r1 T- N! K8 k- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
2 D* L1 p5 _, ~ - HIGH_level_read_time = micros();
2 O! E+ t: V# W" s - LOW_level_read_time = micros() - LOW_level_read_time;6 }3 Q- _) ~. o
+ E9 X: W8 `# i* `" W2 l- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ): N. m2 }" F1 B
- {
s0 _% e' R2 ]0 G - return 0xff;& d' m* |- f$ b9 ~* w: ]
- }: m, i- l2 D9 H3 W/ g" O
5 \3 Q% P- l% F- pinMode(_DataPin, OUTPUT);
& b. }9 K' I8 ^/ s: u" i' j' \8 z - digitalWrite(_DataPin, HIGH);) |# Y8 z7 s! l g. B4 b, G
' V. F `7 x( w; l v- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))( u( @7 ]; \6 M" e! Q T2 Z6 o
- {0 x3 q. z6 Q; z* R. l% K( h6 U( _, ~
- old_data = Sensor_Data[0];; }% K. Q- B" h8 e9 v8 k, p
- return Sensor_Data[0];; J% V3 I' E% L) [7 ?/ s
- }
+ }/ z' n5 G! o( H, P - else" m, B* ]( E6 u: e$ l
- {
9 a. m, [8 |/ Z6 I! i - return old_data;
( U3 w" Q1 G8 I% e/ T$ z9 k* h - }
* y3 e V0 i7 u6 b2 z, K - }
3 g! A( {. y7 f1 }/ P( o
複製代碼
6 v+ r4 e, J4 a- N1 ]; V: EMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
( A3 k! L% ]! f/ h2 h
+ V/ M: U1 l$ q9 Z! F
|
|