|
|
3 F! k/ |; N! X* |+ T9 b
- #include <Arduino.h>
% H/ N" e$ N% c7 r4 `* m4 k - #include <MeAuriga.h>2 `% r% @7 y$ s8 S, H
- #include "MeLineFollowerArray.h". i0 }% }- v: ]% q, `. F
+ W& K0 y4 q# Y w2 c- MeEncoderOnBoard Encoder_1(SLOT1);
9 t" I0 {2 B% q - MeEncoderOnBoard Encoder_2(SLOT2);
- H+ p! P) S9 S! X/ ?( R8 j7 O4 a0 g8 D - MeLightSensor lightsensor_1(12);! s' j5 `5 _/ A( ]
- MeLightSensor lightsensor_2(11);
3 n/ j: d z7 \4 s# G - MeBuzzer buzzer;
" B& R; `' T9 Q( m) W6 a! s7 O - MeLineFollowerArray linefollower(PORT_6);! T% |; A& |& x2 ?4 f
- ; \6 t$ M" K) J& V- s1 h$ u6 g
- #define Error1 1
9 e1 [8 v/ M! e2 o$ ]0 M+ N) X - #define Error2 2$ V# I& i( ^! j. @6 y7 G
- #define Error3 3
- A5 f# ~! Z2 t/ y3 |( h
* D4 [+ x$ C0 x) P6 D$ z- #define Kp 15* }: ]; a6 X2 \; j) D' M
- #define Ki 0.157 e8 K& r7 h J' E, Q* Y8 a. t
- #define Kd 0.03
8 E+ ^! F/ s1 D& t; \0 d; r - 5 B) }* a2 T7 a9 `$ l* b( }
- uint8_t sData;6 [0 p( ]: J# a" c$ O
- uint8_t D1;
( ~' ?7 y& x( u4 `" w% _* } - uint8_t D2;% A% w" M* P2 v2 B: w0 d+ M
- uint8_t D3;
- l( |0 `& ]; ]8 C - uint8_t D4;. u. f% |0 c3 U5 O* f$ u
- uint8_t D5;; D) g" X# q. s! U, h! T
- uint8_t D6;
$ G* v3 u3 e6 m! D4 A9 x. g, v
0 |. r" M4 c1 M! h+ q, W( Z' m- E* J- float previous_error = 0;
1 h; y: Q" U* v) l. _! n1 d# k - float integral = 0;
- k2 R9 ~' O& `5 M5 v- I9 @ - float derivative = 0;
3 }# y! q2 F& D* K/ Z; k - int Speed = 160;
% j1 c! n+ O' S) K! a& @+ f+ f - float output;
4 K0 A7 ]" x) v* w - & n- f% G8 R8 y) H" z) V6 n3 @
- byte Left;. j- { E9 }9 x. M' z
- % X! P4 W* S* H2 W2 y
- void setup()
- z* c5 g+ A( I& i - {7 u- {8 s0 a; p; v! G: u$ H0 E
- //Set PWM 8KHz
% ^" b) p4 I6 o! Z% D! Q' E( K7 L - TCCR1A = _BV(WGM10);
# A. i, U- g$ @6 B7 m: C! V - TCCR1B = _BV(CS11) | _BV(WGM12);
. u4 ^/ z2 K/ p3 i2 x, I8 s. d - TCCR2A = _BV(WGM21) | _BV(WGM20);; ?2 g; B9 G% w& o, I
- TCCR2B = _BV(CS21);
2 c) t0 a) d+ x+ S1 p - Serial.begin(9600);9 ^. z6 R# @- F
- buzzer.setpin(45);
2 [5 q; e5 y# K9 G: s" ?- w! B, O - }
0 |+ \' l* L# J* }
( f9 G6 O7 G: W$ P" p" Z- void loop()# U7 w) d' C# X. _2 q* h$ x6 }0 F
- {
/ K! U+ W: I# h8 ^5 ?) L - while(!((lightsensor_1.read()) < (10)));" R& K; P3 v v1 H2 n
- buzzer.tone(1047, 500);& B* f; V, k* f' D
- delay(500);+ ?% a* O: n! V" c7 N
- do
, i& k: t) k% h" R0 U - {- V4 d9 |/ I) r" \8 w$ ?
- int Err = getErr();
4 {- j( x' d+ D0 O0 M: r( \. x - if(D1 == 1)
5 X) F9 Y- t/ T/ P% Z! ^, C, k- k - {
: M+ P4 Z: e9 n - Left = 1;( s) L: z* I0 O& o7 c$ I
- }
) I1 J' F7 C# Y# S0 R7 G: j) n - if(D6 == 1)6 A" C: ` m6 C- q' V% q
- {
1 g. B2 {5 \) Q& p' b% ` - Left = 0;
7 J/ o; g; ^6 v+ g' U/ { - }( e7 i" W4 X: ^; j1 d( d
- if(Err == 99)/ G- ~2 U2 g T% B2 v/ ]
- {- k* q& P4 @1 r% M/ h# N% ^
- if(Left == 1)
3 y8 ?4 Y! i+ Z - {8 e, l( s% ?0 V( ?
- Speed -= 5;
' l; R" F1 s1 \2 g: B8 Z - moto(0,Speed);
( A, c( b, e( j - do
& g; [ O) U4 [$ r6 ?* { f - {
2 M# O8 l' n+ U6 ?4 T/ e - Err = getErr();
+ e. \1 S# |; F - }while((D1+D6) == 0);" ^% ]' C' F; ~
- }9 n3 a4 v+ P% C# F |1 X; T
- else7 k( v4 A! ]* r+ _& P: G4 _1 y
- {' [5 ?$ |( ^8 F
- Speed -= 5;- C5 Y8 u. i2 c1 t6 f
- moto(Speed,0); $ k5 h6 F8 ^& Y1 v! J* n
- do
/ j4 ?" h5 \- g8 Z - {
. q) J$ E8 }: e: j9 Q - Err = getErr();' O+ I4 O, O4 {( t& [3 Q, m% m/ J
- }while((D1+D6) == 0);: d8 H! K3 j' }/ @4 N3 G. C' Y
- }
; M4 F! }/ E) G) y. \ - }$ ]+ z, w' I" A6 m' }6 n7 E" E
- else
% r* L2 {7 o6 F: J* U, x - {
6 f- F( p+ `8 s% @ - if((Speed < 160) & (Err < 2)) Speed+=1; 8 u* w3 r) `) z. ^( Q7 c" l
- if((Speed > 100) & (Err > 2)) Speed-=2;
) ]4 S0 {! U% J. S6 b5 b - integral = integral + Err;
/ P& C0 B Q! J8 N" w - derivative = Err - previous_error;
- N5 z! ^" a7 T9 U! D - output = Kp*Err + Ki*integral + Kd*derivative;7 ?- P9 s& }/ c! H- ?% ]) k
- moto(int(Speed-output),int(Speed+output));( {3 m( U7 P4 o% ~1 j y
- previous_error = Err;: P) R, U, ~0 {6 V x. d
- }
$ A# \4 _; I, J - }while(!((lightsensor_2.read()) < (10)));
8 O1 F, g, M( p - moto(0,0);
5 g9 p/ C, K4 H+ H! H - delay(500);
" x p8 F+ ]1 ? - buzzer.tone(262, 500);( ^* m% _' j# n' o; V# Z5 r: D
- }* P, L( Z* [2 x) ]
- 5 M) o* W! {" m ~, G$ r
- int getErr()
6 q2 s, l7 A) R: c' B5 K - { + Q( D, b4 r) r. x! K: S
- sData = linefollower.getValue();1 r/ |; `- |7 r/ X9 H1 T
- D1 = ~(sData>>0)&1;$ _# p9 W$ Z+ G! N. r% U( X* d( A
- D2 = ~(sData>>1)&1;
0 C% O. m& y3 |3 N$ b - D3 = ~(sData>>2)&1;
% q1 a# Y+ k8 Z$ p7 B4 u - D4 = ~(sData>>3)&1; f9 Z0 b0 ?% H+ ]* D
- D5 = ~(sData>>4)&1; y7 z2 a, \* a
- D6 = ~(sData>>5)&1;
4 }+ |+ {+ o) I - int downD = D1+D2+D3+D4+D5+D6;
0 }- I2 H& ~, ^* ~. ?& w2 U - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);6 r1 H3 d" A6 n; [3 n
- if(downD == 0)
# a3 G3 J4 O( d3 l$ Z3 n, h - {* p5 i# ]+ [% [/ M0 P
- return 99;4 ?7 {: g. _( G
- }
2 J" J) E0 F: o& u - else
5 {" b8 G7 s- i' J - {! I! Y2 P, Q& D) `0 ?
- return upD/downD;! Q( }' ^& E( D/ U9 T
- }
/ }9 N4 D: m/ D A( Q% D+ ? - }
# B8 h' x! F$ E/ }
$ t" r* \& K# ~( u* ^" G: L0 e$ r- void moto(int Speed_L,int Speed_R)
8 S3 w+ `+ C( l0 I: Y" I4 N5 I0 T - {" q F- y B' {% R- d
- Encoder_2.setMotorPwm(Speed_L);
' A2 f T/ [6 o - Encoder_1.setMotorPwm(-Speed_R);) @5 |$ G. E; E+ ?
- }
複製代碼
9 U5 O' v' m8 S0 [7 gMeLineFollowerArray.cpp
/ b2 w4 c' Q+ ^0 Y- #include "MeLineFollowerArray.h"" @1 \9 d/ k" C! D, J
7 @0 A+ h' X3 V- #ifdef ME_PORT_DEFINED
1 @( \+ e& Y6 n# E3 C; i1 \ - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
; d1 R) S. X* ?) S6 _* X) b& r - {
: W9 H: K1 Q0 ]3 R: v - * F1 l4 n/ ~6 T* D! Q, t+ u
- }
4 j8 B& \" m, R% ]4 m. ~" I - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
+ k, y6 [& @) Q- t6 u, t$ b4 l1 J - {$ n! x$ r5 {# s7 n5 D9 ~& @
- _DataPin = mePort[port].s2;
' u1 ? I% U, r0 R5 \( Y* Q$ k4 f - pinMode(_DataPin, OUTPUT);
8 P8 S5 `' P& {3 `. q8 ^ - digitalWrite(_DataPin, HIGH);
4 w# Z: O+ i1 h - }3 ~3 n: i9 J! R" J2 f# Y
- #else // ME_PORT_DEFINED. C m7 r: ?+ B% s. d) q6 j
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)9 s! d% @1 P( ^: [2 h# X
- {; M+ W4 |- p- |
- _DataPin = pin;
1 b: y8 X" ~" [+ V* n4 B - pinMode(_DataPin, OUTPUT);
8 s! Z' U7 \; U - digitalWrite(_DataPin, HIGH);
( \3 |6 H. w! `& k - }
: a$ F2 y0 j4 k3 t: J7 e$ ? - #endif // ME_PORT_DEFINED2 I/ Z1 K- U/ |* `8 l
- ) R: {! ?' u) T. i1 h% s$ I
- # [5 C! I* A' d; ]7 f/ W2 m4 z& A2 v
- void MeLineFollowerArray::setpin(uint8_t pin)
7 ~% s7 `0 z% N - {, |5 o! S1 T$ t. _6 j
- _DataPin = pin;, t# K) h7 W' H9 E1 X3 a
- pinMode(_DataPin, OUTPUT); ' T. q' J- F1 T- H ^0 W% H
- digitalWrite(_DataPin, HIGH);
' ~% K$ e! a3 S& v# o
1 \. G" V8 m' _# W9 E- #ifdef ME_PORT_DEFINED
- D: X7 E8 I# N: L - s2 = pin;
6 M& X; O& a/ e" R - #endif
- l* J8 J0 @+ z3 T3 j: E - }
; T8 s0 t6 Y" x4 u- S - 8 z) {) i5 h/ R: x5 g
- uint8_t MeLineFollowerArray::getValue()
* V! Y/ m2 [( l7 a* W' a& N4 W - {7 M( W9 T+ ?4 b* c3 d0 f
- uint32_t LOW_level_read_time;7 N! g9 I# m- U) ^
- uint32_t HIGH_level_read_time;
" z- p7 D8 x; o: Q _4 ?1 h1 n9 O - uint32_t time_out_flag;
\) j: v/ x" m7 Y& d! M% l - uint8_t Sensor_Data[3];
: K. C' y3 k0 y0 b0 _! S* y l# s8 n( F - static uint8_t old_data = 0xff;2 l0 k$ p9 l5 e M
- 9 c" c! `6 J& p6 ~& g
- pinMode(_DataPin, OUTPUT);
0 q8 y3 b& [' J3 |: B5 P - digitalWrite(_DataPin, LOW);
- M2 X! ]7 w7 c - delayMicroseconds(980);
8 v' E7 X4 K/ L+ c R - digitalWrite(_DataPin, HIGH);
% e9 X' b( F; O6 l) J+ a3 C- `; Q, b
1 Z* [. {* v: B- pinMode(_DataPin, INPUT_PULLUP);$ O5 O6 J' |5 f+ e' ^7 r3 y0 ]
- delayMicroseconds(10);# `8 `" g6 g4 i, A2 C
- k6 n3 C1 L. Q, k4 L+ V3 `- time_out_flag = millis();
* P0 o8 y7 t/ y$ M, p8 k - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );# c: w1 i% ~$ V; j7 P
- 6 `" \7 s& q- F. H/ x8 k
- LOW_level_read_time = micros();/ m# T0 D7 ]* _
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 R! j) z9 e) @5 K. U
- {
9 X. ?' h5 I3 P4 O: F% e( f4 u - return 0xff;
. ^+ F4 r; r1 H ~ - }
/ a1 b7 @0 @6 C+ |0 P
& o# s% R, } Q8 R4 g. [, m! s: A# K- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
) f. s$ ^- |3 b7 h7 b' ~
0 R `8 j% i- t( y v- H4 x* _- HIGH_level_read_time = micros();9 X! O8 c% F* Y+ F
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level. n& @+ r! r! \ G4 S! G
4 a- `$ K6 o* W; A- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! \( ~: {; ]% {# @0 `$ I
- {& }: i$ ?/ |# `' O% N: a
- return 0xff;
0 I+ u0 Y1 p" @3 m0 s) j' ^ - }
+ Q. o6 `% u( [5 U R - ) U) Q4 I; t4 e& T
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))6 y; N% s4 K) ~* C2 @
- {- M8 S3 ]' Q. K1 P& w! O' K
- return 0xff;
! U+ H) S. k- T/ T( p' F - }$ _ W6 y, h- ^; @) C
% @5 m* i& D' m! B$ L- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ u# r$ L' {5 o - LOW_level_read_time = micros();: A1 e5 ?: ?: c) m* }- n
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
. A4 R- B( h0 t2 {* I, }5 C0 O- p6 a" P - 7 n D0 Q* x% q. n
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
8 U1 A. F: }; N, b& I( U - {
6 r# ?. ?# H: u, J* ]+ y& M - return 0xff;
8 I6 G6 }! S& q3 T/ { - }" r0 i) D j0 p5 {7 ^
- 9 }5 v2 A1 b: N: P+ \( t6 n
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)); K. Y+ o, w1 G( w: s
- {
) U1 \* l2 g7 ` - return 0xff;2 }- h/ f: x F @; o
- }
( ^) K' g; ~& }8 ^6 x! ?; z9 r
* [+ L B& j, B8 p( _- for(uint8_t k=0; k<3; k++)
5 j/ m2 g" r7 I& F# Y# {$ ^ - {" q1 O2 \6 R0 Q1 M) J: Q
- Sensor_Data[k] = 0x00;
, z2 }& V% S& C" d0 G* ] - : l/ j( n; |5 r/ g
- for(uint8_t i=0;i<8;i++)
, c# W. N E: X7 B - {
- M. G" O! p* c0 u - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level/ R9 R( H/ ] C& }* {2 [6 q6 ?- X* `
- HIGH_level_read_time = micros();3 h# h3 c+ d+ {" w% K- s5 b0 l6 v
- LOW_level_read_time = micros() - LOW_level_read_time;
5 L, D7 S" {# K# R. w. V D - 8 ]; M' D& R% ?0 l7 F
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
& j# @# k) W5 Z& H9 b( Y - {
2 v7 b2 N2 l% P( ?# c! ?; S - return 0xff;
7 A0 s! {6 v( M. _% j/ S. t: R8 S - }# i) g9 h6 y2 v9 |
' g7 [$ `4 W5 ^# w- U- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: T$ [$ ~' T' O* K$ }) k
- LOW_level_read_time = micros();
2 _' t B3 q2 B* w9 x2 @; X# J - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
7 y% ]3 {9 h& I5 n! n/ {5 F$ I - % @; n0 W: l& f' c
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
# ?: A$ z/ \" B" M- N - {: \& w6 |, Z) P8 a+ }7 W( E/ G
- Sensor_Data[k] |= (0x80 >> i);
$ z+ b: ^0 M" \4 e$ X7 Y5 m& {! M - }& Z6 A8 `" W; ]7 ^' q) c2 e/ o1 L
- else if(HIGH_level_read_time >= 100)' p9 s- m3 F2 H* ^( D, }
- {
" X. f" S9 R& O, S* b1 Q# J" y - return 0xff;
- i5 |( A' ^5 w! P. E - }$ q: c! Y4 h! S0 C) h! K T
- : h0 H0 I4 m- W( K
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
& d W% q0 P5 E& J n - {7 X0 r1 G/ |. k; s& m, ~
- return 0xff;" z: Q) u; g* h9 s0 M
- }
: f* f" F5 m. \" v7 Q# G. L# M) i" J - }
& L2 ~- _$ _9 u8 | - }
- q5 U! |6 W2 }! N& d$ b) [ - - e9 r7 a m: ~1 w# P
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
& {1 u8 t* s& T( d0 J - HIGH_level_read_time = micros();
4 g9 \, l: i/ w+ x, d; ~2 X4 _- Z, d - LOW_level_read_time = micros() - LOW_level_read_time;
% h0 o* V. b3 i: f
/ r/ h) R2 J( i( w: ^' u- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )& o1 ~7 \3 q7 P4 z/ ~
- {5 z% S _; D+ W# n. D" Q$ U; u. e) T
- return 0xff;) A! o; X, V9 \+ C/ Q$ V& R
- }2 [9 u0 N' f, J+ t: G- u
' y2 T- l$ P: d. Y6 G) O; `9 @8 A6 {0 n- pinMode(_DataPin, OUTPUT);
# x! J" a' j0 I% M% R p - digitalWrite(_DataPin, HIGH);
1 Z7 d) p9 {$ p0 S5 U3 L
0 u/ L. k# A! r1 ]! N/ c, L3 |- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))) d0 X1 A$ Q6 } N3 z4 O1 G( G
- {2 a* ~, `, Z+ X$ u
- old_data = Sensor_Data[0];! ^! K( @( U. [
- return Sensor_Data[0];
- b9 q% `# ?! w - }# J3 `+ N: M( h. t$ I! z
- else
. u9 N) Q3 d# S8 [# A8 l. ]0 ^ - {
: }, k$ k& [8 S% {: Q - return old_data;4 k" }+ u* p5 @$ v& _5 W |
- }
' m+ M! [: ^( M - }
" s: [6 S! T4 A+ s8 C
複製代碼
$ _/ X/ z$ x* B/ b7 j9 i) F& l/ }" tMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
8 w: W# I: H* ^* G+ ]- G0 ]* X
4 U! ` N$ o: @- `! u' X7 k# l
|
|