|
|
5 Y5 k: H$ O; i0 ]- #include <Arduino.h>
. j$ D5 v& ^; w$ Z" `( Y' L - #include <MeAuriga.h>
$ o# E) b+ }7 \, L- @ - #include "MeLineFollowerArray.h"
5 u2 q3 K2 w$ w! J a- z9 C - 9 j- a1 ]4 L* `+ X9 f2 Q; }
- MeEncoderOnBoard Encoder_1(SLOT1);1 m H3 v3 l& Y2 M
- MeEncoderOnBoard Encoder_2(SLOT2);
. q$ n: n T. E5 V - MeLightSensor lightsensor_1(12);7 g; y8 a8 h; B6 M8 a
- MeLightSensor lightsensor_2(11);( O8 v2 r8 I6 n2 p. i
- MeBuzzer buzzer;, W( w- J) M" _ U5 N7 I
- MeLineFollowerArray linefollower(PORT_6);
5 R. V3 c9 _ |; ~+ v* R
2 n6 Y0 ^/ Z* ^8 R+ {. e6 h; i- O- #define Error1 1& I: d) U' z" a) M7 S, f/ J
- #define Error2 2
: U1 S" ?. s6 [+ I - #define Error3 3
. C; l: W( h) n U
4 E0 p' g) _( B" v- #define Kp 15
! j% w* K3 J q' b/ L - #define Ki 0.15
8 M9 s# O. k4 X( S4 b$ f2 g - #define Kd 0.03( K% m% `& \0 t" y8 F
- * v# A3 ]) ~. Q$ ]
- uint8_t sData;. Z8 A9 V2 t1 V( q, Y2 q' y- L
- uint8_t D1;
- T% \* J/ X6 f W1 d {# Q' R: f - uint8_t D2;; R8 n/ J9 H6 f# b1 R) w7 {: @
- uint8_t D3;. r* @" K" @% N/ H8 [6 f0 y
- uint8_t D4;
. m* a# V9 y# u+ a% z5 W - uint8_t D5;5 M. l' Y1 O: F' @; S, I9 R
- uint8_t D6;
) S3 U) q% K9 G" Q5 }
6 X0 M! I( U+ ~ Q3 H1 l2 Z- float previous_error = 0;1 s! c* Q; U, l8 u/ V- l$ y9 \
- float integral = 0;9 [" S) h' a" a5 p9 u8 m
- float derivative = 0;
7 h; B0 ]6 F# U* b; d8 i - int Speed = 160;
5 o1 `5 I. b3 u# Z - float output;
& N7 l8 T8 \; @2 g) ^7 R6 P
3 Y4 }# a) l# [* \, |- byte Left;4 x; T3 {6 e9 Q
- 7 p& p% s6 N) [4 R, {: y* h
- void setup()
% Q) M W1 u5 q* y) e - {* h* u5 ?+ W6 k" p7 p- Q- A
- //Set PWM 8KHz
" L3 Z/ z1 o; [ - TCCR1A = _BV(WGM10);
! l, C0 y* c+ Z* ` - TCCR1B = _BV(CS11) | _BV(WGM12);
6 u1 ^1 w3 _3 M - TCCR2A = _BV(WGM21) | _BV(WGM20);
! P' n! i- s9 q9 g3 N Y - TCCR2B = _BV(CS21);
( K6 f: p$ e8 \$ k2 C - Serial.begin(9600);
" W9 B4 @8 z: |8 c - buzzer.setpin(45);
2 S2 H y, @+ g `2 p# v - }* Q) x* }# v4 m# o* q$ z) y
# Q% _, N& x, e( U) w Q# K- void loop()
e; o5 I3 B9 K7 X - {
* n/ ]" B" C/ t. q) D - while(!((lightsensor_1.read()) < (10)));
, K K; i$ T$ e+ a+ c6 w! i! t - buzzer.tone(1047, 500);
( R! a ]3 u; h* v - delay(500);, H- i4 l" C0 Q; J3 q
- do5 g9 d" H2 w& s% E9 @
- {
% D; ?! A8 \5 _4 l - int Err = getErr();& B9 i9 X1 `' ]7 i6 S4 t4 b
- if(D1 == 1)
6 Z1 V& Y9 d, F - {$ Y) @9 `) a& o: K5 f' c
- Left = 1;
1 Q6 B' \: o) m- L' t - }
5 S5 }4 l6 u5 n - if(D6 == 1)
/ s# m& k+ H% W& Y9 s - {, T; ~. G5 \1 \
- Left = 0;4 D' c$ E. d- x# [
- }& u8 o& K( x- O" ^
- if(Err == 99)
8 T: E% G9 v7 t. x6 S! ^! R! f9 G8 } - {) J" p! |& N7 \( U
- if(Left == 1)
5 y, r- y8 N7 B1 C1 N5 ? - {
: Y# |/ m# h( B* r7 k$ L& W - Speed -= 5;
( f2 ] a I5 Z" x& J: ? - moto(0,Speed);
6 P* W9 E+ K3 _& b; h - do& v4 B$ e! Q: ~) Y u) \3 u! n& e
- {. H1 U4 h" V! S6 N/ U5 L
- Err = getErr();! G! H8 i0 s; W( M) d$ }$ l
- }while((D1+D6) == 0);
g, S7 [8 f3 Z* @5 b9 ` - }! Q5 ~$ f! X3 x; A0 E
- else
. r/ b m/ R$ j3 T1 q/ J, A9 o - {
4 U3 G/ L7 g4 M9 | - Speed -= 5;& c) Z. Z [4 I u3 b( M4 G
- moto(Speed,0); 3 ]3 ?% W/ Q* f& y+ W
- do
) A/ l) b0 Q/ C% z$ N7 x& L4 R - {2 l9 U: R# R1 u7 F; Y
- Err = getErr();
3 w& B2 a2 Z& j; W - }while((D1+D6) == 0);" ]4 Y( x; Q! P7 T
- }
- x+ t) I5 ? G+ y - }2 y+ l1 X" j* Y$ W0 F1 I
- else
. j/ l6 p4 j: n8 |) [' j - {
( R5 P8 c$ O6 g* u - if((Speed < 160) & (Err < 2)) Speed+=1;
) u: y6 T6 G4 d0 x! M8 a - if((Speed > 100) & (Err > 2)) Speed-=2;
* ~1 R; i1 C0 }8 r3 C% t - integral = integral + Err;
) U6 c6 d# u2 v# z% E, ~5 H - derivative = Err - previous_error;1 ~! `/ S1 y7 v0 D4 r3 f+ A
- output = Kp*Err + Ki*integral + Kd*derivative;
6 E' {( P( H, r - moto(int(Speed-output),int(Speed+output));
# \2 q: Z. M- ]& I% ^ - previous_error = Err;
* c. M. _9 I7 ]4 E4 D& r! q& R - }! U5 E% {8 c+ E* Y1 H, @
- }while(!((lightsensor_2.read()) < (10)));: X2 z T! h, J2 T5 x; b
- moto(0,0);4 T/ d# N9 W5 D
- delay(500);# g% U" U* Q3 A( i' R1 x; z+ k2 ^# ~" w
- buzzer.tone(262, 500);
$ }+ X! S5 Q0 @8 O x* q# J - }' L& \% G8 n$ v' h. s
- 5 o8 v7 J) i; @1 G3 j4 Q$ L
- int getErr()1 O1 u4 |" ?3 K! P3 S1 W/ E% A! J
- {
& x, R: M5 T$ W$ ^ - sData = linefollower.getValue();
! X: N, ?/ `: r# V9 } - D1 = ~(sData>>0)&1;6 p6 L+ F" r: V, n [
- D2 = ~(sData>>1)&1;
/ U! j' m' d2 Z- e. M/ X; { - D3 = ~(sData>>2)&1;# z$ R$ f. x7 K' K# |
- D4 = ~(sData>>3)&1;% Y1 ~5 h6 m+ Z+ \7 b; m$ u1 C3 W; L) N
- D5 = ~(sData>>4)&1;0 S2 U. {' G: X+ H' b! Z: W) V! W
- D6 = ~(sData>>5)&1;
3 @& L% j, [) W- u, `! s2 b$ P - int downD = D1+D2+D3+D4+D5+D6;
6 X) h [( Z+ M4 k" w& x - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);# k: L3 B' u5 [4 w
- if(downD == 0)( Q- ?$ G, P6 G# t# y
- {# s _+ |6 V+ s8 }" t7 |
- return 99;
& y. u0 E+ ~6 \* k s* P9 Z - }
; P3 V& p4 |# |1 D0 g2 w - else: J9 n3 G6 y0 U) C6 v. i7 S$ j
- {
: J" Q' I, l& a* f8 h - return upD/downD;
. [) \8 r$ N4 p) _( o) b7 G - }+ C. j% J b7 A
- }
/ D ~) ~8 s% h0 ]' W* J9 A3 S0 k - + ?, b( g7 r, g! P% |% h
- void moto(int Speed_L,int Speed_R)
" T' d1 s- U2 J/ e* c( @4 Y, g - {
, U0 _- L4 A0 I! a/ o$ N# u - Encoder_2.setMotorPwm(Speed_L);
f5 X* T8 u& s" |1 A0 e - Encoder_1.setMotorPwm(-Speed_R);
$ o. W, C; c* b7 ~ r: p - }
複製代碼
, Q4 H+ Y1 w, m7 ] VMeLineFollowerArray.cpp
5 B9 s+ [2 F+ g! B0 a" {- #include "MeLineFollowerArray.h"
! E3 X& Z5 r5 K* k6 I6 L - & W7 u# C$ i! w8 H: U. n
- #ifdef ME_PORT_DEFINED; q: u% C# V" d
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)* t# T: B/ t- e! V( }: r
- {- U' @! l) o8 X1 n$ y
( ^1 z) i" M" |+ o5 t, @" x& h- }9 N m9 a- J" P2 _% S( |4 F3 f' _
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)" @% t% @# [; ^# j
- {/ f/ o$ d; B+ S0 ?: ~
- _DataPin = mePort[port].s2;: Q4 A9 P* u8 `' ] @* e& g
- pinMode(_DataPin, OUTPUT); 0 W+ A! h3 I3 z6 b
- digitalWrite(_DataPin, HIGH);
- b4 b" ]2 a% g! E$ Q - }
3 T0 {# Y1 o& s$ D5 t - #else // ME_PORT_DEFINED
/ k) _/ g+ U3 T+ q9 y& j - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
$ C* Z6 r8 \' l# j; m' i - {) y) }$ m8 Q: v: I3 q
- _DataPin = pin;
2 Z, d# w7 U! ]- W& Z - pinMode(_DataPin, OUTPUT); " [9 X* q5 o- ]' Z
- digitalWrite(_DataPin, HIGH);
+ x+ T5 p! o0 q4 c( {# }, l7 ` - }8 ? V3 n, P% G: |- V) \0 R' ~
- #endif // ME_PORT_DEFINED+ w. x3 S; i: c# w9 ?
( z. h# ?) @- d: `! S0 N- & c& |" z% D! P! z" ] f* o
- void MeLineFollowerArray::setpin(uint8_t pin)
' w8 N0 W' l3 Y% I' J - {2 a1 g. q$ `& {7 I& a
- _DataPin = pin;
! L+ v; N* F- `! M! M- A- }9 b - pinMode(_DataPin, OUTPUT);
" i/ _6 q% e: l$ o8 ^; ` - digitalWrite(_DataPin, HIGH);
. o' E! ^! I( ]7 z, u* N2 O1 i' U - 2 V% Z% Y/ A( ^# F. ]$ O
- #ifdef ME_PORT_DEFINED
; x* T8 p' u( m% l4 p - s2 = pin;
P, b3 Z# t6 U' b- l; T; ~ - #endif
* _5 L5 G' `( b1 T - }* _) L2 r3 w# `
- 9 E( b" k" @* u! p" _, h
- uint8_t MeLineFollowerArray::getValue(); T" a" d5 J$ h7 i& E
- {% Y: L8 l7 y7 Y, o
- uint32_t LOW_level_read_time;) e) ~( f. k7 Y5 _
- uint32_t HIGH_level_read_time;& g1 @( N1 o, l9 ^
- uint32_t time_out_flag;3 |1 }# u7 V3 @2 O) {
- uint8_t Sensor_Data[3];' [6 S6 Z5 f6 a# r6 U. d! ?: j
- static uint8_t old_data = 0xff;, l+ Z: w* T3 f( v6 b# s2 D8 j, H
6 _' B6 s9 L8 I' i, O! M2 B l! A- pinMode(_DataPin, OUTPUT);
0 E; d, a% _; X6 }8 Y - digitalWrite(_DataPin, LOW);# }9 P* C. u8 i) a; y
- delayMicroseconds(980);
# O; h% W9 e8 E% {# {- E/ Q - digitalWrite(_DataPin, HIGH);
, l% f% a* x; u! n" y+ ~. D
5 \ w( e! O& \9 N( k# s- pinMode(_DataPin, INPUT_PULLUP);
% \* J' r8 v2 B2 S. v/ [, B- ~3 x - delayMicroseconds(10);3 I- ^$ a* b- W- S/ f; l9 g
- 7 @. b9 I ?& D
- time_out_flag = millis();
8 f5 b% X7 @8 Q$ o$ I" j( Q: U - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
) B; L$ A. S$ p - " b- Q+ c+ x) X
- LOW_level_read_time = micros();- b4 q% B& `: F+ t6 y
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ o, ^4 K7 P& _- S! G
- {4 q4 O6 m+ y1 r
- return 0xff;
$ R. m8 l' ]3 S$ B& |6 z4 f - }) j, g4 t$ O9 c: _
- 7 Q: g0 h: K f7 V' Y3 W
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 O2 {2 E* c5 Y+ P - ; u" v$ r( z7 C* h/ B2 |
- HIGH_level_read_time = micros();
1 G) k9 J7 R( X( r - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
9 B, t& _! m. h$ l2 N- @5 E' I - ; C0 W: u% d! P, T: a
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out1 v) e3 `- u) a5 }
- {- |* ]6 t. U" |4 E0 ?% m5 |. M
- return 0xff;
' g! |# u. Y2 g2 u6 R - }2 h- O1 S" ^" C0 o' u; X, I b
- 5 J4 w' b/ o1 Z- f- O4 C
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
( C. w+ `5 ?3 S, a: K# m" G, i* Z - {
) n6 u! b4 O" ]1 x( b - return 0xff;3 ]1 r* ?' n+ S6 V2 L: |9 h* O
- }
C; s4 Y! w0 {
0 K9 M% T+ D1 y! |- F- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 }% S# Z+ ^* z, t& u/ W: E - LOW_level_read_time = micros();4 H3 G q& ~0 K
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level1 q h, d$ Q H/ D) H- }7 {
/ U; P0 V, _# c- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out+ F+ y7 R* ?8 I# C/ G6 m/ }
- {
8 A) E0 f$ ^# H5 d2 Q - return 0xff;
3 O- x. F9 { ^ - }
0 X k$ I: r+ O. e2 G. i) ~ - ) F1 P% l& U. I: s
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
# @% r/ ` A. u" p: F - {
+ V6 P; T& u9 n' m H - return 0xff;
1 d7 n8 w8 g# b: V- T - } `0 W" T" e; C! e" [ V
* H. m: u$ _; v" G/ \5 W; U- for(uint8_t k=0; k<3; k++)! y; t! }2 u& w! u
- {
6 v p8 g$ T+ i - Sensor_Data[k] = 0x00;
9 s+ q' P2 w( h' f: N1 S- c$ `6 n* g; t! ?2 Q - . R. ]) H# z4 E
- for(uint8_t i=0;i<8;i++)7 W( p6 \" G1 L( t7 H
- {8 I2 J. t" V( b+ R$ ^, U
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
* V: I; t9 g n+ k - HIGH_level_read_time = micros();
2 ~% G+ L+ D0 | |5 J4 v1 G$ O - LOW_level_read_time = micros() - LOW_level_read_time;8 M, F6 J- j/ g' w
* x- n% U9 ?+ p- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
3 s/ z/ C$ i: V - {+ m8 ~4 }8 {+ I% ]
- return 0xff;
$ w7 S# `3 ]" t" X; L n( O1 P - }- \0 w; H+ F% V" R1 M
- # `; t, i+ O/ N) s
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: \+ G* P& S& p8 v0 i
- LOW_level_read_time = micros();
& m, Y j* \, A! `9 H9 X - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level, E" P' }+ o; v' q* Z
- ; z" q0 a3 b' y1 l( U
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1; ?! D% Q/ s/ J m
- {
! \# H8 D( ^& H) C) M7 o) V% w9 R' B5 M - Sensor_Data[k] |= (0x80 >> i);
1 y+ C" F b3 | - }
+ B; e6 l6 r8 Q& Q) n" @6 \ - else if(HIGH_level_read_time >= 100)9 z! _" D2 G1 g
- {
8 @; z% u- S% K0 J2 v - return 0xff;. b, f6 T6 Q5 R! _2 g/ N
- }
* U* V( U3 t0 [3 L+ I/ b
+ a4 L! U! k% m, C- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
$ W0 o0 f! I6 g6 `: Z" c - {) C. S* T& s$ E; Y0 w* K$ _
- return 0xff;) c& j: r4 v. g1 R% L
- }
. {9 H& j* k0 I& {; s - }
0 e+ c- E2 r; Q7 f! O - }6 _9 g, T1 ?2 w
' j# ~+ h" b. P/ F _- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
, R. i( A- `4 H( i5 ~9 C* Y3 |( I - HIGH_level_read_time = micros();
8 e+ e% C! a' X |) D. i - LOW_level_read_time = micros() - LOW_level_read_time;% Q% y+ Q% k( n# ], s: f( {- R
# d$ }+ m3 W- I$ x8 u- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
' W: M6 O9 z) W# U+ e - {) C0 @9 ~$ A3 h9 `: Y- ^; V; M" a
- return 0xff;
( f" M! r6 @0 a1 ?- _ - }0 m- Z3 b8 w0 L7 _" o! t
7 u/ _5 z+ p1 d5 q, Q+ ]: @7 H' A3 g0 x+ ]- pinMode(_DataPin, OUTPUT);
6 X' ^( ^) a8 D+ O' D& j2 h$ q% G; K - digitalWrite(_DataPin, HIGH);$ F( t# {$ u9 W
- ; D8 p* T; d: g h) J! m
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))3 q% C, }* J5 Z) n) Y) C' t* E8 z
- { O: K5 e1 v: ?/ x8 t+ Q+ t# D( J
- old_data = Sensor_Data[0];
0 Y: f* J) Z7 l% V& I% K& |- n - return Sensor_Data[0];( A3 p# u" t% R2 O& ~" Z" K4 y
- }
- T/ O a1 O! T - else, o J% M( {; v; c0 I; O! p) B7 }
- {4 Z! q1 C# A& E0 k$ a V
- return old_data;! x! N$ ?) i# W, D; L/ g, {! V% u
- }1 Z: T/ M! \+ ^) X+ Y# }# N
- }
& T, j# _' r- {5 J4 {
複製代碼
; o/ H8 ?1 U& P f1 }; ]MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
, E: ]0 D; }; p& }1 \* I7 M2 k$ e3 t4 p+ c* @
|
|