|
|
% {8 @, D$ x7 f( x% L- #include <Arduino.h>
$ i: {, N* O5 b4 f: A" P+ e - #include <MeAuriga.h>+ y& r5 ~, t: k3 I3 f l
- #include "MeLineFollowerArray.h"- u; Y$ l u3 [/ O1 z
- : `" ?3 x$ S1 k' h; c2 u: q
- MeEncoderOnBoard Encoder_1(SLOT1);
3 z: w- ~. t) ]) g5 _; ] R - MeEncoderOnBoard Encoder_2(SLOT2);! E5 H+ S, @9 d! W& F P3 B
- MeLightSensor lightsensor_1(12);' I# ~+ G8 W2 b2 S: Z+ r
- MeLightSensor lightsensor_2(11);
$ d, q( Z( ?" I9 R9 D) I7 @& T - MeBuzzer buzzer;
+ T/ _2 |) w- T4 U - MeLineFollowerArray linefollower(PORT_6);- i4 _3 w8 h% b& ~
- + s! g+ y) L. I* G1 z
- #define Error1 1- |7 A7 y/ P$ H! n
- #define Error2 2; Z3 v% d6 H. @- Q
- #define Error3 3$ s) ]% r, N+ N
2 V+ _* R$ B- g2 z- #define Kp 157 V6 A5 ] D7 }7 l4 M
- #define Ki 0.159 m7 d+ M) p0 V$ \+ M8 P) K
- #define Kd 0.033 s; y7 Z7 D! i3 p( E
+ ?2 @) a; y! S+ v1 w2 J- uint8_t sData;% U: Q2 y$ b% p+ f, |
- uint8_t D1;6 L0 U* `! _% b9 e1 H
- uint8_t D2;
- i# X: s/ F: w - uint8_t D3;- O" {% _, A+ y K
- uint8_t D4;3 n6 ^ L$ |1 [, T+ U- a. ]
- uint8_t D5;
( B1 o! f1 _; A8 I; I9 p - uint8_t D6;
" o+ U; H% E8 o
* x, r8 B* X; E$ s- float previous_error = 0;
, E2 W' A& i+ ` - float integral = 0;
( O" d: f6 I+ J% |$ z - float derivative = 0;$ N7 x4 M; Y d/ |* u
- int Speed = 160;
& Z( ]; Z1 I1 {- L, p/ R - float output;
* f0 P, Q) n4 A w7 t8 p) g - ) d' X6 C8 d) m. O k) ~0 U
- byte Left;
+ a8 U& g9 q+ C f" F
. c2 J) y" j1 J3 c7 w- void setup() 6 W0 e/ h9 n& }
- { y$ h4 G8 t* k+ _5 G2 O1 B
- //Set PWM 8KHz
- Z+ B% J9 J7 o- x2 J# c; {( V - TCCR1A = _BV(WGM10);
( w1 }+ h1 f9 [2 D) F% C: N - TCCR1B = _BV(CS11) | _BV(WGM12);( K+ y' g7 Q- U" f5 x& u0 R
- TCCR2A = _BV(WGM21) | _BV(WGM20);' T( W$ [$ j) k
- TCCR2B = _BV(CS21);7 Y. s q4 ]- R/ I$ f3 S
- Serial.begin(9600);9 P' C1 m) Z! k. ]; q8 ^
- buzzer.setpin(45);1 A" F' K- T1 d, [" _/ d
- }7 ~0 ^) u6 B! ^3 u$ [
7 K2 Q5 m% n3 B- void loop()
, w/ T7 q3 a0 S( X7 ~ - {: a% n: B: Y0 \; h
- while(!((lightsensor_1.read()) < (10)));6 {; T2 w6 s: z; u! ^6 u2 H5 n/ _
- buzzer.tone(1047, 500);
1 E$ r+ w4 n: m - delay(500);; p- \9 L" j% K0 y+ ^$ w
- do
- K" a/ F9 @0 g! B! ~) U) v$ o/ d% { - {
4 Y" O8 c. ?/ J& Q* ^ - int Err = getErr();
8 C- D+ `# O3 | - if(D1 == 1)
. Q9 @4 i( q& x' }- |: s. B& i% L - {, s; Z' [; G. m* H0 `5 R
- Left = 1;7 c& V& D' i9 U7 {( |' Q
- }
& ^# n% ?! n! m( A - if(D6 == 1)
8 q' w, N. t& ~" G - {
2 h7 f6 V% L3 ~6 ~6 a- {6 s- [ - Left = 0;
. [/ K6 f$ V5 r; g$ T - }4 ^" l- E* _ S5 `# k
- if(Err == 99)
9 h: ]) k/ e4 Q% h1 ` - { F# ^/ d0 o; q
- if(Left == 1)
# D& ?& F; _+ j8 S7 m7 y - {8 q; _ v! q5 n( N
- Speed -= 5;+ X/ l5 q, i8 N* `
- moto(0,Speed);
% O- b6 U& S3 ]5 Q8 D# ~ - do
$ X3 e4 E& O6 k - {" o/ Y; k L5 W! g8 _ N
- Err = getErr();
7 M' S! b8 A0 \- z$ j5 E - }while((D1+D6) == 0);, A- `; F. T" B1 H! n# z
- } e: O! k; T- ?3 H6 x
- else
* p+ a5 t& D5 R! {1 Y4 I; `. V - {
+ w$ n/ V, D3 I/ g - Speed -= 5;
7 c7 @+ q3 z& W - moto(Speed,0);
. Z/ o9 H* W" X" J. B2 x; }! p - do
2 _# I1 p/ o0 U$ J6 V - {1 H: q& E. u% i: O
- Err = getErr();
6 Q1 v/ t8 ^; P% M% k6 }# Y - }while((D1+D6) == 0);& `; Q1 } |3 F! }; V
- }
+ i- w+ j7 o) @. x# w9 @! n0 g - } g$ p/ m" F6 L; d4 F2 S
- else9 v% P5 O! ^7 ]! L, n8 @+ D3 ?& X
- {
4 A+ E w% {. U; |, N& J - if((Speed < 160) & (Err < 2)) Speed+=1; : v" `* Z3 b6 ? j- {; k/ `
- if((Speed > 100) & (Err > 2)) Speed-=2;
8 t5 g0 k0 v5 K$ A" @' S - integral = integral + Err;) s2 k2 x3 q. j0 Y7 |' z
- derivative = Err - previous_error;+ w9 K J2 _ e, d* d
- output = Kp*Err + Ki*integral + Kd*derivative;4 R+ _% I( v Y* t" i; P
- moto(int(Speed-output),int(Speed+output));
/ M+ n( u- y, t2 S3 A1 m. N. y9 x/ M - previous_error = Err;8 t& w! F7 h4 y" [: m$ z- V
- }8 c: y+ K' q" i. u# r
- }while(!((lightsensor_2.read()) < (10)));
. R. H" _6 b& N! V- ?6 \5 Z3 ~8 C - moto(0,0);6 `5 ?, G) K* i8 M% u/ E+ Y
- delay(500);
" ~; G: }$ U: q4 K1 j4 t - buzzer.tone(262, 500);- S. d! E, G0 ?6 L$ E6 L5 G
- }% w4 K, E; I" ^6 c8 U
- ; h8 `* a. k/ ~7 R' `5 j) l
- int getErr()
. j4 I) ?2 O) W6 @/ s* L - {
* A+ W; n- c- j5 Q - sData = linefollower.getValue();, a: l7 o. Q' T8 M+ o
- D1 = ~(sData>>0)&1;
' m4 ]6 N/ ~, J( p9 ]- z4 B7 G - D2 = ~(sData>>1)&1;
. H0 N! q( V1 H& g( r! z. K7 P - D3 = ~(sData>>2)&1;
' M, I6 G8 v- b1 D2 | - D4 = ~(sData>>3)&1;
( F" w/ F7 O, G6 }' T, m; k - D5 = ~(sData>>4)&1;
1 t/ \$ L! {7 E% z' R - D6 = ~(sData>>5)&1;; N8 i1 S& L% V4 m; _/ C2 ]; M
- int downD = D1+D2+D3+D4+D5+D6;2 P* B1 f* V9 i+ P
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
7 r( X% W2 B0 ] - if(downD == 0)/ Q( ~" G: Q5 H3 r @+ r: v3 s
- {6 R. Z! Y" k9 k& X+ B- E" g( M
- return 99;8 {: e1 h( a/ y' u5 d
- }
: s! P9 `; s( @4 w7 w - else
+ A6 Y3 z8 a D: P - {
8 f% g( G* ~% | Y* k0 l6 K) d - return upD/downD;
! h4 `* X4 i/ r, T6 v - }
- f8 @5 a# k, r' I" M - }3 K) S) h3 T6 U. F0 A& Z
) E v" Q" ~+ ^- void moto(int Speed_L,int Speed_R); \% v# _$ |; B& R! A
- {
/ c! t' t7 F3 B Q- ` b - Encoder_2.setMotorPwm(Speed_L);5 I! f" A; {: i7 n6 @# L' J, ?4 a0 l
- Encoder_1.setMotorPwm(-Speed_R);
& h9 }- l7 A5 ]8 U/ v - }
複製代碼 , M: k; I0 V! t0 l
MeLineFollowerArray.cpp5 M4 B& a7 t3 S" }! ~; Q7 s( q6 e
- #include "MeLineFollowerArray.h"
) Y" u3 T5 ^5 K! Q: s8 G
- X, v: T% N, n4 ^5 |+ B- #ifdef ME_PORT_DEFINED
0 V v9 t& n/ z - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
/ J/ F( ^+ x. y9 V( u2 f0 P& i: W, @& c - {4 S* o$ W3 A; d; w2 G
- $ m! s& f& P: m; O3 M, i
- }
4 p# k, ]: T4 M- g+ S - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
) u2 D M7 f J - {
( w% P- c( k* H. ?7 h$ v: i - _DataPin = mePort[port].s2;! C' K/ H) F. w& R& l# m# V
- pinMode(_DataPin, OUTPUT);
* \3 k' n4 g3 h& y, U: k - digitalWrite(_DataPin, HIGH);' z6 s9 U8 q/ g+ b g' R) k9 {1 ]5 g
- }
- ^2 `8 k9 _9 ?" i, h - #else // ME_PORT_DEFINED: V- m% T' _, F$ L3 L1 Z' D6 G0 S
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin). P8 \, A9 M# ^- `& R- J8 A
- {
' R ]0 d1 h. e5 q6 E, k - _DataPin = pin;% Z. Q1 L) Z. s+ }9 f+ v( Y' p
- pinMode(_DataPin, OUTPUT); ) q: p* h4 V( F
- digitalWrite(_DataPin, HIGH);/ t3 f6 }+ D8 P E5 S( G
- }
! F1 z0 ?6 x+ m" y! D( |! M/ N - #endif // ME_PORT_DEFINED. \ E8 u% r4 P% L; g( @
- " t- N! P: H: R8 J
# _5 K" y! I+ P4 }% R" G/ ~7 G- void MeLineFollowerArray::setpin(uint8_t pin)
7 J& G8 F5 T6 I0 @" B4 \) S - {, W; N7 ?+ I- K+ E+ `/ O
- _DataPin = pin;
* }1 T& [: A+ [) r Y& z: q - pinMode(_DataPin, OUTPUT);
- V: R X9 ]- q/ z0 e: r( h7 P" w - digitalWrite(_DataPin, HIGH);5 f1 ?6 L, F1 M3 ?
% g8 Z& b$ m0 z O- #ifdef ME_PORT_DEFINED8 |4 @# P2 h4 n3 d$ J% S/ n
- s2 = pin;/ ?. g5 `/ E6 s1 p ~" n
- #endif1 J2 t( t# O( k" C, e
- }
, D9 S5 p' [* a. [) `) `/ U4 q
. m9 Q9 M$ W( ~; V ~8 @5 Q- uint8_t MeLineFollowerArray::getValue()
! v! [) a, i) }$ G; H/ Q - {
9 ]3 k6 u- u7 o G1 L$ K+ o# _% h - uint32_t LOW_level_read_time;
4 \1 f/ Y2 H, |) y - uint32_t HIGH_level_read_time;
8 f; T* N& r9 M) p0 }% F - uint32_t time_out_flag;
9 A( b( R7 \& o) j9 r' y" a - uint8_t Sensor_Data[3];
/ R4 r8 M! h6 ^1 h% D9 r - static uint8_t old_data = 0xff;4 Z1 Y) {0 {7 J5 `7 m4 g1 S/ |. a: w
- ; b# E& h% b9 F8 p: ]( b8 ]) n
- pinMode(_DataPin, OUTPUT);
& i8 W, D# }- U4 u+ a - digitalWrite(_DataPin, LOW);
4 E5 e- P$ P8 y" f) a+ C& \9 f, ~ - delayMicroseconds(980);
E% \7 ]8 }: P - digitalWrite(_DataPin, HIGH);
0 ^$ w; Y; m% y/ d8 h
z. R4 W& q" t2 d' R/ e% ?* d- pinMode(_DataPin, INPUT_PULLUP);
: U/ i$ X# g o. w4 X' t0 l$ i - delayMicroseconds(10);2 ]8 ]" m: T4 g( W9 s+ l1 A
1 X0 }- z& R4 i* U& Z! [3 O- time_out_flag = millis();
; b2 Y5 R1 N; ]5 m) e9 a6 q - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );3 z2 Y- f3 A, }) Q& R
- 2 `' K6 v; N: G, |6 ^4 x
- LOW_level_read_time = micros();8 R8 b W, y9 r" B
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 a, ^1 C% \! R( S( _
- {
! F9 V: u$ l6 f& X: u1 j9 T - return 0xff;
: {, X0 g; D3 N' j% c- t3 S - }* I9 ^1 Y. C! c( c |
- 9 J, h5 V: [# X% u
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! B5 C: d$ i" {" Z+ J9 q4 p3 o - 8 s7 y7 L, i" ]3 ~& z) H X X! X
- HIGH_level_read_time = micros();" C+ N0 p, z2 o
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level* \! T$ e) Y' D' a
- ! u9 k1 s7 k) V3 a) p
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
, {. F+ ^ V9 Q! n4 k8 s - {
) n. x) I/ J+ s' b6 f1 o# R - return 0xff;
4 p7 b0 A' O( Y o - }( C/ k: i; }9 p7 n4 W
. y8 Z; C) q4 }) G) y: C9 H- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
; q8 q3 q f" h0 c& R$ j* K( ~; z- X - {
% j% J3 c5 Q9 p0 R9 t0 o$ R2 G - return 0xff;
0 X- Q }/ ?* U1 H2 g% }1 \ - }( E! F/ w) R }0 m; A9 q% S
0 I1 \; d: D( P) v/ F" g6 [- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
, ^& G: n. z# p. Z9 ~! } - LOW_level_read_time = micros();
6 s6 x8 T0 D* M- N4 \ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level$ `/ z" }1 X! M2 A
# J/ ~9 c! N& ~' h0 y) U3 G- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 j! P/ L1 |, w5 f. l - {
6 e. u2 w. m+ S" V' U - return 0xff;4 k: B3 g K* e$ }4 y
- }
, k! b# T& m: ?* _ - - C, s7 R: m) a, S! \
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))9 A( N1 h$ D3 m1 l$ p/ _1 V& |! c
- {8 ]" t/ o. I2 M }
- return 0xff; M( m# ^) b) M
- }
* g! x: E+ v7 i1 U5 q8 g, E- L
3 [+ a+ r+ M% S& w- for(uint8_t k=0; k<3; k++)' B$ P1 h" ?6 @& i0 r
- {
B; C% V3 r2 j! p - Sensor_Data[k] = 0x00;+ d& m0 Q4 s. a1 ?) `- K4 x
8 H4 C3 k- H D! S) e! x9 S9 d( g- for(uint8_t i=0;i<8;i++)- `# L5 i3 `' C/ d
- {" p3 o4 @8 e9 P9 l6 Y3 i7 ]
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
! I$ K4 W& t; n+ u4 s - HIGH_level_read_time = micros();
: H, `$ s6 |$ M! |2 P9 q - LOW_level_read_time = micros() - LOW_level_read_time;
, W; Y3 F/ r& `- T3 r) A2 g
3 j2 b! \- K' m \- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )# v0 P4 F {2 M
- {2 F4 U0 i c1 _( I
- return 0xff;
: u, o% C1 l$ t1 P9 [( u/ H - }
( l; I! [0 P7 t$ }: M: j' _$ _- h
. F- E3 q# W( k! O- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: _' H( Q2 y# r* ]" x$ V1 }8 C - LOW_level_read_time = micros();# F# r; t& m X3 h5 A9 @; C; L
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
6 l# A" d2 p# d$ f( R/ _
* g8 w$ c: f' D6 @: z3 p- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
3 y9 D- i' T" i; ~8 l8 I - { V0 `' @! M3 Z( V
- Sensor_Data[k] |= (0x80 >> i);( ~1 v$ B& z8 {! ~2 E
- }
+ a' }. Q/ q3 l6 ~: u" r& ?# {$ Y - else if(HIGH_level_read_time >= 100)
3 S6 I- l& Z( ^" {% } - {
( _& N) z& ^& a: k2 R3 d0 W - return 0xff;
. D/ q h/ O' n4 r - }
: m3 h0 @" J E- s. A8 v - K. h( V ?/ f9 o$ V, Q4 g
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)2 _8 M" j8 R/ B( }9 `8 S
- {0 w' ]8 V! z |& L% H
- return 0xff;
+ I. p; C. }7 y6 M. h" T - }+ K, O& G3 _' i3 y/ J3 }
- }4 [2 C w1 Y; v: C O' A! {+ B
- }
2 L* ]; Q) r, t7 U
# `" `& w" @1 b- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
9 @+ @1 I$ b* C. R/ d - HIGH_level_read_time = micros();
$ `5 ~% c* {5 m5 f - LOW_level_read_time = micros() - LOW_level_read_time;- w, W& v7 t9 L2 i
- ! I j; t2 C, }) Q( e
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )' Z) `5 l( D* x" T7 I7 f' \/ {
- {3 W' J2 i0 _( d( G1 f$ D& j$ n
- return 0xff;5 n. |: f1 m1 ~: V( e n
- }
. k( A7 K. R; W% `7 x
! E$ l* y5 S8 v8 Z" N7 L- pinMode(_DataPin, OUTPUT);
! ^' Y) r7 s% |4 ~1 n& x - digitalWrite(_DataPin, HIGH);
4 _# u" d. v* u! M$ H E - $ {) w& k5 T, E; r* J* _! l
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))8 K0 z! V4 y9 g
- {* P9 a" t2 L1 [& Q
- old_data = Sensor_Data[0];
" b: c) Q. I$ }. Y' J2 m1 C - return Sensor_Data[0];( }1 X9 i# G: n1 h5 g0 P
- }
0 i) z1 K4 W1 u5 U2 x! {$ D( S5 q- U - else
) j9 d! U# u6 w9 j( t7 @/ u - {
/ [3 m- k5 y' T. T. |& L - return old_data; k0 \! n8 |- d7 s8 w0 `
- }
. a! [: D" Q* Y' Q5 X4 P" e - }% x* D% d* I- z
複製代碼
9 K* s& r/ V/ hMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
0 d7 C3 X r1 ?9 R) ?; }
) W7 l. B, I! {5 i3 { |
|