|
|
+ [0 K. n7 C, q2 \8 M- #include <Arduino.h>
" ]0 r E1 s0 A, m( X" q3 H, r4 l+ W - #include <MeAuriga.h>8 U1 p9 x' X( G, s9 Q+ J; e/ x0 R6 J2 I
- #include "MeLineFollowerArray.h"4 c/ V7 c% z9 Y! X& o/ G
' C& o; u- C- [1 U! K' N- MeEncoderOnBoard Encoder_1(SLOT1);: e: z2 v) t# s
- MeEncoderOnBoard Encoder_2(SLOT2); L; l( T' l0 w1 W
- MeLightSensor lightsensor_1(12);
( J: R6 i( F) \* N2 }0 d* V- S - MeLightSensor lightsensor_2(11);3 |8 e0 l& O1 h i6 K# e% }+ x, ?- f( L. V
- MeBuzzer buzzer;
. m3 h' c" C; Q; o# c - MeLineFollowerArray linefollower(PORT_6);4 `+ @( [ L0 P7 ~
- / f0 c. f0 i2 |9 W
- #define Error1 1' r- o; a4 `. e* r/ T0 j, y
- #define Error2 21 B9 n8 X$ o0 g
- #define Error3 3) s" e4 i% Y/ A
& V) e; H$ J. O: m- #define Kp 15. u- @& J+ R1 R3 A
- #define Ki 0.155 A) U3 O: F% [8 k% h* U5 [6 w
- #define Kd 0.03
1 b5 W( a2 f& @0 A - , {1 j& L( y( z! f1 u/ a
- uint8_t sData;
& t3 l ~# Y- K0 H8 Q" D1 V - uint8_t D1;! X( g0 x u( \3 c
- uint8_t D2;
, v6 g& _! C0 U( s x - uint8_t D3;
+ J, M; i! \0 O) l: `( b' B* F - uint8_t D4;+ E- z8 I1 V! }+ o
- uint8_t D5;
4 z4 Q4 r% O" s% ~ - uint8_t D6;
- g o5 Z/ T! G8 u3 Z- Q$ K' e
% \1 o3 R% L, d6 y- Q" l1 |# X- float previous_error = 0;: ~2 j, X. F, c9 b6 E
- float integral = 0;
" n- J( i' i6 H6 n, }$ y - float derivative = 0;
- i$ m4 q3 Y8 @. _ - int Speed = 160;, x$ ?, e9 }6 i! [7 E8 h
- float output;
3 Z2 U* t$ |7 u4 K. y - 3 J- _6 h) l6 v5 f' ^% Z
- byte Left;
W1 n. ]8 K/ b! [6 N - ' Y7 O5 g3 H3 n* t2 w- S
- void setup() & n$ i d0 a& E( @8 _
- {6 i7 G! Z4 T6 h$ `8 V8 @% ?% ]' t) f' e
- //Set PWM 8KHz
~0 S& l! y# F y" f - TCCR1A = _BV(WGM10);9 x: F; D `9 p4 k! \& x. f* A
- TCCR1B = _BV(CS11) | _BV(WGM12);
+ J3 E% @1 G3 W1 l - TCCR2A = _BV(WGM21) | _BV(WGM20);
: b" `- Y/ s" Z( J4 C - TCCR2B = _BV(CS21);
" x- E' C$ n8 m5 ? - Serial.begin(9600);
" K o1 v5 p' H2 f1 c - buzzer.setpin(45);
, O7 B* k( V. W4 P) }2 l) C' w' j( { - }
/ Y8 k4 Z) D* o+ h- @ - - R# \/ x1 h& `( w2 w5 v% P
- void loop(); C/ L' m+ [7 p2 Q2 G: M, s9 b
- {1 R3 b6 w3 w$ {; Q# s' T
- while(!((lightsensor_1.read()) < (10)));8 ^# r: w$ R W
- buzzer.tone(1047, 500);, V. F; L8 u0 y
- delay(500);
5 K3 ]2 k) G9 f/ t, O - do
" S. d6 l8 Y0 [$ L8 @6 c - {
' f" k6 S4 T& g8 | S - int Err = getErr();! t; p# C' Z/ H
- if(D1 == 1)
X5 o( k( C* O* ]. i; d! u( F - {3 L; i! _; {3 T3 \9 I/ u# [ T% g
- Left = 1;
3 l" t3 w3 N2 e0 u! y- H - }
) u4 p9 P" m3 t" h - if(D6 == 1)
9 h1 g: {% C3 N - {
0 ~& K6 p0 R4 `- \3 U7 F) G - Left = 0;
' M3 {4 f2 E' N - }
. u+ z. d* H6 [, ] - if(Err == 99)
- v& h: e, J; e, z# f% V - {
; x: k$ i- O4 o - if(Left == 1)
7 x" u6 u+ Z# Q, } - {/ M# m+ P r' w2 ]
- Speed -= 5;
8 s+ j1 `+ e1 n; K - moto(0,Speed);
$ ^% v# s! A' v& j: v/ _ - do
J. h' Y' n: q0 B - {
! Z& _: X( y$ j2 a ]) h5 I9 C- w - Err = getErr();- P7 k! g5 g. I3 f$ B
- }while((D1+D6) == 0);1 b% p2 w( T) s7 Y
- }
& H- A. F3 U u5 Y - else
5 G6 F( ]& i0 ^, L/ A; b - {( \) [4 }1 ?1 h# w9 L' ?
- Speed -= 5;
, P& j# y8 [/ j4 n6 m; t - moto(Speed,0); . u7 F6 i" M ~" h5 t
- do4 ]9 g4 b# ?( ?
- {4 [) i! k8 q4 B& X# \! k
- Err = getErr();) W$ X3 t1 g5 C) J
- }while((D1+D6) == 0);
6 ~. E# r" S3 }" c; `. n - }$ u% S4 ?3 @! n3 I1 e$ M/ O
- }
+ a/ p E8 B/ {" _( e - else
$ {, |3 r5 s5 I) t/ \6 G - {
# K5 P7 V3 l: T* R - if((Speed < 160) & (Err < 2)) Speed+=1;
" e* ^# J0 G# Z( R - if((Speed > 100) & (Err > 2)) Speed-=2;
/ A+ w% p# @. U, J% \, t a - integral = integral + Err;
! b* c* P) o* C( z. h% ` - derivative = Err - previous_error;& f9 _+ k9 X$ U1 u
- output = Kp*Err + Ki*integral + Kd*derivative;
3 m; ?- }+ f6 f% j - moto(int(Speed-output),int(Speed+output));* d* V! q/ f7 g
- previous_error = Err;$ S& k! L8 j7 [( G
- }
1 F) Q% H" G0 l0 N* C k! E - }while(!((lightsensor_2.read()) < (10)));
2 g9 ]; U% @' D6 w - moto(0,0);
, T2 u& `; t, H" [2 A9 i - delay(500);
6 T9 E; E' i1 E% L' d7 G - buzzer.tone(262, 500);
7 \5 I6 V) `3 J6 R( D - }
6 J; j8 Q% M R/ q0 |$ J$ p$ X
# d- M0 P! p4 w/ S% T! e9 x) Q- int getErr()
0 v3 l# u+ a" B9 o! O1 ~. E - { 6 s# u9 ^* |' \0 _. {% S. g; J& U
- sData = linefollower.getValue();
# z' ~& R+ ]7 ?! O' q( N [ - D1 = ~(sData>>0)&1;
1 Z- Z1 ~. ^) T1 n; \: P, o8 T - D2 = ~(sData>>1)&1;
w/ p. Q+ E, Q, o - D3 = ~(sData>>2)&1;6 t2 f$ Z& I, T8 Y; l
- D4 = ~(sData>>3)&1;1 ~8 p! L4 z S/ [
- D5 = ~(sData>>4)&1;# \% u. Z m- Q0 N, J( Q
- D6 = ~(sData>>5)&1;5 a/ a& D- y8 `$ ]+ u
- int downD = D1+D2+D3+D4+D5+D6;
. @- d# K( k/ H( `; l B - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
: c$ |, n. L' p$ o6 } - if(downD == 0)
- \3 F* v/ p$ w8 t) q. ~ - {; r+ r: m$ r7 f+ V$ w7 |' c
- return 99;+ [: e4 ~1 w1 i1 E: g# q6 K' K; u
- }) o( A. G. K' [; P( m4 o
- else
- C2 |4 g* `$ V# L0 A7 y( \ - {& R: m9 I* A" P) R0 d& v |
- return upD/downD;& u5 @! G4 w8 l4 Z$ e! N
- }
4 s8 V) E( W8 p: p# c - }
$ j. n' W$ g: ]& C3 N ?
& M6 Y* n/ O- d( a( n( L+ _- void moto(int Speed_L,int Speed_R) X( z, r; Z0 P! v1 k
- {/ G0 @; C( k) I' {4 K
- Encoder_2.setMotorPwm(Speed_L);
: }8 w- Y$ n- f* Q - Encoder_1.setMotorPwm(-Speed_R);# k2 ]) R- j( a8 j- W: {
- }
複製代碼 ( R9 {" R7 ~) E3 E) F6 U
MeLineFollowerArray.cpp
) n" S% X6 E6 X& f& ?7 X- #include "MeLineFollowerArray.h"
# a( V) h/ ~* Y
" q+ \0 J, W3 Q( E- #ifdef ME_PORT_DEFINED
6 ^. k0 ^5 W+ v& A - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
9 w/ m% l3 ~) D - {
# A5 G: M, X5 E5 U, t' n3 U
- g/ x. q* l) |! c: B. v) c- }$ f& ~3 S2 _( V9 V
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
: [3 I( T: L: K G# _2 e" m! | - {
4 l! c( |' g5 e% n% G; |7 M9 M v - _DataPin = mePort[port].s2;0 X" U7 R! j- ?! e
- pinMode(_DataPin, OUTPUT); $ n, {! U5 r9 T9 s8 ~; e
- digitalWrite(_DataPin, HIGH);
8 S$ Q4 J6 X% Y! A" D& q - }
) Q7 j/ y; K9 S5 @1 z, i6 v - #else // ME_PORT_DEFINED- ~" d$ a' f% n3 `4 {2 ~+ P
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)- U1 ^; k+ S, D) P9 t# c! |
- {8 p8 i. u4 t1 f$ ?- R
- _DataPin = pin;
4 T! Q& h$ O+ D0 v - pinMode(_DataPin, OUTPUT);
# O9 t. y f4 |7 h0 ` - digitalWrite(_DataPin, HIGH);; v# c% U% Y& f) B/ K
- }4 f8 a) ^0 R0 i6 p2 ~
- #endif // ME_PORT_DEFINED0 K) N7 G3 S; x+ G" q% n e
8 e. C! K" n5 ]/ P; P& |- ) m5 ~7 d2 g8 ~$ i
- void MeLineFollowerArray::setpin(uint8_t pin)* ^1 I( c/ I' G7 o: P4 Z6 H3 D
- {
. V! y. U0 g# b& R0 O - _DataPin = pin;2 p2 s* T k6 ^3 N# I
- pinMode(_DataPin, OUTPUT);
& p- f- N' m% e - digitalWrite(_DataPin, HIGH);
/ F- Z* w/ z7 z6 u W5 P - ' _0 Q. n, e8 i2 J/ C6 I$ C
- #ifdef ME_PORT_DEFINED
" N; ^0 V1 |6 t$ x - s2 = pin;
/ l. T/ d/ K/ P$ m6 H - #endif
$ T+ W6 U8 O: |. l - } S3 h x3 g" }8 E# M9 s7 t
- Q' r5 Y4 s- V1 [- uint8_t MeLineFollowerArray::getValue()# a8 B3 {% Z8 R9 e3 e! M
- {
# d3 m" H- ?( Q) k y1 f - uint32_t LOW_level_read_time;% l, Z; D0 n0 F: q5 h% B
- uint32_t HIGH_level_read_time;4 R: o# O v* N- U2 h, I1 T3 S
- uint32_t time_out_flag;6 }4 x# W S3 P; b1 b# Z, I
- uint8_t Sensor_Data[3];
8 n' n' y5 F ?; C# v1 p6 }% F - static uint8_t old_data = 0xff; _* \8 R: K j7 C
- ' ^" C; ?6 ~, Y3 R. |. P9 a4 \9 w4 [
- pinMode(_DataPin, OUTPUT);
: `) p* e7 j1 c3 _* j - digitalWrite(_DataPin, LOW);
: T( d" Y5 U" Q( @' F - delayMicroseconds(980);- {4 P S, d u- m! x* I* Y3 ?' X
- digitalWrite(_DataPin, HIGH);# J& }( O- g% } D4 L
- 1 D! h$ c* ]8 Y+ l
- pinMode(_DataPin, INPUT_PULLUP);$ _4 {5 [2 ]/ T9 A6 }
- delayMicroseconds(10);
6 U4 `) \ m3 b) v - 7 d3 A- `9 U4 u5 Q" ]# s/ U
- time_out_flag = millis();
) @! v+ p/ @: p2 [8 j( c - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 g& ]( A2 G* N! c$ D
- ]2 y0 R6 A- s" o& V& X0 I$ s- LOW_level_read_time = micros();3 _& w; u' k5 F) r$ ?5 ?$ x' L: H% n) A
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
5 g. s' k( @' Y$ @3 H6 X - {
5 A5 u u, n! ?! s c - return 0xff;) _- @! \ d; g: {3 F4 ~1 e' X
- }8 Q- |3 \8 ^/ Q. v) K3 N5 ~
# D- @: w# A" _' s w. w- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );4 h8 S; L- F) G5 k
6 q) t+ [; Y! T8 G6 ?; q3 B6 W6 {' r- HIGH_level_read_time = micros();
( `7 M2 {6 s( Q+ t' [ - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level1 ] R7 K" K6 y9 M
- ( `% k: T9 R; p' S2 \
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out8 p5 w8 _0 \4 Z+ D
- {
& q! A# G% R4 q! V$ n& N - return 0xff;" M4 c( i' Y! s2 b
- }" ?* |; b$ R# S
- * {( `) I( A$ `5 [7 F: z) \
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
/ B$ b$ u/ Q* y6 X3 f# b- l - {3 C4 W1 {: m1 g: ]2 D/ J
- return 0xff;+ }, T( [& {) d- V9 C% j
- }* N/ I. X1 t( m2 `9 i8 J8 C4 z
% g5 s2 k% [3 x1 f+ F3 ~ U( G2 k- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );/ n$ O5 O4 m2 v- I4 d. ?2 L8 r6 C
- LOW_level_read_time = micros();, L1 |5 a% X% U
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
$ [; `$ L% k" S& m4 E: M
6 \# }( W% l9 p F" H- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ W0 N, A' B9 U0 N) Y" l7 F
- {- g' Q" X7 ]! q9 S
- return 0xff;
1 Q2 f8 T# n# b; G" a* L- U - }/ O; y$ B6 L8 b
- : X1 j3 c8 s5 G
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
8 p/ Y5 `2 h8 }! V9 L! v - {4 H5 C% v$ C+ i6 K2 S$ ~+ Q
- return 0xff;$ T$ U9 @! [5 u' j: h# ?' u
- }
( R3 k. E* w6 F; R - 8 O) F3 h1 n1 i D# y4 |; g2 O6 s% @
- for(uint8_t k=0; k<3; k++)
, Q! L# \# N9 H2 d; J! n( q - {
4 G: |9 l6 f3 s - Sensor_Data[k] = 0x00;
) [ L! ^, m' q4 ^7 _4 Q$ l) z2 N$ A
& D9 K. [/ @$ \+ P2 l! J0 @- for(uint8_t i=0;i<8;i++)
r$ m3 c( ?2 k4 \) c& v m; o7 z - {
7 U' x: V- K3 v0 k R - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
( M3 R1 g0 K+ }5 `' s6 F - HIGH_level_read_time = micros();
6 a& D. C7 N# \! J' G; S) J, l - LOW_level_read_time = micros() - LOW_level_read_time;
6 |$ }5 G$ z) ]$ T4 Y$ @$ Y - 8 q' P% Q7 m% t! T+ {
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )! |' |- o2 C: @& w' \! r
- {: a* d0 e8 Q* G& j! a; u( f" F
- return 0xff;3 g/ ~; T3 X6 z d2 p
- }
& O7 v ?1 y$ F
" x3 D/ S v( x4 R, z" V) H- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
' J1 W! Q$ y+ S \ - LOW_level_read_time = micros();
5 K2 j$ R1 J* O$ h - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
9 `; m0 A! }& N4 |$ E - / V1 o1 I# Q2 E* `) C
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 15 q; _# ^/ j& m N3 m1 h
- {0 O6 @! ?- r s! f2 a, \
- Sensor_Data[k] |= (0x80 >> i);
1 E9 m7 x7 x: V3 w - }
. {5 I1 `" {. i6 b - else if(HIGH_level_read_time >= 100) A6 U ~- P6 c4 _' R/ E$ o$ a4 a
- {
# ^7 |( z K" G, v. P - return 0xff;
+ R5 c( H" f+ G! c/ \5 q" m - }# C, @6 d$ H# L. S; u8 S4 U* D
- ; _& \! m9 A2 w
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
% b- p( R5 c3 j; T - {
+ H5 A9 y& g4 L) H: L8 \$ k - return 0xff;
% U# V! Q: ^. w" X& ?! X0 w' N - }" S. N$ ?8 M3 j" T d
- }) O$ B2 C- ?) R4 c% D5 U
- }
: j- A! I+ M: w& ^3 I$ S% m
. ^" G' E# g9 v% z- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
! S% C o# b$ @# e1 ~ - HIGH_level_read_time = micros();
[5 t8 l" z3 f - LOW_level_read_time = micros() - LOW_level_read_time;' [9 N9 {( {6 ?: ?
- 9 R9 [/ } c- _4 d3 Z; W7 B7 M
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
$ J! F/ j4 V9 \% J! @, K0 z - {
- I* B, }! A7 u+ x - return 0xff;1 Z0 g% Y/ f- H# ~# O2 Z* r7 Y
- }% b# t& d- H* Z' q, A) u
- 7 n( H0 A- v3 {& o4 k) Q
- pinMode(_DataPin, OUTPUT);
$ |* P) |( n8 g - digitalWrite(_DataPin, HIGH);
. |( b4 R( K) p) } - # k0 c5 L! F. j/ ]/ J0 u
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))5 p4 U7 W+ `2 L8 Y0 h7 T8 h! u5 E8 H8 B
- {
$ S; A) X' h. u( L: V+ W7 X: v - old_data = Sensor_Data[0];
) d; [1 h9 n9 H - return Sensor_Data[0];
% `- {9 A( ~. B# S( c - }; p8 C' m- C. Z$ e/ E: w
- else
' _6 G. O+ n, O$ H( G - {& ^; F6 g8 j$ I% M
- return old_data;: g2 s# e' i0 n6 L% ~0 S! `0 g
- }0 ?! w" q8 V/ c I* u
- }
! ~+ }9 v9 ?4 u' D4 K5 b, q# y1 @
複製代碼
: H! `* @- ]. K; Z+ _) bMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
1 U' O; \2 G/ w i9 j3 s U- }
4 L! O5 C; G$ I. k; ? |
|