|
|
! q% q! j) E' a
- #include <Arduino.h>
( r9 t" Q r1 p6 x4 f) m - #include <MeAuriga.h> M7 t- g. w5 q4 T
- #include "MeLineFollowerArray.h"9 D. \+ _# \) Q% Q& C4 }
8 V+ w/ x: M: ^: E% Z3 z. W$ Z- MeEncoderOnBoard Encoder_1(SLOT1);
1 c3 `, _% w" L. J - MeEncoderOnBoard Encoder_2(SLOT2);
& ~5 b ~4 q& @$ E) R) ? - MeLightSensor lightsensor_1(12);
7 b- e6 k, v+ ~- V I( E - MeLightSensor lightsensor_2(11);4 i/ J/ B* O& U e
- MeBuzzer buzzer;: ~; A( {0 i t) o: A
- MeLineFollowerArray linefollower(PORT_6);
0 K6 V1 l, S0 y L$ y' \; b3 z4 Z - / a" m' r& C9 i6 I: V' x' F
- #define Error1 1
" t5 V& Z& G2 e, v4 Y, z - #define Error2 2" }" W7 b7 G. c1 I6 o; R" @
- #define Error3 38 X( X; `1 ?5 |/ E: s1 I" n
- 5 \$ } {! x, @$ E: y c
- #define Kp 15
3 r) {+ {2 U6 }4 o: e# G/ M5 Z - #define Ki 0.15
2 i5 B* ]& @9 [7 r* k - #define Kd 0.03/ P0 d+ E5 W4 ^6 v% H* @' D" c1 f( \
- ) H# n2 k, E$ r- T. v" k7 b1 d
- uint8_t sData;( t2 e+ X; R& `' e" {$ W
- uint8_t D1;0 U: X% f, v4 p
- uint8_t D2;5 I5 u/ @+ }$ V6 L* r2 }: @9 \
- uint8_t D3;
2 i) R M/ N# k$ G - uint8_t D4;1 n* r& u8 D' U/ T$ L8 _2 y
- uint8_t D5;# A# s1 J% R; W3 V
- uint8_t D6;
& C9 v' f0 q" ^0 J8 r* S
9 y4 V) U& K9 V% [ Q. X; F; S- float previous_error = 0;
7 t# [$ N* a, i, p- J I; Q - float integral = 0;7 [4 C. `: h# m
- float derivative = 0;
* N4 L" d4 V2 m" A, p: ]6 [ - int Speed = 160;
\) @; A5 m: S9 W7 ?( t - float output;6 T1 }8 C6 K) v! w4 O2 H
- , e) u, b8 f8 r; G' t
- byte Left;
4 j. U* H1 Q t$ z
: o! r5 V" V6 `. }& C3 _- void setup() 8 Y: @* Y9 U+ H4 d" c: o
- {
, B( A9 Q* H+ M0 @1 q2 `5 o0 w3 O - //Set PWM 8KHz
2 P' f4 T) W F4 i( k4 i3 x( K- d - TCCR1A = _BV(WGM10);! I6 A4 a8 [) a! G
- TCCR1B = _BV(CS11) | _BV(WGM12);5 ^$ A* ~" l# s
- TCCR2A = _BV(WGM21) | _BV(WGM20);
& C# ?) L& t7 O# I! n" W) C - TCCR2B = _BV(CS21);
! l2 \) w7 I: h: p) [, O x - Serial.begin(9600);
9 o! l, M/ Z2 l - buzzer.setpin(45);
+ l. e# z7 Z" L3 e1 d* b4 G - }
% f; s2 [ b; l) Q: O- j* z. [
1 M& W& ~2 E/ U# @- void loop()' b! s8 K }, y
- {
3 d4 L2 v3 W- k# E/ @3 u5 q - while(!((lightsensor_1.read()) < (10)));
( K' m/ O! A: H; p - buzzer.tone(1047, 500);! Y4 w4 W8 ?5 T2 ~6 Z' v
- delay(500);
3 W- }: P/ V( E( U9 O4 l - do' F' n5 c5 {( u0 @! c
- {
2 X5 W# d/ {9 S: w - int Err = getErr();
- y( q K/ j' g, E; g - if(D1 == 1)
6 h( d! G7 i& T! T/ i - {& g d. u# H& C
- Left = 1;
. s; U _7 }; m, A: W2 k - }
+ x! F/ {! E' y0 j4 p - if(D6 == 1)- ^$ ~4 n( a" F. Q
- {
8 I& Y( w6 d( J - Left = 0;6 _: J* R( m6 u/ Y% d
- }7 U$ F( V' F- ^# A9 O) K: k
- if(Err == 99)# F' _2 M& y/ |$ q7 q
- {; ~& ^6 M1 E0 B
- if(Left == 1)
+ i# w m5 i2 X( c - {. ^0 E2 V! G6 r0 T, M" J
- Speed -= 5;5 T; b% U( v" [) o C1 M* Q
- moto(0,Speed);. B4 ~6 b2 y7 u2 Z
- do
% A$ `1 v2 _0 o! C3 J9 `6 P - {/ I7 A% R" M, z! E3 P, W
- Err = getErr();
" |- n Z8 w, d - }while((D1+D6) == 0);) Q: {2 n3 i7 X" }1 j9 s* l
- }: m8 [1 S- t" q/ A. N2 Q
- else' C4 n. Y. r. n1 z5 z2 X
- {
4 ?& B& G( x B# d - Speed -= 5;" T3 Y q$ N: E5 h, w# }5 u% i
- moto(Speed,0);
8 L2 }0 n$ v; k! Z2 Y - do
( m- h* [9 W! B3 y& r - {" [% O+ ]0 ?. P) u' }& f2 D+ ]
- Err = getErr();
) z! c: E# R0 }' _8 { - }while((D1+D6) == 0);" Q9 f5 @. V1 ^% u3 C7 [
- }
7 n( ?. G$ ?3 E; N8 d - }% ]/ \/ x( ]* F1 ^/ O
- else2 w# [" R% k5 }, g, Q, j8 k
- {
$ n6 D) l/ y* f - if((Speed < 160) & (Err < 2)) Speed+=1; 4 N' r u" G7 q7 E
- if((Speed > 100) & (Err > 2)) Speed-=2; # A% i) L$ U6 w2 b& x/ u
- integral = integral + Err;
5 D% n8 i9 @: ^. _+ q0 G - derivative = Err - previous_error;6 p; q& F$ |3 _
- output = Kp*Err + Ki*integral + Kd*derivative;
( f; t# j9 I+ j, l - moto(int(Speed-output),int(Speed+output)); j0 ^2 d) T9 E' h0 V) I1 n
- previous_error = Err;3 P4 h ^$ r' A. A9 W8 `
- }
& m. s& N% J* q3 W$ f - }while(!((lightsensor_2.read()) < (10)));( Y/ U0 M4 W5 Y/ k9 }7 r
- moto(0,0);
5 m; b6 u9 g" C- A - delay(500);1 g/ {1 {$ `3 {- C7 b. r. G8 X
- buzzer.tone(262, 500);
; n- L8 @7 y; ?+ N$ [( T+ ` - }
* K2 z- S9 r# d - & J5 T* }' h8 V3 c3 z
- int getErr()
h' m! C H, D e, ~ - {
`+ M; v9 ~8 @ - sData = linefollower.getValue();. T6 P* C. m3 d3 Y/ J6 d
- D1 = ~(sData>>0)&1;/ b$ M/ }- y4 F0 X* X
- D2 = ~(sData>>1)&1;
3 _; m3 u* p- r - D3 = ~(sData>>2)&1;
5 z0 h, y, {" V, ~- c - D4 = ~(sData>>3)&1;
& o* U( F/ ^2 p! [9 E2 L l - D5 = ~(sData>>4)&1;! V" J7 a" g3 t
- D6 = ~(sData>>5)&1;
X$ ]5 J/ s& ~5 b - int downD = D1+D2+D3+D4+D5+D6;, |# G; Q! Q; f& B) b
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);9 c) V! {5 V: l0 L* G
- if(downD == 0)9 f1 t8 `& e, E2 c& l
- {
% x* W0 A6 y3 g7 G s4 M - return 99;/ t3 t9 f6 D, R/ D0 |9 S5 N" L: j+ [ i
- }, }: g9 [$ z7 z* {; |
- else% k, {1 M; ?9 F$ o5 B
- {" R1 S* Q2 j. B5 j% E" e
- return upD/downD;4 }" g3 u0 P3 }9 V5 [6 P& `( R
- }" E' x2 X I5 _. P+ r/ ]; f
- }. T- `6 \- X4 u; |# I# L+ x7 s
- + s) W5 e W- y7 }0 `3 `
- void moto(int Speed_L,int Speed_R)0 g; g1 S1 O- ~0 ~- s
- {! r, h( \ p. N! M
- Encoder_2.setMotorPwm(Speed_L);- n8 ?, p# k- N
- Encoder_1.setMotorPwm(-Speed_R);
2 b" B+ j/ o% o" J" S5 F+ d; R- S - }
複製代碼 2 V: ` V% X2 u' l
MeLineFollowerArray.cpp4 d% u1 e( p9 P# P5 [
- #include "MeLineFollowerArray.h" D2 G. H3 o* w' D+ v0 ^; @
- # A2 f1 ^) h, X5 Y
- #ifdef ME_PORT_DEFINED! ^' l9 C4 J: ]! L, [
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)7 w9 G* {& P. G& R4 A# X! H: U
- {
( p% |4 o& e9 B" w, ] - $ U. C1 M8 g1 N
- }1 ]) v5 W) O5 i( n; R! M
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
9 B4 d. I( g6 N& z0 A0 ]4 |; q8 ^ - {
3 ^% ?) b' B) V( G - _DataPin = mePort[port].s2;6 E% n5 I7 D' e$ i" G r
- pinMode(_DataPin, OUTPUT); ! p( l% A: p% ]
- digitalWrite(_DataPin, HIGH);- ^6 X( E6 L( C& c
- }* }2 Q/ i& d; T
- #else // ME_PORT_DEFINED
1 x1 _/ r D. T) O. N! L2 ? - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
4 u& i, O* g! n0 A0 }) Y) J4 n# U# t - {
6 j( m% I6 o% z9 _7 @& D - _DataPin = pin;
% Y+ n1 C: n5 x* T/ e' L - pinMode(_DataPin, OUTPUT); ' B- Y' {, J1 b4 n4 d
- digitalWrite(_DataPin, HIGH);2 I$ R. ~) Z+ \. h
- }
" }1 Z) P9 U1 I J b: n6 Q+ e6 j6 U - #endif // ME_PORT_DEFINED5 P+ P8 P" X+ Y; I! M8 t
A/ E0 `0 P- T7 r% c+ T9 c
; l4 n" P$ n0 r P: n5 j6 R$ l" d- void MeLineFollowerArray::setpin(uint8_t pin)" q E/ U5 e, `2 X6 ^4 P
- {
( R8 i; d8 U; b1 i2 { - _DataPin = pin;
4 @& E1 I/ j1 ]' M; N: k, x7 x( ` - pinMode(_DataPin, OUTPUT); 7 m( J. }( W8 u4 L* N
- digitalWrite(_DataPin, HIGH);- _" {8 }: @& r7 l4 v1 ?
- ' b2 L$ d9 K& h
- #ifdef ME_PORT_DEFINED
6 R1 a/ B( Z0 v# m4 q1 F - s2 = pin;( q/ W7 S. D$ D1 A% d; A& E6 o
- #endif' p& `5 i: R1 p9 [ @
- } W1 _, G, b0 Q. V
" Z# i) N0 O; v* g$ r. z- uint8_t MeLineFollowerArray::getValue()
1 K6 O. w9 o/ u& q- S1 s" v( T - {
1 {" l7 e) [" d; ?! W# B' j' V' {& t - uint32_t LOW_level_read_time;. Z$ {- D+ ~4 R. a
- uint32_t HIGH_level_read_time;; {! |" ?( S$ c1 R1 r: E4 K4 S
- uint32_t time_out_flag;
+ H8 ^! \4 ?3 }" h7 ]! O* i; | - uint8_t Sensor_Data[3];: @- K9 s$ F6 G9 l
- static uint8_t old_data = 0xff;
' a( ^6 p, m, d1 V6 Z% ^+ G - & B4 I& G A) @
- pinMode(_DataPin, OUTPUT);
6 \4 b1 d4 m. j; N - digitalWrite(_DataPin, LOW); f4 h8 o4 k) T$ G3 ?
- delayMicroseconds(980);0 \. W/ l4 k2 o7 |2 Q
- digitalWrite(_DataPin, HIGH);
2 a _4 F# C$ E" e# Z - % E* p c; ~: k' b+ j3 G
- pinMode(_DataPin, INPUT_PULLUP);5 ?' P7 K( y I. ~6 ]9 e
- delayMicroseconds(10);! S' M! F: s' m
- 8 @1 X" g ?: t
- time_out_flag = millis();
D& L7 L0 E: O6 x - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );! o8 H' l) g; u! @4 O* N
, {3 q0 N; W& h7 L+ d- LOW_level_read_time = micros();1 N# r! D8 q+ w! [. u" k& H
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
& l3 m# c3 E/ w% S% {) ?( ` - {
1 W/ ?2 ^) V1 D - return 0xff; J3 |2 L; S0 @% r
- }
$ o5 ?8 e5 D& K* v
/ S' \8 u( c4 }9 Q, d) X- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
) J/ T: C6 g! J. B/ I - ; {7 M( @. [( b. |* j" j" {6 M
- HIGH_level_read_time = micros();
G, \* r* X( L# p - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level; L8 o5 Y* R* o# M. W
- $ p& x! x0 E, d+ A
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ J4 t2 A u; ~0 C8 Z, D5 t: R
- {0 a# R9 O# g$ p8 M! w/ f5 g
- return 0xff;# y( e" H3 Z! R. _* M8 q6 I
- }" H: T) E9 Z- n4 ~& @) v
: o8 @: Z4 c. h9 ?5 D- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))' c9 A" G& o0 G3 y; |6 E, U% ~
- {
( Q5 y8 b& K1 M) Q- y; E7 h& P4 { - return 0xff;$ j2 X* B% m" _. n, b1 ]
- }
# z% X' C+ D4 V1 y" ~1 e - / i0 Z4 D$ G: _8 N
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, ^9 ^5 S( y# t* W4 a+ Q- ^
- LOW_level_read_time = micros();5 R% y0 T; E' B0 V( s
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level7 ~' Y7 t# G1 k
- 4 B0 `& m5 p" M# W
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
, ~1 d9 b, w Z) g" } - {2 f7 D1 h. ~# X
- return 0xff;" I: S- M" F$ o3 ]$ R
- }
. Y, _ X+ b- M
) b2 M. V9 L8 ?' b- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)); P& e! C( q( Y6 k3 Y
- {9 l$ l7 p& S) Z: p' B
- return 0xff;6 V7 [' b! M7 _. N0 f
- }+ G3 Y' f1 Y& M8 a& k
* g/ r+ d( W# p6 ?- for(uint8_t k=0; k<3; k++)
+ _ K1 O9 J8 A9 B - {* o+ _* x7 G6 q" w/ u2 p" z; ~
- Sensor_Data[k] = 0x00;
) \- ?3 y' ^" g( e* j, g
* D9 K5 ?4 K9 w- for(uint8_t i=0;i<8;i++)
8 w0 }, \" F: O" I$ H - {
7 q8 ~5 F5 \% F Q a& w8 w( G# k! ]/ O - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level7 P W' ?7 D4 }4 O$ o3 i
- HIGH_level_read_time = micros();
; U6 P ^3 Z( H - LOW_level_read_time = micros() - LOW_level_read_time;# K8 \! V, i ^, T
/ d+ i7 _$ j% Z, c3 m( A- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )% \" Y% S: C z2 g" s9 V
- {
- I7 E, [7 n+ o3 N( m - return 0xff;1 k9 h( m+ a. B6 w
- }
; e" R5 ]! C3 `# {1 ] - - ^5 U) \2 b5 B' y& N8 P
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 U. R9 k% ~% T& c k! Y2 s# e
- LOW_level_read_time = micros();
! M/ r5 @6 t% F9 } - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
4 J; k/ _" D# z0 r
6 ?2 ^4 g* }( Q: j- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
+ k1 v r7 O2 V, S9 E - {
- R& I7 ^; ]% k2 q7 @# `( Y - Sensor_Data[k] |= (0x80 >> i);
~1 a6 E8 Q! R; o! L3 E/ Y - }
0 i2 W2 N- }6 N+ m8 S; m - else if(HIGH_level_read_time >= 100)
- g$ `7 l, a. w3 V2 b - {
$ L0 o' d" L2 f$ @! X - return 0xff;
8 E; H4 Z F l: u/ q4 a0 e - }
$ c$ P- ^5 C, s
/ U% u7 C9 D6 f- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)7 A) z! u- ?, X. M) X
- {4 w* Y2 i0 G" r) M2 Y8 X* [
- return 0xff;
, w/ d# D% C- L; m - }
' ]- c" d, g, e! Q, a7 J+ ~5 H - }3 F: e. [3 z2 M7 A8 Z+ @
- }4 D2 ~. F; n/ C2 ~4 r
- % m4 b/ f, v5 A: ~
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level8 S F; i Q. H7 T! R8 b9 q9 g
- HIGH_level_read_time = micros();- J# q$ r. P1 P9 u- F
- LOW_level_read_time = micros() - LOW_level_read_time;
8 j. C5 `% I- b2 \* a! P, E - ; L* W" X0 m# X$ O1 P0 K' Q: c. B- A
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
1 K! r% s% v" H) L* F+ B - {. F( l B' s( ~6 M
- return 0xff;3 w) h! m/ W6 ~1 f: k# b
- }
& M) s: c c8 R* f8 z4 M - 8 U2 J" J5 m" n# o1 y
- pinMode(_DataPin, OUTPUT);
" Z( R' |4 T! v4 p - digitalWrite(_DataPin, HIGH);
" }/ f# ?, X' [( k( ?- w
4 z8 V) L1 r2 ^- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
7 y+ p) g% D+ S5 d3 Q6 K! z' |* E( x - {
6 R+ M' Z/ x: n5 n - old_data = Sensor_Data[0];. K' \. q! o7 Y5 T; A
- return Sensor_Data[0];, x1 E: R0 [, z& Z0 \! g
- }
, w0 V S' ~. } - else, G& m7 l! B9 E6 F4 E
- {
4 c( ^6 Z7 j4 q: Z - return old_data;; O" n- U$ _. l% T' Z' I2 m2 Q
- }7 n2 I) c8 `. q _4 A
- }
8 ?* N7 {2 o) X4 b% H# g
複製代碼
8 v0 U' M+ t/ q& [! cMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
: a- J: i8 m) Z/ A* i' F$ ~, i* g w7 K$ U& E
|
|