|
& x; ~0 m4 t* R; j: E- #include <Arduino.h>
1 n% h! K& _3 J0 _" k1 t" ^ - #include <MeAuriga.h>0 E7 t( @6 S" X, f1 J6 b& Z
- #include "MeLineFollowerArray.h"" w$ N* @6 C" F% Z m
- ! q' Q# m R$ \! o2 X3 d K
- MeEncoderOnBoard Encoder_1(SLOT1);
! m5 U5 k$ b9 _. e( J- [0 _7 Y - MeEncoderOnBoard Encoder_2(SLOT2);5 N" [) t4 W: p% \& e* e! G# c6 d
- MeLightSensor lightsensor_1(12);- }, C% S" \- A, k) L
- MeLightSensor lightsensor_2(11);
7 `; `+ h; a; S! N1 i X* C - MeBuzzer buzzer;! g4 u. [& W4 K7 p
- MeLineFollowerArray linefollower(PORT_6);
, I% N. ?" m" V$ `* O) x$ a' x' c/ `/ v
5 z' T5 K3 l3 ~" w5 c# c- #define Error1 1
. j/ l- ?7 V7 S( E) C; J - #define Error2 2
% C7 R8 E$ _* e - #define Error3 3
* N8 J K: K* K5 w
/ J2 G, W" S" h: L/ Q- #define Kp 15
0 t% ]7 r8 u( K2 ~ - #define Ki 0.15% g1 w: ]# Y* q2 z( Z
- #define Kd 0.03+ J$ }4 v9 \/ A# t! a9 q
% |! D% j- F* O% c; |- uint8_t sData;
5 p: s$ a6 v% \& h2 a - uint8_t D1;
8 S) n4 b6 Y8 s+ l# w& e; D: P - uint8_t D2;5 e( u$ x/ @* O7 l+ c" V
- uint8_t D3;/ Z2 R/ T/ E/ U, h# N( f& C& z+ L
- uint8_t D4;
+ J+ Y) S( g" a, R - uint8_t D5;
$ Y, i7 n2 {2 e/ p5 X& G+ w: r - uint8_t D6;
' m7 `6 M6 \5 ^: R1 S
2 d1 h' U. q& N' y- float previous_error = 0;
( X$ B0 }3 }. ^% u7 Q; T0 | - float integral = 0;$ @2 ^ M2 g/ V
- float derivative = 0;
' ^0 [. R3 f7 Z% P$ g$ W - int Speed = 160;
z) s7 m+ x' h - float output;
+ g6 X/ ~! ?+ R) W* E- h - p( ~# ?3 F: `* [6 N7 _) x+ p
- byte Left; O- [! M) D7 ^% l9 p; M+ j
- ) N. a( Y5 g# Y( z; h
- void setup()
( |4 D5 q! u4 c, Z" p$ r - {
! J/ j; D* U; @* I u; E2 t - //Set PWM 8KHz
6 ?7 g. m1 u* H7 `- W+ X6 N# i - TCCR1A = _BV(WGM10);
0 S' |1 v/ q. Q4 A4 y8 v - TCCR1B = _BV(CS11) | _BV(WGM12);
7 B3 _8 h- u8 S& K9 A' ^0 q - TCCR2A = _BV(WGM21) | _BV(WGM20);
% {+ x: [( Q4 V: O) I% s7 \7 z. i - TCCR2B = _BV(CS21);! f8 ^$ Q0 @* t$ b! m- ~: l q
- Serial.begin(9600);3 E. ^" T4 Z, y& {4 i
- buzzer.setpin(45);
" \3 C" z" R1 u' C - }/ r7 W/ c- d5 p. _3 {& j G$ S N/ X% |# _
- ; q% |0 k5 T. i5 H
- void loop()' u3 r$ F+ ^: ^) [% `0 C8 d" H/ e8 T
- {
4 [. J c$ t. c, I- l2 n) r - while(!((lightsensor_1.read()) < (10)));: I8 n; N c2 \
- buzzer.tone(1047, 500);6 F, G1 F, d* x _
- delay(500);
8 n' g) b8 [4 e1 X - do
4 Y, _" W) j% _+ H: N6 m' ~ - {
2 N0 d+ L# v; t - int Err = getErr();
( A. `# L5 S0 A. z; s) ~! F$ D - if(D1 == 1)" Z% J; a$ K) ]; `# ]2 w! ~8 t
- {
& e; v4 L9 C" _% u6 ^% s& @ - Left = 1;
: B( b8 l V7 r d - }
* @9 E# V8 Z( q7 O( i: q - if(D6 == 1)
8 V5 k5 `: H* V2 o3 @6 n3 w - {4 M5 W/ ~- J" [+ l# Y8 [" I
- Left = 0;
$ R& n' K( F W8 v& \ - }
+ ]2 p) j0 S i# r$ j - if(Err == 99)
, j' N6 O2 l' W1 }8 l - {
) q: X$ A. e7 Q0 D+ Q" W; }& f - if(Left == 1)4 C: G d* L9 P
- {* g0 _6 G, C. `5 m+ O
- Speed -= 5;; G# V+ x+ m, Z
- moto(0,Speed);' v3 \+ R/ W" N9 k' o1 u; ^
- do; {/ H% P4 u% ] I W6 W. S* K
- {* ?' |% j* ]6 k, e: u3 T% n1 D; d2 p
- Err = getErr();5 \9 U9 k x% N, n( @2 j! R+ L
- }while((D1+D6) == 0);
: X# e4 R$ B- R - }
: i) g4 g4 x" X; d - else
S4 j! H$ s6 k$ J - {
F k9 A: k0 k( M8 v7 T - Speed -= 5;+ D3 f4 t8 a# w& _! q; ?* C' d
- moto(Speed,0);
8 I! H$ T) V/ V# @, j! V2 b$ A - do
) t0 L" T1 y" Y3 J; L - {% Y q) D2 v; N% o
- Err = getErr();( a' K6 m$ ^9 h5 R
- }while((D1+D6) == 0);
! f- z2 K8 E4 w/ l! n: |) x - }
0 b- u; A7 }; E$ m6 a - }
' c. f: x$ y2 s3 }' ] - else% g4 p) h" I$ }/ e7 o
- {
. s2 j+ U+ F, ?2 l2 C! K# P - if((Speed < 160) & (Err < 2)) Speed+=1; . O3 L( X4 F, ?' n( ?0 a
- if((Speed > 100) & (Err > 2)) Speed-=2;
. J5 x% M0 d! Q! F% `$ d$ j Y* @" C6 ~ - integral = integral + Err;$ ~' K, c: _ n0 f
- derivative = Err - previous_error;; f' Q9 b/ ` u" m- i: m5 X
- output = Kp*Err + Ki*integral + Kd*derivative;" e3 @/ w5 G5 n" h0 N/ n
- moto(int(Speed-output),int(Speed+output));
% z. B# L1 E f5 |5 O- ~ - previous_error = Err;
# y1 c: K, U3 x" b& z/ }1 T6 J6 r - }# W& A6 p, A# l9 M7 }
- }while(!((lightsensor_2.read()) < (10)));
$ G+ H9 G% }" }" t+ A2 Z0 x - moto(0,0);" L1 \( Z; _, P p- {
- delay(500);
( P1 |- E' ^3 |. K3 ~" I [8 l) C - buzzer.tone(262, 500);
3 y7 _- X8 j$ ?. X c - }
% N8 p6 i, a; G; y - 9 ] G) v0 m# x( M0 k+ x
- int getErr()
/ j, ^5 ?# _9 y) t4 ]+ O - {
; J: d4 ?) g6 y1 S - sData = linefollower.getValue();
4 N& X1 g; o- g, n: c - D1 = ~(sData>>0)&1;7 x; X% I2 ~ m* ?0 ?3 A4 `
- D2 = ~(sData>>1)&1;
- n+ l: n/ L o - D3 = ~(sData>>2)&1;
- ^6 H; K0 [2 S% w7 ]4 v, n& u - D4 = ~(sData>>3)&1;3 Y* g: J+ j5 p$ t
- D5 = ~(sData>>4)&1;
$ A% P" K$ e2 a/ Q: L/ W - D6 = ~(sData>>5)&1;
# p$ \" @- B& R$ u6 G& M - int downD = D1+D2+D3+D4+D5+D6;
2 R/ L& N# {4 i1 ]; P2 j - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
4 V# h) u5 D7 R& U, O- ?: O' Y/ g - if(downD == 0)
9 T) T2 M7 ~6 W, b5 W0 W) l - {/ n7 L. W1 ^9 o& {
- return 99;* u/ J) R9 ~) P
- }
: K2 y1 k! o2 j. U2 q% v8 M& r; M - else: U6 j# h6 P& W; i b. H/ T
- {5 O$ T) w7 f7 Y; z4 C
- return upD/downD;8 {' W$ q9 T7 }+ Q; ^* M8 i3 a
- }
, Z! w3 A% M0 D- I% B - }
) t0 H, j* s* T2 L: D0 Q; v! u1 W - * q( ?: t. b( W5 Q; @6 s \/ S4 O
- void moto(int Speed_L,int Speed_R)
+ O# Y, d! Y, E' e8 c+ b - {
9 Q, k& Z) b( q" V - Encoder_2.setMotorPwm(Speed_L);# q8 @7 T" Z+ w% J) F: m. [% S
- Encoder_1.setMotorPwm(-Speed_R);( H) T! }5 K7 a' ~0 d
- }
複製代碼
% |4 C- {- s/ HMeLineFollowerArray.cpp
( B$ K! R+ r U. h) i- #include "MeLineFollowerArray.h"
; e. z1 o3 e" E6 z - ; P4 X& j& z9 A$ t" w3 V1 Z
- #ifdef ME_PORT_DEFINED" n h4 z1 P' M3 \ W1 r& ^* ?
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)! k8 a$ w- [+ z0 U. g, E$ c, F
- {
|6 |' L, @7 o/ U. X
+ o9 N6 v0 f+ C9 O6 E! N- }
1 q1 D) M9 s$ x! M1 u* K. [+ e' R - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
4 O9 ]" a {( t& V* f - {# k j; e! w/ x: H
- _DataPin = mePort[port].s2;
& s' {" {- l# G, f/ [ - pinMode(_DataPin, OUTPUT);
/ l, D* o% X* G - digitalWrite(_DataPin, HIGH);
2 ] j/ M& ]. Y% y - }) ^; v8 @% W9 \- U8 z \5 n$ K5 ^
- #else // ME_PORT_DEFINED& ~* y$ z* H( t: `2 }# T2 _
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin): b4 ^6 J: m3 E# Y9 K1 l
- {9 }: x' M# g$ O' m
- _DataPin = pin;
0 ?* W1 @# u! [5 j/ ?; t1 W. S! Q, y0 x - pinMode(_DataPin, OUTPUT); 0 y- ]+ C9 ?$ k7 w6 b
- digitalWrite(_DataPin, HIGH);3 n- H) t& N1 L8 {! C
- }0 i T9 T, L7 ]1 `) g! Z0 l
- #endif // ME_PORT_DEFINED
2 o6 D# K1 R/ e7 l9 f7 R" G$ l& a - : V: g: r, B8 _( M
- 2 Z! z9 t, d' C$ |% v3 F
- void MeLineFollowerArray::setpin(uint8_t pin)* `* h! W7 R% x! x5 q2 ?
- {
& \0 t- t1 N! x. I - _DataPin = pin;
% z9 J; E, C7 P; h' _ - pinMode(_DataPin, OUTPUT);
( v b8 R/ w0 s1 n, Y2 v! n( e - digitalWrite(_DataPin, HIGH);5 y) ~0 T5 ]! K+ j# L
2 T1 N6 p) N! f! s9 U- #ifdef ME_PORT_DEFINED
- d- I( M1 d% O2 O - s2 = pin;: _+ x) v5 j7 b
- #endif0 W" z2 z4 N$ X
- }6 ^) Y* |" D3 W, W
- $ T9 y3 k) h) s# O& w/ ~
- uint8_t MeLineFollowerArray::getValue()3 k& P7 G3 s0 M+ l6 i* b# B
- {: t" `. [# Z# O; J w5 u
- uint32_t LOW_level_read_time;
/ s0 T/ _' R |- @ - uint32_t HIGH_level_read_time;
4 k% a# k) ?* l* S6 j: F9 h) H" ]3 | - uint32_t time_out_flag;% z ?2 P; }4 M
- uint8_t Sensor_Data[3];; I8 o' t2 w: Y7 w: n
- static uint8_t old_data = 0xff;. P! q5 a4 T8 D/ G2 j
- % x: q1 I& y0 I8 P* J4 l" S8 g
- pinMode(_DataPin, OUTPUT);
, P N- W7 Z& |7 W" W0 _! W, N - digitalWrite(_DataPin, LOW);5 m8 R! g0 D0 J G v$ A4 W
- delayMicroseconds(980);
& ~3 t, N- q! C& p - digitalWrite(_DataPin, HIGH);
D) s! Y2 ~7 C q A+ G - ' x/ x8 U. C" A% G. X
- pinMode(_DataPin, INPUT_PULLUP);% N* T* [, O$ n) J
- delayMicroseconds(10);
0 b, x }$ _. `! T, s* C
" S0 [5 |% R% W3 ]7 D- time_out_flag = millis();
0 x" H/ p( J8 G# `- [8 u, L& ` - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! T: _4 n$ d. L- c* e. K8 L
7 t$ W- e, @2 O" U$ t- LOW_level_read_time = micros();" R8 n; i4 l( F% D7 B
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 o u0 Q- B/ A+ q, L3 e
- {
2 t3 \5 S8 V% p; P- D - return 0xff;
4 t) f) ?" J0 Z- E6 r - }
/ y! i% Z$ |+ k2 |3 j2 F - . ^ r, {! g- k j$ u( g7 q; n/ Q
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );5 f5 D4 _/ ], l0 c
- 3 u9 E F( z8 X3 E2 T% V
- HIGH_level_read_time = micros();; ~( e& ^0 ~8 x8 r! [
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level( v+ l9 i7 q& X& s/ R( @0 C& m
8 r2 v! P# Q/ U0 N- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out$ d2 I" d: n2 c3 S6 T* [9 O9 ~
- {
9 Y% G+ x ~: s - return 0xff;
8 l, u' Z2 q! n' ~ - }& l! O9 K& V" V% C7 ^
$ m& Y8 r$ s6 g7 Q0 ?- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))5 E- ]. x- {7 U( N+ s
- {
( L; @: r% Q5 I$ V# @* D8 ` - return 0xff;
" N8 R; Y( F4 E; E' u8 I! m - }
1 c5 `2 A0 J9 R9 g. }/ P
$ E6 {6 y. r Q* B0 D! @- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( a) R5 v+ j1 L8 R0 R& H - LOW_level_read_time = micros();
, x" A3 Q9 g( i# P - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
9 M/ L) ^/ [' S4 c% u - 5 d# l) d3 [( ?) r7 F3 B+ Q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
" d2 |" V8 V ~! V/ h - {
! ]" \7 \, x3 d2 t- d$ o0 d: K - return 0xff;4 Q0 p; ]3 Z8 `2 C4 H' e
- }) t. w" k9 S& v2 q, D, v
, Y6 ?/ p+ r% C. }( f- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
; f: M1 U. r+ W/ E - {
8 ~. h' t( [" H3 ?9 n9 k. U& a7 b - return 0xff;
9 Z; P7 G7 ?: o+ E - }7 y3 F h* b8 c0 @* S( d; l5 `5 N
/ }6 {* b& v5 \8 H- for(uint8_t k=0; k<3; k++)
5 b" M0 Q: U# Y2 L# ?$ Y - {
- {- O9 o! }+ @: V3 G - Sensor_Data[k] = 0x00;' w* |3 }* |9 j: Q' [6 g* N: `- P
8 f4 p* [* r1 x3 i4 y! C- for(uint8_t i=0;i<8;i++)
- }1 @7 |) T# V" o - {
- z1 |. ^/ k; _+ | - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
, x( a4 `" f d; [: y - HIGH_level_read_time = micros();( H! X" a4 f. O
- LOW_level_read_time = micros() - LOW_level_read_time;
3 U* F3 a R0 W
( p1 h" r; L. N+ S/ p- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )+ G" |. O" @1 P: {. x4 t
- {
/ F. v Z1 U& `/ g - return 0xff;
8 r% E5 A: b/ D: O5 ~8 [ - }
& d) O6 ~4 A8 R, B; v5 ]/ `8 p+ t/ x - ; z. e* Y7 o: O1 U1 _ ~$ n
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );4 G. p1 i, Z! l! K
- LOW_level_read_time = micros();5 v+ E K' g) l* L2 X) K* F
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
, z( c Z; K- Z$ d& s( E - 2 m4 \1 K! h) M, D4 Y
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 17 \- R2 }1 E3 k3 G! z, u& x$ f
- {
+ T, w& K! l' \$ n# m7 c: _ - Sensor_Data[k] |= (0x80 >> i);
/ B5 V5 R5 \ M' P# m+ T9 G - }" R4 j* @4 j5 J( A
- else if(HIGH_level_read_time >= 100)% C8 A' N4 y0 o |) _3 V
- {% [7 q( K9 R2 G: n* u
- return 0xff;( k" I9 `5 r0 U; B5 Y
- }4 L" I& D% l- R
7 c! m: o9 I N# F% J$ h' H: Z- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
7 O, a& z# b$ `, O - {& y' J+ z# S1 i! ?4 P# m
- return 0xff;
! y. m) u- u8 { - }2 r0 {& k9 [6 \4 c
- }0 e$ O6 u. l3 x( t4 D% m2 e( u& C
- }) x& F5 Z( t) ]* V( W9 o1 l+ f9 ?( g
( T0 y- S4 e6 U8 t- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
7 e, h3 Z, P) B7 \3 y: @. j - HIGH_level_read_time = micros();
, m8 M: N4 \* O2 _ - LOW_level_read_time = micros() - LOW_level_read_time;
# O2 ]$ h9 q/ v7 F# K) n! T/ p - ! Q) _; ?5 }+ x& E
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )1 G* B# i' E* N8 \; n% k
- {$ z4 l1 s; E. u( m w, I7 @
- return 0xff;; J4 N0 S* l: I) R
- }
* E* }2 b7 E7 G - # v. w: A. }- |, E. I% L
- pinMode(_DataPin, OUTPUT);3 [$ X9 k7 V- D3 i( q
- digitalWrite(_DataPin, HIGH);
( c) s4 c6 y- a) d( w# |; L' K
7 M' Y/ y1 D: h3 `" C- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))9 R" a- u I4 E5 c1 y
- {
" H |+ O% N/ H) k. q$ g - old_data = Sensor_Data[0];/ s! J z9 @ S: b
- return Sensor_Data[0];
: G( Z- f9 X% z' b# |5 E' ?& l$ p - }9 q/ N# T2 B! |! s3 n
- else- J' o0 G9 c* ]! }- y# Z* b4 `9 b
- {
# j$ l5 T- x" L2 M8 l3 O$ u - return old_data;8 g4 l' Z6 ]0 V
- }& K+ o c0 E' V! ?! a7 e* B
- }
) u" ]' u7 @2 [5 X- ~
複製代碼 9 Q& _$ @4 C; N, V
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
/ I1 k1 j" W6 |$ X2 C# V
& a* T1 A" D* x& I- {
|
|