|
|
% ]) P- e9 h3 H3 T' `- #include <Arduino.h>, q& F1 s/ G+ x+ I( [
- #include <MeAuriga.h>* Y3 n2 z" t4 a' ^# `% I1 @
- #include "MeLineFollowerArray.h"
, ^( g% @/ K2 |1 h$ d- g - 5 g: M* z. @# U; D) P1 `
- MeEncoderOnBoard Encoder_1(SLOT1);
: M2 T/ p5 Q6 u# [ - MeEncoderOnBoard Encoder_2(SLOT2);& f( l5 d, g. ~/ ]4 n
- MeLightSensor lightsensor_1(12);2 d6 \( H3 V% A( N0 Q
- MeLightSensor lightsensor_2(11);6 |: j5 }) S% g: F m
- MeBuzzer buzzer;/ ]6 g+ c$ D/ [& w- W# }
- MeLineFollowerArray linefollower(PORT_6);
9 ^' V: I" o1 X, f) ?, ~ - 8 P( M4 H& I& ]% C2 W! B/ a+ n
- #define Error1 1 ~/ S0 U* a% w8 q5 [7 v
- #define Error2 2( B2 H! i v& C
- #define Error3 3
& u( \1 Q/ q5 E
9 o1 T# u8 n: ^: c- #define Kp 15
: H3 y% N) D& S9 K T4 K: l - #define Ki 0.15
, j2 T( w( I" E% O - #define Kd 0.03
$ X4 f+ q' l2 i- u/ b3 w8 {# h - / Q7 k8 S% m% L! \3 w% j# E9 z
- uint8_t sData;
9 N$ ^( _+ o$ c( O) R - uint8_t D1;8 ~* k" f% I. W
- uint8_t D2;# m, W! ]5 E' p) Y' e: j
- uint8_t D3;
% }" M/ l( s! P$ |# g0 } - uint8_t D4;% l$ W( y6 G- i5 X3 j5 O6 V5 v) h& b
- uint8_t D5;# E6 I2 u! s; h6 {6 D, B
- uint8_t D6;" L2 y2 `) w4 N! E$ N
- & H. U/ D1 G A4 j# f- Y1 _, ^
- float previous_error = 0;/ u3 K* \; X; `+ k, Q1 |
- float integral = 0;! [+ [! P) l/ _! ^; _
- float derivative = 0;
8 G/ q& U% c. A' ]8 k/ k - int Speed = 160;
2 m2 ?; A; `- n6 |* d - float output;
2 w( ~9 E/ t7 m5 \8 c! o0 h - # o! m( P0 q3 z4 W7 [
- byte Left;2 ~4 G) s) b$ c5 _$ m, d3 s
+ }6 Z: S- C+ i5 v- void setup()
0 Y* i* a- y# Z3 f - {
& W: n* R+ d T - //Set PWM 8KHz1 O1 `: q; n& w2 V1 S
- TCCR1A = _BV(WGM10);
; |" v$ }! X! D8 H - TCCR1B = _BV(CS11) | _BV(WGM12);
) |4 r: r- b T8 d5 v - TCCR2A = _BV(WGM21) | _BV(WGM20);
9 T1 r& b6 K: \1 l; ~5 Z* o - TCCR2B = _BV(CS21);/ p$ ]. i6 A: h/ u" I) n
- Serial.begin(9600);5 M/ ^( N3 E5 Y3 N9 j
- buzzer.setpin(45);+ Y3 }- [! B7 L) b2 Y
- }
& f- {& ^' |8 S3 h
) ~" r' m7 }# t9 Z7 O8 V* ~- void loop()
( e" M: ~# o- _. `) w' c - {
% j5 E! W5 H/ Q$ T - while(!((lightsensor_1.read()) < (10)));/ U5 m& J7 c, n. R. E
- buzzer.tone(1047, 500);9 q3 V" j. P9 Q, R
- delay(500);
% u' J% w" S3 P/ a$ A' s, Q# [ - do
8 N( U/ H7 N% \: ^/ M - {
$ w0 Z! P) v3 s - int Err = getErr();9 w' Z6 [5 U5 z( m
- if(D1 == 1)3 r/ s1 b/ N1 L8 ?
- {. r4 h6 [ t' a$ y* `1 m- y* C
- Left = 1;
' o+ z5 G! r% @% m - }! M7 m% P, J3 v1 n
- if(D6 == 1)4 _4 g1 x0 [# N( b: [5 r* \3 K9 L
- {" ~4 `: B1 n- F9 y! R
- Left = 0;
: F, j) q' m) N+ u6 ^ - }1 F' M5 F: F& R% h, d
- if(Err == 99)9 o+ t* T1 F: | X3 D( I" K4 k t; \
- {' z! ^/ z7 m; k( \1 d
- if(Left == 1)% _# S+ u" Z+ N
- {
0 \6 }/ H6 U7 c, \& q, M' u- q! P - Speed -= 5;
1 e# R2 r) o" ^$ ?* U - moto(0,Speed);* g( P+ k' \% e' N8 A
- do5 p2 S. n6 U5 C1 r: I1 {7 K5 R2 I
- {" U+ b& _' o( I6 w) G# R
- Err = getErr();
% U, ?2 w1 q; z* @& _2 j - }while((D1+D6) == 0);- k+ Q! a& @2 h6 L' ]6 g( |
- }
" k0 }+ v/ V: h; h# ]0 d - else
$ d9 S- N5 n& g+ _ - {1 ?% M/ {1 ~& T
- Speed -= 5;
' m3 p/ G s) W$ C% [ - moto(Speed,0);
# r- x0 t3 V. b0 B - do
$ J( m, d$ v% u+ I3 H) D+ W; i - {* e0 E6 {5 h& ?. Q: \
- Err = getErr();3 y8 T5 _7 _7 t" m# v
- }while((D1+D6) == 0);5 z' `. d# L w+ }1 a% i: ^/ g
- }: n# Y: }; V6 l! R# p" G
- }
4 f! G* H: Y" D - else4 \! r) R; }: C+ a5 N( A3 }
- {
+ \& C! C$ o7 c& s/ T w8 Y - if((Speed < 160) & (Err < 2)) Speed+=1;
+ ~: [/ }- R7 Q# u' n' D- p - if((Speed > 100) & (Err > 2)) Speed-=2; , v5 }3 g& A) l" |" ?
- integral = integral + Err;
* b2 P5 G1 t, F( u3 p - derivative = Err - previous_error;
; I1 r ~* V! j* g6 I. S5 U - output = Kp*Err + Ki*integral + Kd*derivative;% x- a- F) f: y4 |
- moto(int(Speed-output),int(Speed+output));6 @9 ] Y% i7 S/ w% y
- previous_error = Err;0 ]4 W3 X6 ^5 B$ o. L
- }+ E$ T+ Z, H/ o! b# s1 u2 `
- }while(!((lightsensor_2.read()) < (10)));& c- ~ ~$ j3 m% G% H
- moto(0,0);3 m# n4 f( W" z4 ]; T8 e
- delay(500);$ U$ H7 ^2 j2 X& n& z, G
- buzzer.tone(262, 500);
6 Y) n3 }! m, m, U% J2 E! ~5 n - }
4 F; m( P& X+ t$ _# t# \; P. I
, e' i/ J: {+ N- int getErr()5 P& w; D) s; ^6 X
- {
2 C, S7 Q2 r0 s0 D0 a0 m - sData = linefollower.getValue();
4 W/ ]4 p$ R: g3 [5 Q& } - D1 = ~(sData>>0)&1;
. N5 b x& J! x* P6 o# @ - D2 = ~(sData>>1)&1; j: l" [7 }0 ~+ j+ f n- ?
- D3 = ~(sData>>2)&1;) m5 O# k" `0 ]
- D4 = ~(sData>>3)&1;/ o) T- s7 h: O# R9 T
- D5 = ~(sData>>4)&1;
# {- n- p7 ^: v% c - D6 = ~(sData>>5)&1;* ^) w6 h9 X4 N/ h. {& w) b
- int downD = D1+D2+D3+D4+D5+D6;5 L0 i \ c$ b& F" E/ i: h& q+ F
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);, H7 [6 Y. y b( M* S4 M, I
- if(downD == 0)0 g4 r3 J5 p& n$ @8 R% G1 l3 V
- {
$ C3 B# Z$ R+ O/ [5 W - return 99;
7 d" X4 j: \ e$ M/ h4 X3 E3 i! E @ - }) w O, u4 E5 O3 N) f! F
- else0 s$ E' ^$ ~# f+ j- N. `
- {
2 ]6 ^; ^0 c# X5 n! E/ ] - return upD/downD;
8 k$ Y' w# r' k/ H1 H- E& @ - }
6 z+ E |. @, J @; s) ] - }0 n `/ {) v( D4 ?7 N3 D
! l7 O/ f: A3 R* c" O9 Y- void moto(int Speed_L,int Speed_R)' }$ I) w# Q/ Q6 M2 a3 ?
- {% c& k# Y% e- z! p1 f
- Encoder_2.setMotorPwm(Speed_L);
) h1 u$ _( a5 ?9 O! o( W - Encoder_1.setMotorPwm(-Speed_R);2 R8 \0 f) F9 t+ }
- }
複製代碼 ( n! ] v+ @, e0 M6 N* O
MeLineFollowerArray.cpp
, S& C5 }( U0 Y$ A2 c; p4 s9 L- #include "MeLineFollowerArray.h", x9 ]: K5 k+ K% Z8 u- P
- - Y# y8 ~3 b0 _, h0 [
- #ifdef ME_PORT_DEFINED/ n; ^, o: G' {7 x/ Z7 p
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
+ Q( ^, @$ m, r5 { - {, @1 R/ v, P6 q
+ J9 R" `7 x% l- }
! {! M% R$ ^" o/ A7 e - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
8 {' [1 H: X- R, r' a: t; g# ?; T - {6 e8 G# B. Y) F) N+ ?
- _DataPin = mePort[port].s2;
% a; Y) C+ |' P - pinMode(_DataPin, OUTPUT); & t H9 X9 M1 G% B! l. Y- S! l
- digitalWrite(_DataPin, HIGH);1 ]- v5 ~4 @2 b# F" ^1 Y
- }7 _9 J1 o% \8 K8 `
- #else // ME_PORT_DEFINED: x1 N- k( w7 s4 j+ D+ W# \
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)- h& E. e5 u( x
- {* F @' {$ l) l
- _DataPin = pin;/ q8 o& _2 j0 y/ l
- pinMode(_DataPin, OUTPUT); 3 i- D1 M5 i |5 i# Z* d
- digitalWrite(_DataPin, HIGH);% x' l+ ~+ @+ w. B, f# v$ t
- }
3 m h0 X0 { |- K; _7 x ~' n8 y) Y - #endif // ME_PORT_DEFINED
9 d# H/ A- \7 n+ S# X/ X- S( }9 D5 Q - / k; | ?% Q v+ a5 m* C
- ) e3 I' i& L- g, D/ ~3 A
- void MeLineFollowerArray::setpin(uint8_t pin)$ W8 d* a) w! b1 M9 C- q
- {9 Y' Y+ \& m/ X+ X0 _( H
- _DataPin = pin;6 z2 [) [4 _2 _/ ]2 Y3 |4 r! [9 S
- pinMode(_DataPin, OUTPUT); 8 ^& m7 q' J8 o3 M( y6 t
- digitalWrite(_DataPin, HIGH);
; n& Q+ y1 ` u7 _! ?' k0 \ - $ H5 E' T+ M4 ^$ u0 h. e
- #ifdef ME_PORT_DEFINED5 s! b, D* e* \! A3 Z
- s2 = pin;
, u' d2 x/ T1 q0 t) L - #endif
: g. L p9 m' }- R - }6 ]* P6 d$ z$ P, i: V
* I. I3 L2 Y1 T2 i; G- uint8_t MeLineFollowerArray::getValue()
5 s/ E6 G- H' A+ \ - {4 T( R5 G" F L- c& Z- F
- uint32_t LOW_level_read_time;
* [3 ]4 R+ I0 F/ P$ d; _+ ?' [) _ - uint32_t HIGH_level_read_time;$ b4 F' K* c+ x0 {/ n
- uint32_t time_out_flag;
/ t- `: ?4 u; T3 }7 W7 | - uint8_t Sensor_Data[3];
2 Z/ F. k2 ?; C/ I' Q+ q. Y - static uint8_t old_data = 0xff;
, D8 E% R0 t4 p
* d( U; w; P0 h: D) B- pinMode(_DataPin, OUTPUT);) F% J$ o: j+ I+ t+ ?' p
- digitalWrite(_DataPin, LOW);
$ I. }* x) J0 b - delayMicroseconds(980);
3 z# h9 p) ~+ F - digitalWrite(_DataPin, HIGH);9 |, E3 ?8 Q$ W5 H3 T8 C
- 1 v0 r* @: C/ s8 P( L% Y
- pinMode(_DataPin, INPUT_PULLUP);9 m2 F3 t% E. F) H+ u6 @" v! s* U5 L
- delayMicroseconds(10);+ p0 ~9 t6 N" z: ]$ ?, k# ]3 V7 t
& I W. K& b" Q: v- time_out_flag = millis();' { Z$ \8 u, z }1 r7 Y, s" m w
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );5 [( U# W; O( F- n7 H) G: G
! x; U f8 o6 U% C- LOW_level_read_time = micros();
6 A6 Q" B9 n1 m2 b - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
. S% D1 `; |8 \! L0 n- \8 S - {
: ]2 E6 T% h7 o$ e+ j# i, \0 f - return 0xff;
" v L8 ~0 N, f. ^! k( C* j/ c - }
! F- |& {/ P; w' ]
* P) X9 x& T9 R- Q- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ ]+ E/ P5 K$ A6 c0 \/ L0 o' r! K& U
- 3 h/ P" r% |. Z: o7 ^' j
- HIGH_level_read_time = micros();
7 S4 l/ j' U7 p - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
- R; T2 R# B# f R& z - * U% g1 |1 P9 j4 v0 C% C, h
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out7 H0 h# W( E: W* p8 r6 f( Y
- {
y( u+ _! f8 A# ^ - return 0xff;+ K! p: _# l; u$ e
- }# b0 ?$ n; T: P6 ~
- / M' N! b' H! a; k/ a0 Z0 e
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
8 f1 f7 N) q3 [2 a - {
9 Y2 o$ d9 v/ b6 ] - return 0xff;* K# L/ |) ~8 f( _& a% Q8 ?2 k, I
- }
* d9 [2 }( A( Y - 4 e; Y3 O" V: F) U. E
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 S8 U: z' O7 b4 h0 S" a. @ - LOW_level_read_time = micros();
. z. a2 @) F9 I - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level* |- h. P2 h* Q* M1 v) _5 h
2 F* U3 D3 L: B) v- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out5 Q" `$ M$ j# `( }- D& K* L: e
- {
- b4 n P7 O* S X - return 0xff;
$ I. _) D' c3 S& ? - }
6 z0 n9 [* A. ~5 V - " c% F! K1 u) e0 k' l. ?9 @ ^
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
1 ?; [7 c4 m) w0 f - {3 b4 Q5 ^4 G! i% P, c, |
- return 0xff;
$ @# Z* v/ O1 j- n# j4 P - }
6 v" T! g% `. m3 p4 G) r9 _ - P# H# E J! V2 v* n
- for(uint8_t k=0; k<3; k++)% S- z- v+ ~7 L7 `: i0 W) |
- {- V1 [& `1 z. l6 M. _
- Sensor_Data[k] = 0x00;
+ Y1 G6 ]) i( z+ Y/ W/ P" b - u& C9 m* x: `2 f! Q
- for(uint8_t i=0;i<8;i++)
: e2 M! T6 b+ t9 G - {
3 g& L! ?; M1 J# Z - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
0 b8 y6 F; p3 R" K$ b/ z - HIGH_level_read_time = micros();* \6 O7 A% H) d- D4 I
- LOW_level_read_time = micros() - LOW_level_read_time;
6 ?2 K; H2 `3 s' _
9 U0 ^* y: Y1 K3 n w, K4 M- C- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )- N2 z- Z7 ?% Q
- {
, \5 L) c# m# U) A p1 B - return 0xff;
+ B5 K" o$ G. K" N( S - }. s' t' i% w: p6 b
! U; z1 y3 _. a8 J: Z- I- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ U& c5 F8 F* F! z; k) A' f - LOW_level_read_time = micros();! u# d& R. f* s1 t2 l! k
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level' n: S* n! `/ _; B+ d# Z& L
- 5 g! z7 I$ X5 S2 j- A% ?
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1; F9 F; c; E- N+ \5 }; r4 P. f
- {
8 K6 X3 X c3 G& i- H3 Y/ a - Sensor_Data[k] |= (0x80 >> i);
& q$ K* y$ r0 A - }6 Z) U) g6 c. u( h6 V
- else if(HIGH_level_read_time >= 100)
$ Q) @) j# @8 e - { O( P) a; o y- a$ p8 x$ X
- return 0xff;
# _5 y0 ?$ e- U" n$ l. d( a - }! _$ Y, [9 X" ]! l- a4 K w. z
: s9 k7 G4 P- `0 B; [% h- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)- Q3 I5 r2 A) Z# x" ~1 M
- {
% I- l/ }7 b: ]7 [ Z - return 0xff;
9 J! I0 M$ o1 l F6 F - }& ~: Q9 P. l) F$ U1 }. Q
- }
! D+ t8 J5 p; y. L( | - }
3 S" l" A6 I8 k5 P+ s& \- s! D& { - ! I1 ?. ~; t9 f0 c1 i% `) Y
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level4 Y- b2 d' i8 \4 N1 f2 a, i
- HIGH_level_read_time = micros();
! k! T2 Y# k7 W$ X( l1 i$ F - LOW_level_read_time = micros() - LOW_level_read_time;/ x; i$ x6 f& o x1 F5 y- S" h; S
- 5 ^. z/ w" {" }% c* D9 G, r; C
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )! L, q' S) `: L6 m! H6 l
- {
$ H; k+ z/ p2 ^ - return 0xff;6 u/ G; y" j0 p: Z# D4 S
- }5 |: {, ~/ b" W$ |
- a3 L! A2 } o& a) f; k) _2 v
- pinMode(_DataPin, OUTPUT);
D! Q, Z# P. R( {8 e* b# i - digitalWrite(_DataPin, HIGH);4 M4 O# M% T9 J9 U8 i
: C0 v( t$ `9 S7 L4 O- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))& V9 s+ x1 d/ r
- {+ e e- l, n6 m6 e% p+ r
- old_data = Sensor_Data[0];: v8 Z4 l1 D0 F i6 R. n
- return Sensor_Data[0];" ^" g# M8 x2 s, N0 |( ?( A
- }- \& N/ A! C! \* S9 l% f
- else; l3 |+ B# y; ^, l
- {$ x/ x! }1 |% m
- return old_data;
- N# }& @- Q3 K: h" E% [4 c3 T( d - }( W" E, B" ^2 w+ ]& c a; A- m
- }1 z* o: G8 ^' G$ [% Q
複製代碼
1 U9 o( l5 K# S6 X1 F1 SMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
8 k3 k' {+ g% L
3 A, h; a- W, H4 O& k4 R
|
|