|
|
+ _! B) g @* N: e6 x6 t, y/ m& O
- #include <Arduino.h># A% A' k- K& l) X2 `
- #include <MeAuriga.h>
( M. N$ T; _7 |" l/ k" |6 f - #include "MeLineFollowerArray.h"- m9 {, i- B& q+ W ?
# T3 g8 f0 V9 S2 ~/ R# U$ Y- MeEncoderOnBoard Encoder_1(SLOT1);% H3 y6 d- F7 F! O; x) U# O
- MeEncoderOnBoard Encoder_2(SLOT2);$ n3 |$ i* T3 C! |$ _ G1 M
- MeLightSensor lightsensor_1(12);2 ], J% i8 P# u& l% v
- MeLightSensor lightsensor_2(11);
6 u$ w) p8 b5 \! R( f - MeBuzzer buzzer;
1 W$ P' K2 ? ~, B, X0 O2 t - MeLineFollowerArray linefollower(PORT_6);& H1 ], l x0 l8 [& V- L# E
- % X# }$ {, A$ E u
- #define Error1 1
8 ?6 M4 _; y m5 A2 p4 z - #define Error2 24 x! n" B. h' r, T! ~
- #define Error3 3% o$ V# m3 e* a, ~$ f
- 9 K i& O5 T; \! ~3 o
- #define Kp 15: v k/ f3 U" E1 a9 A# B
- #define Ki 0.15
# o. h" n9 Y% q. K - #define Kd 0.03
# z8 D# h+ H: z/ g( P. v* G - / u/ B( t3 z8 x4 ^6 Y h
- uint8_t sData;
, a1 m# P' ^" M% W: V+ i# [' l - uint8_t D1;
' n0 J/ R$ q8 l - uint8_t D2;. k3 A( C) W, W
- uint8_t D3;- k0 V3 S% }1 m( t
- uint8_t D4;
$ m% c' u( j# [7 P - uint8_t D5;
' I) T; S0 u+ o/ I+ v - uint8_t D6;
+ f+ L. M! y5 `4 H \& | - 3 E% A9 b9 ~5 {! u% F
- float previous_error = 0;
4 C6 y+ l% w( b; C% x - float integral = 0;' `4 v% q4 a% O: k
- float derivative = 0;% Y- h: b3 |; _/ x3 u# ~. \: L- a
- int Speed = 160;
# L. z+ @( ^% q+ A8 P - float output;
( _+ E+ ^! U1 R2 i- p2 f1 e+ W
! x( N- ^ t K& H4 |- byte Left;
: X! D8 A) B& Z5 k3 ]0 H - 4 J4 C( B& P; O+ F H+ p, i
- void setup() - B! d8 I$ b8 R8 f! U3 t
- {
8 L4 b0 ~/ ]5 f; b. i* k - //Set PWM 8KHz# Q# l' I" \* x, e, @" Q
- TCCR1A = _BV(WGM10);
1 H t1 w* n) h/ o- I6 Y9 q - TCCR1B = _BV(CS11) | _BV(WGM12);2 K) c/ [; u7 \ ]) i
- TCCR2A = _BV(WGM21) | _BV(WGM20);6 M* ^1 |+ }" i( p3 h. A1 r r
- TCCR2B = _BV(CS21);
2 w0 H% o2 G* ~& Y- Z - Serial.begin(9600);
5 {, `3 c* {4 C - buzzer.setpin(45);4 f6 K$ b. G0 o9 n# A
- }
+ X4 s1 R8 c m% }& d6 W - 2 i. s. j( A$ e7 m
- void loop()
R( ~5 n1 w& }9 \ - {
0 Q2 Z3 r- H6 B& _7 V - while(!((lightsensor_1.read()) < (10)));1 Z& h f; S6 H- B0 ~
- buzzer.tone(1047, 500);
0 l9 P7 Q0 a% `! U( @) m m7 _ - delay(500);3 x! ^, R. M7 T( P* w
- do
! _5 ~& d2 p' C' f+ j& N - {" z; y/ Q/ G& ?! V5 O
- int Err = getErr();
m- f* C" r( W1 U4 R, `( y. o8 E - if(D1 == 1)
% \3 [ j: F- W, X- K9 _ - {
s4 I9 t8 w# u* R% R - Left = 1;8 e! n. s n- V8 @- g3 K
- }/ ]3 t* J8 X0 c; B3 n( j7 m
- if(D6 == 1)7 o9 V1 E: H% x, R! f D4 J
- {
) H/ {; }2 ^( m3 a; N' _ - Left = 0;
' x. ]7 U/ q- v' G2 ~3 o - }, g5 _+ ~8 i7 B v) S$ F6 g
- if(Err == 99)
8 Z6 v2 f" K! i2 k" Q/ y( p: ]0 I0 s. J$ @ - {
6 S: a1 C5 l. g9 K8 C - if(Left == 1)% _0 n! |5 A' ~4 i9 p: d( T
- {
" d& V; f! W# b( P; h" V! E - Speed -= 5;
6 P! f7 U" W" G2 D$ S - moto(0,Speed);# {2 K! J5 h8 }, J3 p5 E
- do
/ h& D6 b+ @* V - {
+ j; ]+ J5 d# k+ r5 b - Err = getErr();
. H9 p7 [/ X6 a6 [$ z - }while((D1+D6) == 0);
8 {- v' K6 C# e" ]) I: L' ]7 u - }6 f. n z5 I) e! t9 m
- else
2 Y' n1 W$ y1 w3 W" t9 A - {) n. p4 I- [( K+ Z
- Speed -= 5;
0 `7 p6 e+ {) P% I E) ~ - moto(Speed,0); ; v W. R1 O$ A
- do
% h: |+ P) ]1 K6 h+ y8 X) f - {* F+ i N4 r; L& q" x+ u) s
- Err = getErr();
& L2 k0 W; O J. T( B- L' E - }while((D1+D6) == 0);
6 ?- O( `# N, U" y - }% o* |) _0 c) ^3 h6 U
- }
% m- D& W6 D6 O - else
# V7 ~1 M5 M9 V7 B8 u7 C - {
4 h- ^ l" T! g w* L1 ~! g - if((Speed < 160) & (Err < 2)) Speed+=1;
5 l* U; ?& \2 x5 Y - if((Speed > 100) & (Err > 2)) Speed-=2;
8 X* `) E0 ?- B( z - integral = integral + Err;" c( U6 z$ k( p, s) V5 O- a
- derivative = Err - previous_error;
! }' ~2 Q7 x! E" ^5 V& r+ \& ~ - output = Kp*Err + Ki*integral + Kd*derivative;
3 Q. P2 z r8 g7 A; |$ {) X - moto(int(Speed-output),int(Speed+output));/ n$ p8 B" b' X, J2 X! R
- previous_error = Err;
) D! y" }$ M: t( S; _- V3 x - }" u. K S3 ?# I) l E6 }
- }while(!((lightsensor_2.read()) < (10)));
X% _6 R5 n, s, Z. A - moto(0,0);
7 q, r. @3 f% Z& U - delay(500);
4 g, x0 c6 X, h! Q- Y% D - buzzer.tone(262, 500);- _% I6 u5 Y6 l! }3 S R
- }
# \6 p4 t% K; d' @
4 U6 `( Z! m6 I$ g1 |- int getErr()
( x! I6 O, I0 {9 e - {
# n( }5 ]4 P- Q+ ?5 j3 j" O& ~ - sData = linefollower.getValue();0 p/ Z2 e/ H* e t
- D1 = ~(sData>>0)&1;
, q4 H: p6 W, @* N - D2 = ~(sData>>1)&1;
6 s7 O! [2 V8 w2 m6 S3 c# F - D3 = ~(sData>>2)&1;0 Y9 @# E; A. f/ e% f
- D4 = ~(sData>>3)&1;
$ k3 q/ c, v: P, P - D5 = ~(sData>>4)&1;
! @. ]* q3 T# P4 n/ | - D6 = ~(sData>>5)&1;
8 u: d$ ^: Q6 y/ ?1 y4 ~ - int downD = D1+D2+D3+D4+D5+D6;" D+ j7 I6 V8 g! J9 V
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3); t6 D E& J2 {$ {8 p5 C- j R5 B
- if(downD == 0)
I2 D! @0 a/ z2 {- G( P5 I; P) y! |; O - {/ }9 F5 u. o+ B8 Y
- return 99;! ~/ ?! v- \2 n2 N7 z! J4 H
- }
( H4 ?) h1 ?, Q1 f, v- J - else3 [5 l7 [* i* O$ R
- {
* L9 V" X& a' N - return upD/downD;
: d# ?" ~1 U _: X+ N - }4 V. R( m3 j# X3 V# A/ N+ O
- }
$ v6 u' m% G+ o& U8 g, M7 T; b+ W/ E! b - 1 c9 o/ i* k4 v, Q- Y
- void moto(int Speed_L,int Speed_R)
$ q' Y( i7 ^9 V4 V' y' A+ R% P- H: h - {) I& C& |1 J3 l7 @: W: U* ^
- Encoder_2.setMotorPwm(Speed_L);
- H) J l4 D: L8 M4 j - Encoder_1.setMotorPwm(-Speed_R);
. a7 L5 z% B/ m - }
複製代碼
9 T) t0 h0 ?$ Q/ f- fMeLineFollowerArray.cpp
2 f/ }- Z8 {2 w3 F& v- #include "MeLineFollowerArray.h"
+ D$ O$ |3 h1 O6 P$ c - + U6 K1 J" Z0 y! ]) N2 v
- #ifdef ME_PORT_DEFINED
. r5 J1 ]# J+ W& { - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
* q$ q2 O5 \, a - {
0 @: c4 v; G. x7 S; {+ a$ _
3 B Z' P5 [% E. E( ~2 `. q) S- }
6 E1 y! P3 ^! W; e$ F - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)7 f( k) ^- ~+ e# P4 C
- {
: l$ ?: |2 u5 S3 J. w - _DataPin = mePort[port].s2;1 d! @* B+ ?0 C* S9 B% x
- pinMode(_DataPin, OUTPUT); 8 X. ?* p5 f7 h: `. i, v Z
- digitalWrite(_DataPin, HIGH);1 i1 |7 S* }6 v9 D
- }
0 x0 {7 p% x# f; [$ i - #else // ME_PORT_DEFINED" U# e/ r J% _& v) G* |- z5 Z
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
0 {1 p! G$ ~. l N - {" {# q8 |$ w( O- C
- _DataPin = pin;$ U7 M8 \- g1 I7 B8 y. M/ \
- pinMode(_DataPin, OUTPUT); * W. l: J6 ]8 X4 `# d
- digitalWrite(_DataPin, HIGH);
: m8 S& {3 @7 R8 g. z/ V/ q- I p& i - }
* d$ @- O4 t D) o - #endif // ME_PORT_DEFINED
& R2 c/ c% }- H: | - * e$ w- ^& ~+ M# p$ p
8 g+ d( S. X! y+ U0 z- d5 l3 U/ A' M- void MeLineFollowerArray::setpin(uint8_t pin)- }5 |" ^% l) X% S ^/ L
- {
* F Z& m2 a" d6 [3 \9 G9 O - _DataPin = pin;
( I# c& a7 a |% [9 b - pinMode(_DataPin, OUTPUT);
; Q! F9 e! k- k1 ` - digitalWrite(_DataPin, HIGH);
7 j2 a; P3 ^9 z$ C& U4 c3 X - 2 k" s& q4 r( R, g8 s* C0 l+ {
- #ifdef ME_PORT_DEFINED2 r9 f' L0 k. y& Q( x/ S" r* `
- s2 = pin;% Y' [3 `4 x( U
- #endif
3 W8 k* g Z% f t5 ]6 c. d - }4 ^! j* b0 q3 w7 Y& Q+ A7 j
; @, ^; E& P9 p2 T" D( t3 _- uint8_t MeLineFollowerArray::getValue()
- j+ R0 y7 d* L8 x" T. p - {
5 E5 C1 z; D$ y" \- L. ? - uint32_t LOW_level_read_time;0 d) x) Y% K9 O2 P$ e/ S
- uint32_t HIGH_level_read_time;8 q# A* X8 ], p! H: J$ l$ y
- uint32_t time_out_flag;" V# S2 J# m+ }1 H3 z
- uint8_t Sensor_Data[3];
; l/ }/ L( E7 V0 t - static uint8_t old_data = 0xff;
! `+ V4 q H2 f+ D- ^! z' X* F - ) ^' {4 e# k4 [6 ]9 X3 a
- pinMode(_DataPin, OUTPUT);1 g8 c7 x/ F0 A# D
- digitalWrite(_DataPin, LOW);* G# G! V) \; T: t* ^
- delayMicroseconds(980);7 N# J) }+ c* K( g9 x
- digitalWrite(_DataPin, HIGH);% `2 F1 [: W$ K3 r
- 5 E" M$ Z g5 l) {
- pinMode(_DataPin, INPUT_PULLUP);7 Q8 b2 E& R) _8 t6 a( d7 T
- delayMicroseconds(10);
3 v5 e- }4 N5 p: V5 L - + a$ F: q* g0 J8 D; C* ]
- time_out_flag = millis();" f' _ s+ H# c; N1 V
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
, |! B- m5 z4 u1 B: N _
. \4 [ S( S! T9 E9 J7 z% M) N" a- LOW_level_read_time = micros();7 Q/ E4 x5 r5 X. p
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out5 H# d) E; B2 ^# Q) V# g
- {) l; L6 V* X6 X
- return 0xff;2 M8 y5 _. m4 V# E; m: O
- }
' G% \' H# R" N% b+ [! f" v - * T& l/ A1 p" g! f+ f t; |. p
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );2 c4 O6 r3 D0 e7 w
- . c# r* }# h7 c* E
- HIGH_level_read_time = micros();
3 _) ]/ |7 D) y - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
/ V8 Q5 }8 A* A
4 F. d) \5 g+ ] R k- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
- J5 f& w/ s8 W2 p - {
9 G2 J: P3 R8 @$ d6 @* ~% H5 f9 U - return 0xff;! l. I7 b# D, @, a
- }
! E7 ?3 i8 Q% t8 b: s/ k% `
, ^: H f3 m- J5 N9 Y9 @% }) n- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))5 F ^1 l$ \8 o g. ^
- {
# T( Y5 S X; p# ?$ s. O! ^$ Z( C1 w - return 0xff;
5 ?" n" B( t- b t) {5 p - }! l/ s4 f7 o; N- e
) h* T& e3 T* L0 c6 B1 w4 v- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
2 _9 h4 f+ R1 f# v - LOW_level_read_time = micros();
+ K$ d6 a5 i( U$ N3 _- c) t+ J/ s - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level7 [' A r* Y+ N* K
- 6 R: b/ L& H2 G7 e; Q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* g' u }6 q, S8 D6 Y3 G/ n4 o
- {( X, \$ ]3 d, _( x. u4 ?
- return 0xff;
0 M9 \4 J A v: y5 E - }3 b' A$ A4 N9 q3 F" a4 T0 X- l! i
) f8 F1 \. ? }1 e9 k7 f. O- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))* q9 J7 v4 B' f3 B. t6 b" K
- {1 |: r! ?4 I3 G. E4 \( i* Z
- return 0xff;6 B5 z9 i, i. |* }
- }
3 s$ n, u. ~- ^# S - * h& f" ^8 E% s- g' Y% J
- for(uint8_t k=0; k<3; k++)
- U; e% V# i! u4 N6 r% p: [# b - {
- ?3 X1 `" l6 B; V - Sensor_Data[k] = 0x00;
7 x9 P! B: ]# z8 d - . S. f. `6 v# m! z7 M' ]
- for(uint8_t i=0;i<8;i++)
3 U' x) p5 | i" a - {
/ _: c. w1 |4 B* A - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
1 h2 E* ]# z5 a6 d4 M, Z - HIGH_level_read_time = micros();& u1 T* P- T6 F) D1 U
- LOW_level_read_time = micros() - LOW_level_read_time;2 r& Z' E* R y9 F# ]8 F
- 7 ?" Q2 }% i# }5 P T: k
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )/ f8 {8 O4 D7 g; S9 H+ [
- {
5 w6 o+ O ~' T% H - return 0xff;
. Q- X7 u. q4 s8 G+ i8 U - }
6 @$ m& y) J+ m
- m* C! l& f" K. @) L' f- n$ _- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 `: u$ e! Y% ?, {% p" p7 S+ h
- LOW_level_read_time = micros();' ~8 A& a$ y& S: F" L
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
W7 G a) `! O# _1 g$ I: H
2 u) D* \6 Z% @/ n- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 11 k+ i3 \( u Z) c
- {4 T2 ?/ f7 j& e% R9 j
- Sensor_Data[k] |= (0x80 >> i);3 c; W- r+ P" M \' t& v
- }. t* c$ N5 ]4 e7 H7 K# _
- else if(HIGH_level_read_time >= 100)# O: m$ }( B! K9 M( _; `. W
- {
0 L; n9 V' Q4 q - return 0xff;; S" {" `' Z4 z& \( E+ L( _) n
- }( Q9 O7 f/ l" ~, }
2 o# y+ {" ^& l" Y( e# R$ [# D, H$ I- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)2 y$ s1 ]7 F! }5 ~4 Y: _1 F( V
- {
1 B: }! H [; |$ \ - return 0xff;
- @, c' s" d: X7 b4 r - }
0 H5 x8 }$ U5 g# \9 K0 O - }' C' L0 D& B: E6 _' L7 j
- }
$ D7 V7 o+ X* c+ e8 e4 _5 K1 C$ u - ! t; a( p0 H1 D# w; ]3 X; Q
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level( x U6 p4 y; D/ d& h
- HIGH_level_read_time = micros();/ K& _7 P7 C: v4 g0 { |, L7 W& b2 s
- LOW_level_read_time = micros() - LOW_level_read_time;2 }6 ^. V& N: G$ a- R3 G) Z
- 1 m$ _2 T- t$ ~0 |% ^0 _, M% d7 [
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
$ Q$ X4 T! q4 }& `+ ] - {
$ R, Z Y" Z$ ~3 f+ `8 C - return 0xff;
6 K1 a* N# c& Q. V4 z - }% ?0 o6 ~0 J# R8 O
- . W. ^' Q' k2 T* O5 v
- pinMode(_DataPin, OUTPUT);
" r0 D3 Z& ^2 Q3 M9 q0 m a( b - digitalWrite(_DataPin, HIGH);) F- H5 G5 P7 X
- / O/ ]+ Q' E) w. d
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
2 k! m/ T E3 m; C6 g - {9 X, f: H8 z0 }3 X; c
- old_data = Sensor_Data[0];& E' b4 p/ \( _
- return Sensor_Data[0];) u9 A- _- a5 l, a
- }( N: S \6 ^& p
- else+ {# x D K9 J* j
- {* a& I: `6 B' k% o, D1 \% h
- return old_data;: w% S4 x2 E+ s6 l. y7 m* a
- }
3 Q) Z. X) o- X* @% E2 ?- O - }
% O( _0 X9 w9 c* j
複製代碼 " U* h: _6 I- E; T. Q( r4 `
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
0 J" s3 c- g+ q% r( g
. {9 s9 g4 f& w) ? l |* P
|
|