1 O1 z5 ?4 R1 ?4 L% g6 G #include <Arduino.h>
& P [7 @2 {: c& i/ j: D #include <MeAuriga.h>
& W' h! E3 W6 e! s #include "MeLineFollowerArray.h"
; f) U& h) h7 n: ] ?/ M 3 S' n, f& V5 [' T3 V& F5 Q; W
MeEncoderOnBoard Encoder_1(SLOT1);
" k3 u% U; m" m MeEncoderOnBoard Encoder_2(SLOT2);, c X* I, D; y' ]
MeLightSensor lightsensor_1(12);; _! n3 ?& B) s- d/ s
MeLightSensor lightsensor_2(11);2 l) l. W# K! c& N/ |& O* Y
MeBuzzer buzzer;
/ b& ~! ?& G3 m) d MeLineFollowerArray linefollower(PORT_6);
2 @. V. V+ D, S+ A! i
' l9 S, y o J( u# j3 Y #define Error1 1* A+ @- a( y3 q
#define Error2 2
0 j& P3 f& Z* E& {/ Y #define Error3 38 ?; W+ x* q% n4 u6 ~6 C, ~5 @
' x: H; L# b6 Y #define Kp 15
1 v3 H# L& J' P; A5 h #define Ki 0.151 f& P% d+ `! Y' S/ A7 Y
#define Kd 0.03- }) c' z, Q5 i A
! D K( S. ~1 Q' z4 r+ M$ f9 _# X uint8_t sData;
* k8 f( p% p& d! r uint8_t D1;
. H, S* w$ t7 w, L& M" ^1 R uint8_t D2;
: X$ Q) V0 A4 H9 F0 N( J4 b uint8_t D3;
# Q7 v8 Y$ \* y+ q uint8_t D4;
+ U' U3 y; R+ u8 |, ` uint8_t D5;
9 {' V8 f7 w3 [% e L( H uint8_t D6;9 m# |* g' A: |# D. K& D* S3 n0 _
) f# j! @$ o+ _% N* |
float previous_error = 0;- ]% A |. Y7 F, i4 a& j
float integral = 0;# Y. P8 E, {' j- k J* ]. }& l
float derivative = 0;
9 O- P% c% D! _0 |, u/ h int Speed = 160;# _2 g* O/ D9 Y9 A' u" `
float output;
. U Z0 h$ v, t( M5 H: m# {( k ' E) N. g" Y V
byte Left;9 O9 D/ ~; t, [$ R) A/ v" \5 M: H5 L
$ C( W7 F9 b3 n void setup()
; O: f: {. j! E. V' C9 c {8 R# ]! ?! u- s$ L
//Set PWM 8KHz
/ w, R) ], B9 {. O0 k/ d- ] TCCR1A = _BV(WGM10);
# m% L8 u0 ~+ f) ]- J, H! p TCCR1B = _BV(CS11) | _BV(WGM12);
, I' |! y( |$ v8 k6 [ TCCR2A = _BV(WGM21) | _BV(WGM20);
, U5 ~$ x. q3 F- w' f TCCR2B = _BV(CS21);- |5 o( N5 E$ K) R- u
Serial.begin(9600);3 d, \* A4 O7 P3 G) {. F& w2 W' y8 L
buzzer.setpin(45);/ P* ^) U) B& N7 k
}
. ?: |$ Z8 v1 ~; }+ }5 N5 d
6 i% \% q" X, X+ x S6 j void loop()* [3 j) p+ c* C* Y, _1 X
{
$ Y' e& e' V( i' U2 I7 R% f* q; P* p# ?8 O% p while(!((lightsensor_1.read()) < (10)));- ~4 F5 t' s; X3 ?& e) Y
buzzer.tone(1047, 500);, ~" |5 `6 e0 P3 u8 \1 ?
delay(500);
2 r! |5 ]$ i$ ]! q% V do u5 T; L {9 ]5 @3 D! V# r. {- b
{( b3 [4 R: d& f; @- y; f
int Err = getErr();
, e F! k. b8 l' g6 m; G if(D1 == 1)1 D& h4 z. C( t/ h
{6 O* T f2 x9 s0 [% {# P
Left = 1;& M) _8 D ^. k" `
}
+ ^. ]* ?5 J# K! X" c if(D6 == 1)' t5 B: I! o0 U) `1 i% B
{7 D/ o8 G+ ~8 [' N" @6 E/ J
Left = 0;
1 I6 g* l5 S, i, P+ I9 U, F }
( A2 `$ H$ p- O& x7 }$ I3 F if(Err == 99)
$ i3 Y7 W+ P0 k3 u& p {) @1 _( o9 j: L2 D& w4 d0 U3 z
if(Left == 1)9 ?% F; G+ s( U" ^* x5 h V' ?+ H
{" q1 t! j% ^2 g& z9 V+ X
Speed -= 5;
6 Q: d. e3 f, G* Q# O8 l G moto(0,Speed);
/ n7 E0 W) z- R, F% d7 m do
) l' C! O2 P `7 ^3 x {
- j7 h" a; d7 S, ^ Err = getErr();/ T0 l/ H& w1 s$ B [3 u! Q
}while((D1+D6) == 0);
* K1 n, w u, v }; i- A9 a& S, n; R3 c, o; t: l' v
else
4 ]7 I7 E6 z& N) W! x; Y# I7 e3 u {$ I2 P% M! G( P& u# h( x* b3 |
Speed -= 5;
( L$ f9 Z3 U6 g- P* r moto(Speed,0); ( h4 v- t# M$ b7 N4 R+ J8 ^
do1 a$ }; n5 ^3 X3 X# P/ P, H% W
{$ Y9 f+ `1 t3 x
Err = getErr();0 f: _8 f7 ~& o7 ]$ i
}while((D1+D6) == 0);
7 l+ R6 J8 A. L }
3 M2 n. z7 l% i* u }8 X! z2 `( W ]: g p U
else: z; L5 j9 e* U2 }1 h
{
; A) [$ ]5 R0 S {. R if((Speed < 160) & (Err < 2)) Speed+=1; ; u {1 C: q) A( X
if((Speed > 100) & (Err > 2)) Speed-=2; 2 c8 Z$ o* q6 E3 Q
integral = integral + Err;# U5 J" w$ l' k
derivative = Err - previous_error;
6 S* Y) @) A1 f& k1 u- U output = Kp*Err + Ki*integral + Kd*derivative;8 @$ B! S. `5 G r
moto(int(Speed-output),int(Speed+output));) T3 { Y4 b# |
previous_error = Err;0 |$ [7 d( }4 n1 u. J) l8 Q
}
3 P; t0 m- @, }9 }- A }while(!((lightsensor_2.read()) < (10)));
, y& k$ b4 l4 L5 n/ U, I9 j! S# j moto(0,0);
3 ?- F1 a2 U2 s1 I0 e: ~2 r delay(500);
# S1 \* ?7 Q7 Z1 ^- G buzzer.tone(262, 500);
. ]( Y* c& R+ P* y# y7 Z }
4 S/ B0 v( B# b! F6 w' ~ # z8 C, s: W9 c& G4 F. \
int getErr()$ F% w3 \* _2 q
{ 3 x! C, n6 p0 b; d o+ V& j
sData = linefollower.getValue();* M: s# \, ?3 s/ u
D1 = ~(sData>>0)&1;
; K6 X( d7 d$ Y D2 = ~(sData>>1)&1;
5 c2 m4 E: {- o D3 = ~(sData>>2)&1;4 f8 _! W8 P. w% b
D4 = ~(sData>>3)&1;1 R+ ^$ E- k" _8 v, V7 U
D5 = ~(sData>>4)&1;
, d( H7 }! E6 y& h) }4 [ D6 = ~(sData>>5)&1;) g/ g6 C) m7 c* X0 J
int downD = D1+D2+D3+D4+D5+D6;, I4 q# O* _7 T# ~! @
int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
) B ~) d. p! V3 I% ^7 i if(downD == 0)7 ?# B% F6 m# R) D$ {
{
, T8 P3 {8 |# J' I return 99;
$ x% m. ^- a4 b) K) b" h: d0 ` }& Y3 }& b/ }2 w7 h
else; E" k$ k- u% ]1 Q5 C Z
{, H5 L! X/ L: q( I
return upD/downD;/ R! _/ Z% ]0 f; w2 l9 ~
} N" a2 {3 h: q/ U3 e
}
6 P1 N) H* L/ Q3 U 2 ~3 B x; x" P" |) c# c3 _
void moto(int Speed_L,int Speed_R)3 ^" v: M! W( ]5 z! W
{
# c$ }) H7 r7 i$ D% d4 i) H Encoder_2.setMotorPwm(Speed_L);
( y% Q9 f* K& E! d Encoder_1.setMotorPwm(-Speed_R);. K7 ~$ b$ Q% V6 e. \# L
} 複製代碼 $ n# B& p; F; }$ X$ E J
MeLineFollowerArray.cpp
3 v* R5 Y4 Y3 U$ |- P1 ?' r3 I$ o #include "MeLineFollowerArray.h"7 V8 P, S0 I M* H+ k( q
5 @) N9 J; v9 E% V a #ifdef ME_PORT_DEFINED1 \! w8 E6 H2 ~; I% f" G) [
MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
2 m' v+ O6 S) S( L8 q- l {
4 Z/ l- V% K& e! r9 _ & M- R8 n h B! V. M" X \
}6 x1 J$ P0 L, O% ]. n
MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port); B9 c! u3 w; H: j
{
4 L3 [" W+ g) _; O. p+ T _DataPin = mePort[port].s2;
6 {$ B% C! w9 h$ g pinMode(_DataPin, OUTPUT); 5 ~' P1 ^+ m) X1 t) A J9 _( u
digitalWrite(_DataPin, HIGH);
/ F$ M' T; ^- }; z; f+ T }
! N3 h$ Q {7 [3 a #else // ME_PORT_DEFINED J9 d. d4 Y" e" f* r# z9 @
MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
# N/ Z* U, J3 M+ h( [ {2 ~: y$ O3 b0 G: Z
_DataPin = pin;
5 ?" e8 M, e* ^ pinMode(_DataPin, OUTPUT); 1 k, N2 O4 l6 C' I, k" e( L' B6 j
digitalWrite(_DataPin, HIGH);
. c! i& v+ d1 O& O; P) o3 Z }0 Y4 Z/ A: ?/ g! {& s9 u% ]
#endif // ME_PORT_DEFINED4 E5 o3 @+ r# _6 o( d
- g1 p2 E. w- S; C! q1 O
. U. V# A6 M1 c' U' z2 E1 E% y void MeLineFollowerArray::setpin(uint8_t pin)6 s1 a; v5 v2 @8 Y$ }
{3 O) ^' Q% ]/ {
_DataPin = pin;: R0 j# A5 k; \# A8 F% n
pinMode(_DataPin, OUTPUT); , A/ v% a2 |0 S- x) o- X- I3 V
digitalWrite(_DataPin, HIGH);
. W: l/ u; V3 M: B5 M/ b6 o : E, b! ^& \4 h: s) L- x
#ifdef ME_PORT_DEFINED
9 R3 D; l+ `7 Z6 n5 N s2 = pin;
8 _- l9 X* {# E# x #endif
- D/ D4 X6 n9 j( m' \" s }
4 t4 V2 j7 e& f7 a' C; b, w. W2 T 3 G4 \! y- c' ?4 {! ^0 ]
uint8_t MeLineFollowerArray::getValue()
" |8 w# Z. Q. G! x& j0 `8 | {
/ d( |& `+ W# W0 ? uint32_t LOW_level_read_time;
% Z* y, p8 W2 A6 o% N uint32_t HIGH_level_read_time;$ i( M0 u/ K0 ~5 W- F$ {
uint32_t time_out_flag;2 i- s8 q$ }8 X+ x$ D- }3 {
uint8_t Sensor_Data[3];
5 y4 p3 B1 C; j# k' _* D$ N static uint8_t old_data = 0xff;
' s0 W8 }" x. J+ y6 A" L - a9 t4 [- S+ h* Y4 |1 B
pinMode(_DataPin, OUTPUT);
! e. {/ q* l8 s6 T% W' e2 G digitalWrite(_DataPin, LOW);
" H1 T) F5 I* O7 t) } delayMicroseconds(980);
+ A! g, w r1 l* g4 v9 d3 X digitalWrite(_DataPin, HIGH);
% W' P( E1 W# S8 F: ~6 V( h: j
/ W; {1 ]8 E3 R1 h( d pinMode(_DataPin, INPUT_PULLUP);- r3 z+ O$ F1 Z$ y! ]8 h* E( f
delayMicroseconds(10);
. z5 p! Z5 J" n, q * }/ U( |5 V0 v# n4 q* x
time_out_flag = millis();
% n) g. e5 D# Z+ h6 x- { while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( B8 Z" ~; f$ j* n - p! z2 O. v: q5 e, i2 z
LOW_level_read_time = micros();
9 V2 ]3 S$ U7 T" |; \2 A if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 f/ ^% S, T. R( R9 h0 H8 Y {! z" U# M4 H& o$ Q2 \
return 0xff;
5 t/ ]( A1 O7 c( e6 U# i$ H- r }
5 {# }2 W! v* T% r4 L* A
1 P: Z" I. n% U( C" x while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
1 t8 \3 I& p1 W9 J
# s; [4 a- v$ d* P HIGH_level_read_time = micros();
4 V: F: g4 @2 `! M, y LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level; t; N: M* C- d: u) p: Y: C# k% [
" @6 E& T* k0 c) I, U if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out, {) x1 `* }# B8 _7 s1 r
{
N- p7 J* E0 W return 0xff;6 g. _: G4 n; t6 ?& a- r
}
2 j" t+ k! f0 z% a
+ f! K( ` w% f5 q% Q# Z if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
- S4 n! \* j1 s K: T8 n {
0 Y4 K( B5 i) |2 z, r return 0xff;7 @9 z5 u, B* _
}" u6 ~3 p$ ^) l2 R
- @; B7 y% Q4 p
while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( ?) X! C, f8 u) S/ i/ f, S
LOW_level_read_time = micros();
. H2 g$ q. [+ N% B% H- ^3 Y HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
) Z% e8 C6 Y$ {5 x; H
! j6 l3 F% m4 q5 ^% A4 I if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 B( ~9 `% x* r3 ~2 n& ^5 F {5 l+ G0 b+ w+ f1 _
return 0xff;0 b* U& B2 k" G& R3 }
}+ d1 D& ] ^* L5 W, g* B
, M A! b3 z3 E4 D. c if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
% Z& x9 @4 w8 _/ F0 o {' C2 Y! I" D; @: v4 p% p8 ]
return 0xff;
4 [4 y# m# \, u* `$ `2 b$ ~; b4 U }
- k4 f+ f; d# H4 ]' s$ ~' @
. P! r6 M0 n( `5 y [ for(uint8_t k=0; k<3; k++)' W) {( E* n2 M
{, z& q) ~* u, `# F8 x6 v- P
Sensor_Data[k] = 0x00;1 D# }3 g$ m% t& \$ V
, |; n$ P, s, Z0 @* p7 G8 [! k
for(uint8_t i=0;i<8;i++)
0 Q$ r6 z. p; r: R- i9 p6 O {. g) w2 w7 G, K3 r7 I7 V
while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level! E' h- A5 m) F. k
HIGH_level_read_time = micros();
( H8 H: h( [6 Y LOW_level_read_time = micros() - LOW_level_read_time;
& ]5 L0 w; _# T0 D: S 2 j& s/ L5 d4 E5 Y- [7 I ]7 _
if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
0 j6 c* [6 s: _: z/ `( p {
4 X0 u) B: y; z% k n return 0xff;1 \1 H$ |( d- L$ ^& j4 f
}( V" U' K7 v+ V
{6 e& X4 p0 _1 R" H
while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
2 F/ y; ~5 `) n# n: r+ D" c LOW_level_read_time = micros();' u( C) S& u. g3 \
HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
0 r' X( F$ s3 B : b2 R0 _2 I6 @* ^! p
if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
5 Y) h8 P- ?& E5 `- M" S {; v' o: B# k+ u0 h5 h e
Sensor_Data[k] |= (0x80 >> i);
3 I8 P+ W9 d% B1 k% s: A% ?6 k( r }
* O2 j2 p7 D* i8 z. K6 Q2 }$ b else if(HIGH_level_read_time >= 100)! e2 w' {. V) g) y. h" L$ B. @3 b
{- f+ m0 T( a- Z3 d& X7 d5 V
return 0xff;
; n: w9 p- H( b/ R- h a }! m' Y* ~ e1 m1 Q" v' S6 r" b0 E
3 b( ?! f) Z, p: @ ~7 [* R" G: y0 Y if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)" x6 q0 Q! _8 Q8 j9 O2 p
{
6 A2 |1 M) D0 w& z return 0xff;
( Q L' w! k$ x! q. F }, D5 @7 d, y; `! }" u
}
9 |0 r* a5 ^% f6 J }3 w+ [/ u0 ^. m; R
5 m( D$ X. C7 J- w
while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
/ \2 o+ W$ c8 |/ f1 K6 X7 Z) \ HIGH_level_read_time = micros();
) v3 M: n& H# a& v0 a# g LOW_level_read_time = micros() - LOW_level_read_time;/ W5 v0 ~& m. Y4 ^
' v' P$ z: I6 I! h8 B4 j if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )6 Q$ s. b" f/ y ?7 d$ m
{
9 R( F( }8 o; v6 y9 N4 d4 V return 0xff;
- ~. a8 o2 R# s/ |1 x( Z }
5 h5 N% o: L- P6 c$ v
/ P/ p D# D$ L! e+ w' q/ A pinMode(_DataPin, OUTPUT);
( D7 f$ m. m& G9 Y. i digitalWrite(_DataPin, HIGH);/ B" }7 W6 q: h4 [$ Q
5 g/ A7 E1 A) v! g
if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))1 p$ D0 F) j- [! {2 \
{
1 q6 Y& ], F O old_data = Sensor_Data[0];( C3 u& y8 W, R: W" e- I
return Sensor_Data[0];) o% x# Z: N4 d ^
}0 a, y# V0 K4 A( \
else# V& X+ p, v+ ?
{3 @* P! v B! H4 p9 F# _) I/ w
return old_data;
9 g# W9 }$ F( |" | }
2 T( L0 @$ i8 w `9 D }
( e; |, N) i6 [2 R# I: Z 複製代碼
, M8 v6 W; O$ \: [) J0 j3 L6 Q MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
3 H! ]- F; c1 N$ h2 I2 t
q C9 R+ `3 a