|
|
( m/ w% z4 S3 ~2 Z E, Q# L( A5 v- #include <Arduino.h>9 t! b" A& n* t( @
- #include <MeAuriga.h>
- e" l a1 G; s2 N. E; | - #include "MeLineFollowerArray.h"- E1 f3 [- W2 x: C! |
4 J( |$ k4 P7 E# L, |- MeEncoderOnBoard Encoder_1(SLOT1);
3 q Q& \/ r6 W+ O8 a - MeEncoderOnBoard Encoder_2(SLOT2);3 F0 w5 e+ I; t4 H8 ^. y+ R
- MeLightSensor lightsensor_1(12);
9 @! p U. a/ l4 ? - MeLightSensor lightsensor_2(11);
) S% J. `) M( G2 y - MeBuzzer buzzer;9 e7 i0 W3 T9 o4 \2 g
- MeLineFollowerArray linefollower(PORT_6);
" E3 |3 o, M$ t5 {4 c& H7 P& A* B - 8 t# w, p1 {5 g7 f
- #define Error1 1
) a) d/ W1 ?- G- Q6 p" }! h - #define Error2 2. H! I. b6 C( ?: L& S' z
- #define Error3 3
. `% \( T% |6 i g. X& I- P D - , i7 {3 T# X& p* w
- #define Kp 15' Y2 R* r' A' ~
- #define Ki 0.15
2 f, l7 S4 D% l; {' ^ - #define Kd 0.03
/ g0 @# V0 g0 E
' D5 O0 ^% y4 m7 B8 [7 a0 M2 \4 `- uint8_t sData;
8 J% Y# U! h \( A - uint8_t D1;- X' i. K4 J% ^: d0 U4 V* K7 z0 Z
- uint8_t D2;
9 w! X: Q+ D- l3 g6 J# _7 r1 \ - uint8_t D3;) H( g5 n$ w1 e C" c h; _
- uint8_t D4;+ f4 s# I% E1 F1 x
- uint8_t D5;: ]- n6 i% k y0 N( z5 i! S
- uint8_t D6;
8 w8 f8 E4 M& t+ s8 k6 r, Q3 Q
& H! w- A# t2 \, n& Q0 C- float previous_error = 0;* ?# Q; _9 y C$ n4 @! S3 R3 {7 d+ x
- float integral = 0;$ t, J4 S& y U0 s$ ?4 V5 Z/ X
- float derivative = 0;
. _2 k9 P, q* k) G" \# p+ z+ j - int Speed = 160;9 j" Z& R N9 s5 Q3 Y, @
- float output;! j* T) A2 c5 _7 T0 Q( D3 ?5 g
/ ^0 K- ]3 F1 W/ G- byte Left;) m" A8 O% ]) h9 H; o# I
6 b1 U8 p+ x4 z8 v0 G7 W* Y" b! J# }- void setup() " L: ?) e2 S$ F2 A: h& f9 ?
- {/ [7 v( H: Y3 l
- //Set PWM 8KHz' h. c( c @7 H0 |3 ?
- TCCR1A = _BV(WGM10);
# w$ D* x8 J" F - TCCR1B = _BV(CS11) | _BV(WGM12);
( S( H2 p! ~! J" n1 m+ D - TCCR2A = _BV(WGM21) | _BV(WGM20);$ w2 Q4 Y/ ?, T4 G8 \
- TCCR2B = _BV(CS21);" Z! ^# a8 G l* U! B/ b
- Serial.begin(9600);" ^% \; [* B$ E2 T0 z
- buzzer.setpin(45);
6 ?( T/ }! t: m$ `# P4 _1 o - }
/ f+ U; j7 a8 w8 T6 P - 6 ?' s% N' P. g2 P
- void loop()' q0 N0 } J& E' ~5 T! T2 N3 v
- {' g- q! s2 ^- W
- while(!((lightsensor_1.read()) < (10))); t6 T5 i8 Y7 t4 k3 H( i
- buzzer.tone(1047, 500);- _, p! a' S4 S3 i! \) R
- delay(500);
7 `" Z) w0 {% \2 w& E- n9 } - do) H% K5 ^ w" `6 I
- {
) L1 }% z% z+ w; n - int Err = getErr();
y% q3 s2 H' Y: t - if(D1 == 1)( i/ H0 W& ] o. ^
- {' v Y( T2 G; j( H1 |3 _
- Left = 1;: t; `3 J: w$ t; N8 u0 Y
- }4 r8 S# `3 f0 g$ V
- if(D6 == 1)4 @ }" n# j5 N. @1 ^6 y6 V/ @
- {
1 [& N" L" O, B' O- s2 l* D* P - Left = 0;" W; S9 d# ~: e3 w% e
- }
% ~4 x- J3 D/ q. I, `# O' K- G _ - if(Err == 99)* B& j' f' e9 u0 s8 E
- {
7 a; k0 c! i! H- [ - if(Left == 1) ~6 R) q* x+ c+ x- I4 z" v
- {
! B2 y; b4 v% Q- ~( G - Speed -= 5;& p/ X( G" r0 U% d6 E- w
- moto(0,Speed);$ ^+ A- ?0 Q9 ?* h' }% S
- do
0 ]4 l- _- o5 w0 G* b, `; i8 X - {6 H+ k& Q. X: [6 z J3 H. b
- Err = getErr();
& p; ]2 a! q* x- r8 l - }while((D1+D6) == 0);2 E) G5 P+ h5 f6 O/ t
- }* I& b8 @1 e0 N
- else9 }' z9 H3 i/ G0 m! } j
- {
0 Z& y0 O, R( C$ y" c2 q - Speed -= 5;
9 s2 E3 T0 r" \; ]4 I - moto(Speed,0);
! F7 Z e7 X D( k# g2 ]- M - do1 G% F% S# n6 j; i7 K
- {
/ ?5 H: y7 L1 F% ]3 i, S - Err = getErr();2 l" c. z" W1 Y3 T: S
- }while((D1+D6) == 0);
1 N! F) p4 ~5 ~) T9 T - }) \9 [1 _' K& K& d( F; T4 q
- }' B. y; I+ G+ J: R( i# ?3 y0 p
- else
' m6 l# c$ k4 p+ z+ l - {6 Z8 a6 E, c, s
- if((Speed < 160) & (Err < 2)) Speed+=1; + C+ x9 Q# k; Y- z1 ?: @$ a# O! c/ m
- if((Speed > 100) & (Err > 2)) Speed-=2;
8 R# F- Z' V+ y7 U - integral = integral + Err;
$ x! l; ]; ^5 y! l3 r - derivative = Err - previous_error;
# O0 c+ V* {" i+ y% H4 ^# t - output = Kp*Err + Ki*integral + Kd*derivative;# a( X, W& |: |$ V# \0 T. x& L
- moto(int(Speed-output),int(Speed+output));, U9 J1 j5 p. W
- previous_error = Err;' x) J! P. ~% q9 ^
- }
$ g5 |9 \9 ]: j2 m+ n - }while(!((lightsensor_2.read()) < (10)));
! H( p5 V8 e8 s! ~$ E% S& L+ V - moto(0,0);6 @ a! u9 o+ y9 G, T! S
- delay(500);
. F2 X6 z7 L7 Z/ ` Y - buzzer.tone(262, 500);
6 r" q# J) g5 P3 w* A | - }
' a1 C8 ~2 E. f( v0 q8 Y4 o - 6 O. {1 G2 ~' m" s; ]4 V) P
- int getErr()+ u! m3 }& i2 e5 G! L! i
- { - W% h7 y% e6 c6 [7 ?
- sData = linefollower.getValue();
& z# O! a7 _" E7 m' C& o - D1 = ~(sData>>0)&1;8 m, x8 @) X) X. B: c6 P/ n
- D2 = ~(sData>>1)&1;* i5 B' G9 v6 t* F9 k s5 _
- D3 = ~(sData>>2)&1;
+ ~$ i7 i3 K7 j& I" A - D4 = ~(sData>>3)&1; f Z" i& N6 C9 y/ x; \# [
- D5 = ~(sData>>4)&1;! U: g/ W, j" W; e; p2 p Y
- D6 = ~(sData>>5)&1;5 r0 K! O/ `' m. | I. b9 H. G
- int downD = D1+D2+D3+D4+D5+D6;9 R/ B" u! X0 V
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
0 c5 j( w' `: g% T( t - if(downD == 0)/ d; \( E3 Q" H6 X
- {, h8 ]4 H1 S* l/ y/ D2 y
- return 99;
. a5 O7 g! ]9 t# P% ?+ M8 t# x3 V, q - }
7 S& b7 u/ U" ~# g( a- \! h - else) c: j& x, ^( B6 ~1 D0 }
- {
. `5 w' X# H g& w - return upD/downD;0 L1 V+ O' @( q
- }
. g# Q2 Z4 |7 L - }/ t: H% e; J; m' j7 j
- ' N1 ^, d9 @/ Y7 Q, p2 T# d$ B% ]
- void moto(int Speed_L,int Speed_R)
) |" L9 Z! n) B& h2 K! G - { W; A# l! P. p. }/ k; [1 B; O% N
- Encoder_2.setMotorPwm(Speed_L);
' _, N% D: v9 g& w) X0 g - Encoder_1.setMotorPwm(-Speed_R);
e- p+ C& c* f$ K4 k' d2 W - }
複製代碼
W/ q1 M: E* T$ nMeLineFollowerArray.cpp% H; l- {1 [3 l3 ]3 [. p
- #include "MeLineFollowerArray.h"
: r$ x2 n: F# ? K+ _ - ( f4 R0 \ I. H' v1 r# g3 @8 w
- #ifdef ME_PORT_DEFINED( G* |+ i7 `# r. I+ f9 t: C2 Y. o
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)2 N _3 c8 J4 I! e8 v6 R
- {7 Z4 c: D( q i. z/ o
" E+ q: P* `# l% B- ?3 a- [- } e8 o7 ]0 F$ |6 c4 P. \4 ^% T6 u& R) w
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)3 }5 x7 u0 y3 E
- {
* y1 F {' o9 v' c; l2 Y - _DataPin = mePort[port].s2;5 Q- [% q0 s. k3 c3 ?
- pinMode(_DataPin, OUTPUT);
4 v! F5 R0 z7 v: ^( U/ }4 [ - digitalWrite(_DataPin, HIGH);/ D/ T- o# `5 r6 B6 t& {
- }
1 l( E$ ~& \4 E# p z4 b$ A - #else // ME_PORT_DEFINED5 m7 c1 Y0 h7 W2 z
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)* D6 k/ H; K2 x- E0 f7 a# F
- {
% d' U: m1 w/ z8 ~$ h5 Y - _DataPin = pin;+ P ?, t7 h! K: B7 n
- pinMode(_DataPin, OUTPUT); . R$ M+ }- O1 ?; T; i {' A$ s
- digitalWrite(_DataPin, HIGH);
! C" r& c( ?3 S9 w% L' c; ~' I - }$ z. j# d9 a4 ~) I
- #endif // ME_PORT_DEFINED
! A1 G. c2 p; R$ ^: I4 F - - E1 z, p" B& i# _$ e- A& f2 O
; `. }% P( @' p- void MeLineFollowerArray::setpin(uint8_t pin)8 h+ M W) M# K! d0 ]4 H0 ?
- {
3 T/ N3 p! E7 w1 T9 C5 [ - _DataPin = pin;4 _! Z& `, t5 }
- pinMode(_DataPin, OUTPUT);
6 r8 y4 _. y' A3 t$ y1 y - digitalWrite(_DataPin, HIGH);
! s3 N T4 r2 \2 x - ) i {; }; t+ \( L2 ]+ A: W
- #ifdef ME_PORT_DEFINED
9 U. A* [1 B% y/ _8 `- a9 f x - s2 = pin;
' ]1 Q. t% ~0 [# P6 x7 @/ F - #endif
; c( O" e0 I: u- n! J - }' x7 y- A9 R6 w" c: c
7 j7 \( y; A$ h; A" y6 i- uint8_t MeLineFollowerArray::getValue()7 [- S& A. z; G0 G( h: K
- {8 T0 w' }- ^+ F2 X$ z4 I
- uint32_t LOW_level_read_time;
9 Z3 g1 S) g5 H3 v - uint32_t HIGH_level_read_time;
+ T# i$ g3 x1 q. A9 f6 P, W( q - uint32_t time_out_flag;
1 D) a4 z: n5 q. ~ - uint8_t Sensor_Data[3];4 [( f# p# @. _9 M. b% w
- static uint8_t old_data = 0xff;
" _4 Y; l2 }+ W1 Y - 0 N! r# U+ u1 P0 x v; R
- pinMode(_DataPin, OUTPUT);
) d( t& Q9 y2 }* P! | z - digitalWrite(_DataPin, LOW);
3 z6 X2 x" K9 D0 i h. K - delayMicroseconds(980);+ Z( A1 i9 N% p9 s" E
- digitalWrite(_DataPin, HIGH);
0 S5 S$ ~- w$ v4 b2 `- z" y - ! v2 F" o8 J% ^ R
- pinMode(_DataPin, INPUT_PULLUP);
* ?$ F3 D* V4 J J7 Y% I5 M - delayMicroseconds(10);: F0 z& }' R: ?4 h6 {6 F ]7 t
% V. \1 Q1 V4 F6 w5 o/ W- time_out_flag = millis(); @5 s! U, b3 }% I& q! j
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); |# R$ C# _! L0 X A
3 S3 b, O8 N, e7 V1 A- LOW_level_read_time = micros();
$ ~& f- E8 d& z3 O7 q - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
' ^) N% G. ?1 @: i - {' b: e: H/ Z+ G3 E, t
- return 0xff;
$ a6 o4 o& C6 C7 N3 H, F - }
' T8 Y3 J3 [; i1 K0 h" Y - ; C, o. K; a3 a# X7 K' \
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( y" {% i7 o6 t& F7 h
6 f# h* \ ^/ I: e0 @4 x; \- HIGH_level_read_time = micros();) R0 G& Y e S A7 j0 ^
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level- M9 @ P0 ~# ]' p' [
* n$ d) `; Z% j3 O% M' m! h- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out1 c9 `1 T [) ~* e3 j1 F7 k
- {
' m6 ~: C3 k: o" [! Q - return 0xff;% _& L W5 Q& z/ D6 a8 P1 c
- }
" H9 T& [' R" b
' J/ C; u- b/ B1 n3 S* q" }- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))9 A9 a, v; L+ o7 h) s
- {
, z2 E6 _4 {5 Z - return 0xff;
* m8 e5 y- I, g - }* a% {# o4 t. e4 N. J9 B
- # G) Q+ }8 x0 Q) }# h8 F2 j. Z
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 g1 A$ @9 l- P4 r/ i
- LOW_level_read_time = micros();
& D" U0 E* g: _ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
% L0 e# i' ]& y( o% R8 `
3 L& |7 ^! K5 ^# K' _: u- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
; A+ P2 }. R4 O" @ - {+ ~6 y4 `4 R9 a. D! [& W: q* {
- return 0xff;; T/ K6 F. A. N
- }7 f/ `* R; e! A. G
. B) \0 \, q( u: K4 N; r4 e- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))' p6 k% ~, J/ h
- {
+ Y7 X) Y& z% A0 ~6 y, w - return 0xff;7 |/ p+ h; A& Y2 b6 N
- }9 m& v6 O9 I7 w: l. T
- / E( K; d, b$ t
- for(uint8_t k=0; k<3; k++)- I5 M; Q0 f+ p: z' v7 H, w
- {
- e" e8 Y; \* \; x# j: r - Sensor_Data[k] = 0x00;
1 r9 _6 q3 g! y1 {
' |( \- ]4 q: S H7 {- for(uint8_t i=0;i<8;i++)- m- L" [1 y2 s4 |6 x
- {
+ F# _" v+ `2 v3 U; V8 c6 I* s# { - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level ^& j; u4 |; I
- HIGH_level_read_time = micros();; g% A3 C" A7 k ^
- LOW_level_read_time = micros() - LOW_level_read_time;
: o! g; J% D& u
! J v' v) h c3 l& |" P$ A- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
1 W* I2 |5 r. A; Y/ H - {3 r$ [0 M% _0 h$ y
- return 0xff;3 X+ ]1 J- p6 c8 j: l& ]6 V( c) _
- }+ ?. _1 U: g, d
: j, B' R6 u1 s% ]- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# ]" h! K0 I/ y7 o i/ C - LOW_level_read_time = micros();
, t' j `+ V* ?+ Q2 [/ N. t* @ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level( |$ N& f7 S; k5 a1 M
- 6 O0 Y4 w) W$ A
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
) B. b0 u3 `. I! _5 t" | T1 M - {
9 }; C5 b9 A7 @1 c: n& a9 r - Sensor_Data[k] |= (0x80 >> i);6 M4 O4 h8 z; ?$ g
- }: w- d( r- ?( g# r. V2 {
- else if(HIGH_level_read_time >= 100)
+ y( e" L9 q8 N - {, k4 z& u% _, v' s4 k7 T
- return 0xff;7 T. q8 n: f' H8 n1 D0 V
- }
- `& C; x) W, V4 b! k - 1 U6 D) p. T: E5 `
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
8 Y+ z$ b$ n% c - {
' k: t/ u; b% F - return 0xff;9 ?# |4 q% [7 ]* J6 ^9 y
- }# i/ r! {! W0 }2 m" K& ~' C
- }8 W" H8 B2 D+ P; g) _- o. U. D
- }! r7 y8 Q6 j8 j: c2 g1 ?! g8 [
- ( w! c6 w- R9 C
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
" [9 w3 N" _# i, [6 K# T6 ?4 d - HIGH_level_read_time = micros();
x B8 f. I4 V6 X; k - LOW_level_read_time = micros() - LOW_level_read_time;" Y- |' V; g. e4 F3 I. I8 D+ x5 N
- , L. V2 j7 c: O! C- v+ D$ f
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
. x7 C5 p! s6 [( ]0 r - {
4 X; e' b0 Y5 Z: o) r7 I - return 0xff;; |0 G6 K+ K4 \5 v, \+ u
- }
7 m8 h0 ] A8 ~) l" R. ~
, _/ P( I/ o& D3 f" W" k9 J- pinMode(_DataPin, OUTPUT);
5 }4 J5 |- a8 o, R" X- I4 B# u4 ` - digitalWrite(_DataPin, HIGH);
' g/ M5 m8 ?9 m/ S
+ @9 F) o j9 x% C7 r* R1 c- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))5 r9 U( E" o# m6 |$ ^
- {
5 L, p0 O+ n" M- t7 p$ s - old_data = Sensor_Data[0];
0 Q/ j a. `5 {5 q; I0 X - return Sensor_Data[0]; b+ o% [' y \" W8 C0 X; l
- }0 I3 E8 _0 t# h }5 e8 } j/ R
- else1 R6 s5 V$ |, L @0 L
- {
: E- j. f! z7 Q# r+ d$ g# V - return old_data;
) H( X( i# x. n: z I - }
/ X( j. |( B8 P! S! s: E/ y - }
' }* c, K$ U) a l
複製代碼
0 N& t+ {; m% L" P) | pMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
3 @5 \. ]! S7 W5 F* L* q3 o! |9 ^
9 `6 J' s4 X; t& s
|
|