|
|
+ r2 g2 ?* h& m7 Z
- #include <Arduino.h>4 z' w2 I: B2 f9 [9 G5 g" f
- #include <MeAuriga.h>& e0 D) ?4 q+ n; Q+ z
- #include "MeLineFollowerArray.h"% Y6 P, y3 c; y& K
( \" S- g: w4 T+ q/ x, b/ L- MeEncoderOnBoard Encoder_1(SLOT1);
. Z, u- x0 j7 |% N% c$ S1 M - MeEncoderOnBoard Encoder_2(SLOT2);& W( w- L9 v& w- \" x" D
- MeLightSensor lightsensor_1(12);
9 S/ V+ K2 r2 s* Q% U- O* h* t {; i1 I: A - MeLightSensor lightsensor_2(11);
# k [$ s D4 M; W - MeBuzzer buzzer;/ h& ^/ a, L0 @" p% l4 p* f
- MeLineFollowerArray linefollower(PORT_6);
+ Z! b1 Z, R: [6 c K# W' G `
' l2 [; u6 `6 X C" |. e- #define Error1 16 \+ H! n9 E _' R& E- ?; [6 w
- #define Error2 2: U$ L& U& {% I3 o' X5 r3 m9 l) I
- #define Error3 3/ r- I. D3 m( @) q7 G
1 ^4 d; y0 n& n, w w* U- #define Kp 15
H4 l9 C% Y+ t" ^! I# h2 C, } - #define Ki 0.15
$ [" U1 W {7 H2 ?, _) T - #define Kd 0.03. u9 N% q2 K2 W* f8 Z- q4 Q4 }
: N; o: P3 r8 i n( Z- uint8_t sData;8 }& B# m0 _: F) c2 @" r
- uint8_t D1;
8 u+ E9 s3 p1 j, Z3 t: ] - uint8_t D2;3 U' I0 l$ S4 }7 Q3 J; B
- uint8_t D3;& E# [, \1 a* M+ O2 Y
- uint8_t D4;
" b7 Z; ?2 d# J: b, h - uint8_t D5;
F" a3 ]$ l* i$ S! y, J - uint8_t D6;
* U- {! E% B- I9 K9 O( y2 V2 B; u
8 ~$ E! q% a7 L5 I- p6 p5 c% N- float previous_error = 0;7 d! j( p* ^* O m4 H% z/ ]
- float integral = 0;
9 f* k0 C. [: N$ y- L - float derivative = 0;3 h$ L$ z' Y' V& u2 i9 d* M
- int Speed = 160;: z* G1 c B# P+ u7 b6 C
- float output;
" s. {- Y( ~5 \4 r# _
. A( |% j7 z' ~9 S/ z9 I8 w- byte Left;( v( Y3 e) Y* |8 O H: m
' M- K/ B' e9 x0 z# y- void setup() 7 v8 H. p) P; d
- {9 F) W$ w* U% s* X
- //Set PWM 8KHz
/ r1 Q6 R$ J; ?! D1 d, Q; j - TCCR1A = _BV(WGM10);8 j/ ~: o' f- l& ^/ e
- TCCR1B = _BV(CS11) | _BV(WGM12);4 i& a5 I C- G" g, Z K% f
- TCCR2A = _BV(WGM21) | _BV(WGM20);
6 T- B4 b% W6 r1 Z - TCCR2B = _BV(CS21);
. B) E& c# c1 ?" ] - Serial.begin(9600);/ q- |7 _' g4 [
- buzzer.setpin(45);
* _3 A5 f2 y* m- v' z0 r$ i) Y - }
% M- {! e6 n+ S/ D9 F& w$ c; S - 6 }& ]& x( H) U2 s* h2 ]
- void loop(), } K9 X; F) {! | t- `
- {$ M3 t9 N) x1 S6 }; U5 H
- while(!((lightsensor_1.read()) < (10)));. H( Q3 d K% L7 G& A& D/ A
- buzzer.tone(1047, 500);
- x1 P+ F8 Y& E$ [' n - delay(500);
/ c/ c) U ]: j6 B2 p1 Y! n - do
8 H7 g0 `$ ~/ J6 P% ^ - {) w, z$ ]; s" v6 x" B# B
- int Err = getErr();0 m" T: [# \3 B
- if(D1 == 1)& n" L0 z% o' _+ |+ t% H, b
- {
/ Y( P" M3 }2 i: N- m - Left = 1; \# G' q; B7 h
- }5 \. K$ h z5 Q( q! u7 b! ~
- if(D6 == 1)
, p2 ?+ [ D1 }$ k - {
# I% p: n; u3 Z* k* [ - Left = 0;6 P1 Q2 G! Z: j# `; ~% M
- }$ X _4 n" H, l5 ~* k4 Q
- if(Err == 99)
1 H2 w, q7 A( c* W0 y/ _ - {$ N' @" M& O$ P
- if(Left == 1)
, \" ]6 e) E5 @: G6 v# t* [ - {$ V3 |' }6 s7 \) g, L/ A! s
- Speed -= 5;
+ u) \/ q9 B( G# ? `" k7 g4 L - moto(0,Speed);
/ C+ [/ G3 z, Z - do; ^* ]% h/ j% x& v5 ~4 A. t: Q( e
- {/ M1 d5 f: l; v' H* h
- Err = getErr();9 {( `; K( h& s& y7 R+ G$ u0 g
- }while((D1+D6) == 0);. r$ X+ P2 T- |! x) c) N7 g
- }% F- {; q1 t6 c. `& t/ R6 E( k
- else+ ` p7 |5 A2 q' E, j- G1 f
- {# U; t# d3 R ^9 d5 ?
- Speed -= 5;! a: J- J- Y: S% W( ]) {
- moto(Speed,0); & v$ T( a# p, |. y" f1 C
- do" s! Z. P9 ^7 s
- {: @/ o0 _1 i/ [3 L
- Err = getErr();1 T" X' F5 E8 M q
- }while((D1+D6) == 0);/ F" Z" }0 H' n, w
- }
- V% S2 L3 v ~- O - }) @2 f; F* O0 z/ b! O4 R; @5 x
- else+ z" `8 a# Z/ M5 w8 T3 y X
- {
' G* I E5 c& V1 R - if((Speed < 160) & (Err < 2)) Speed+=1;
% u% X+ m( V; _2 D - if((Speed > 100) & (Err > 2)) Speed-=2;
: i, S7 @2 f% c* `+ j4 L - integral = integral + Err;
1 t/ F5 Q- }( p0 Q( s: B. W& I - derivative = Err - previous_error;. m% _0 R- [$ D) k
- output = Kp*Err + Ki*integral + Kd*derivative;% F8 [3 w1 c) @1 U2 F9 Y1 }
- moto(int(Speed-output),int(Speed+output));
& N! z) v. f; W+ Q! o7 @ - previous_error = Err;
$ h$ h" Y: l3 @ - }$ [. F/ `' l$ j7 |/ Y5 b: j* M
- }while(!((lightsensor_2.read()) < (10)));- H5 |$ z. b( b# N9 G- Q' g
- moto(0,0);
+ n2 J/ Q F W8 A2 { - delay(500);, e; o0 S# P, R9 g
- buzzer.tone(262, 500);; C% r& N) u% S% }) w6 Y
- }7 ], ^; y, G# x$ P& `" D+ v3 v
) _3 G: ]0 C$ x+ ?& ?- int getErr()
" |7 I) D5 @! ~# j1 L# D - { 8 X/ |0 o# }- e# `" g/ V, E4 ^
- sData = linefollower.getValue();! j. `9 x6 ]& q" P3 w
- D1 = ~(sData>>0)&1;. L3 x/ r5 i1 y g
- D2 = ~(sData>>1)&1;
' ~7 S( A0 Y) I# h2 O! ] - D3 = ~(sData>>2)&1;7 U- p; f. B; I; |3 i" _6 ^ V7 U
- D4 = ~(sData>>3)&1;
1 ]9 x3 @; d7 E2 F4 y) x3 Y - D5 = ~(sData>>4)&1;$ ]: f/ i9 d$ V; w7 B
- D6 = ~(sData>>5)&1;
" r& f' `# ~: ?* f1 v! ]1 f* u - int downD = D1+D2+D3+D4+D5+D6;8 a: [' R1 b* }
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
\; A# f/ L7 x) Q# O4 f - if(downD == 0)2 w6 J$ m4 {2 B! C
- {
' o/ Z j! }6 m3 |; l - return 99; r# {( [6 N' s z( n0 X6 |
- }% H. X$ Y4 }' A) s* W8 y, o9 @7 h! |
- else
( T+ O9 B* A' Y' h8 w - {
7 V& K) Z e4 r" `9 k( E - return upD/downD;- x/ Z5 K6 Z5 d1 i" U- V0 E$ J
- }$ E& M6 p$ B# i0 l g) @
- }
& N0 y! K [5 h
/ i, D- `6 c% r D0 |3 Y- void moto(int Speed_L,int Speed_R)
# u) n1 V7 A0 K4 Y F' ` - {: P" {( u+ o8 D% T& K: S
- Encoder_2.setMotorPwm(Speed_L);
. e5 X' t6 r: F/ X. A - Encoder_1.setMotorPwm(-Speed_R);' S! v0 B0 o- s0 M+ n
- }
複製代碼 8 O* n& s2 i- k6 V; p% @! n9 F+ ^1 S
MeLineFollowerArray.cpp
. c0 Y5 {: Q% W% |6 K! N& T- #include "MeLineFollowerArray.h" M5 Q# z, d) u4 S; F
- & l3 S- X' `- Y" M1 }
- #ifdef ME_PORT_DEFINED
! M4 W4 ^. G/ Q; ? - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0): ~! V* ^# X- t
- {$ T: W1 Y: p2 p# F7 c a6 N
. O$ |: F/ o# H4 V$ l( s* P, r- }' V9 g' R% ?! T9 j
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
: { R+ q" K. V6 R" r, X4 \ - {% q" |9 f, @2 v8 b! [, y4 S. `
- _DataPin = mePort[port].s2;9 N/ h' w; e$ N' X9 ^$ \' r% T, b
- pinMode(_DataPin, OUTPUT);
1 g& ]$ x8 {# {2 M; a- K2 w - digitalWrite(_DataPin, HIGH);
- s, H' T* {* D, O - }' b5 w9 e/ d8 y# p$ V7 i6 d; h5 L
- #else // ME_PORT_DEFINED$ A; M! E4 J# [. i& @) _
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin); X" _+ N+ p: _
- {
7 h) |& Z* O- b9 {7 p5 E. Y - _DataPin = pin;6 w5 } a. f5 |( u
- pinMode(_DataPin, OUTPUT); # \# J4 Z1 ?+ j; h) m& \; b& x
- digitalWrite(_DataPin, HIGH);
" [1 D" S; I/ _3 H' q - }
+ C3 s, J/ K2 r2 n - #endif // ME_PORT_DEFINED
* y- c3 p- o+ i! D) s( { - # g1 i+ u/ _- G! @$ s
- 9 D I( f1 o2 w& \1 G, w4 S
- void MeLineFollowerArray::setpin(uint8_t pin)" { z2 h* F) V, i2 ?% L' _% Y
- {
/ B: _! F* r+ U7 u( D* ^/ ~% Y, [ - _DataPin = pin;0 N9 M. R% d# V$ o6 K! a _ B
- pinMode(_DataPin, OUTPUT); ; F4 W! Y1 t: i4 B: z$ ], Y
- digitalWrite(_DataPin, HIGH);
* F) J3 ?4 C9 S( B - ' g1 { E" q$ c+ D% h( y; S8 }
- #ifdef ME_PORT_DEFINED+ n [1 }7 H* U- s
- s2 = pin;
0 k9 w, y* ]8 ^: o - #endif
/ N0 H7 k& y3 l, U - }" v/ e7 m- J8 N* M# M# w0 V
" n3 G; \2 t! t# `& m- uint8_t MeLineFollowerArray::getValue()
* y2 C C" U T! O, H7 w - {9 A$ U# J8 ^( h* n% e) o* l
- uint32_t LOW_level_read_time;0 y1 g) p& `0 u1 b, `
- uint32_t HIGH_level_read_time;( y9 r" [4 }' Y2 d- M! E, s0 ~
- uint32_t time_out_flag;
: ~4 G6 {* S0 P* Y/ ~5 }1 S1 { - uint8_t Sensor_Data[3];
) X; S$ C: N' S8 N" s5 v; S9 ` - static uint8_t old_data = 0xff;
! k8 m3 ^% c' e4 ^; Y N/ t
" O4 R2 P; f1 a% M) \/ T- pinMode(_DataPin, OUTPUT);
: h0 F* [' n7 ^5 z - digitalWrite(_DataPin, LOW);! ]; R# @2 z. M$ C* b J# r
- delayMicroseconds(980);
: Q( ^3 I- C# x1 ~! ? - digitalWrite(_DataPin, HIGH);
* P" C, q! |( k' U9 N
2 G6 Z/ W/ i, F& L' t) [& l( D) v- pinMode(_DataPin, INPUT_PULLUP);
, `; M; M2 E V. j G - delayMicroseconds(10);' T% e! o- D% M8 H$ b
- $ @$ s& Q; I1 K( y
- time_out_flag = millis();* t3 f9 a! e" S
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* r4 r m( _+ N8 E3 B h
N, V N6 \: E$ u0 I. z* F3 J- LOW_level_read_time = micros();; X( y. ]' P6 X: {6 y; f$ m
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out: N0 }) j1 n: f; O; k7 @7 b+ y
- {
[7 `/ U9 {& m+ E- S0 F - return 0xff;
6 t* R2 Q7 S. d N+ ^) b - }
8 q9 D9 m- a. { - 5 |4 W) t8 y {) T1 W6 j2 x3 ?
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );6 m4 B3 q* z3 u9 H3 d5 L' I9 t$ h
- 2 X2 h7 l) B0 B& t2 x
- HIGH_level_read_time = micros();
/ v( K- H; w! Z: D. A% q5 X2 `2 V - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level. o& d6 z% v0 [
. _. U2 Z4 j2 h7 r; w4 g2 e- J- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 ~5 k+ [8 h7 R) h& V( Y. b! q
- {1 p# t0 b6 x' _) v- P4 m
- return 0xff; s( u1 j+ M$ G$ o$ }
- }% _1 j3 V: L) a$ u, R
9 E: ^) M3 g4 z- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))% r: u* F" v0 m) x6 x
- {7 e3 O4 s: w, _& A( A" R5 m& j1 v2 m: F
- return 0xff;0 j- n9 d; i1 A: O
- }% D) c) y- F5 q0 [: ^5 [; L; s
$ L: b2 p8 ?) w* b! E- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
, \, L8 R* a2 s5 M5 v - LOW_level_read_time = micros();% q# |4 B; D- L' X+ G3 Z
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level [6 D a5 ~; u# s
) d7 L% K& d" d$ f- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! I' |2 p: o4 Z# O" X7 r* p/ z
- {# M u$ k0 k p2 e Y- g, h/ d+ E
- return 0xff;
. A: M1 _$ K) Z( ~ - }+ b6 T2 o- x5 @& a1 m% j* |* U2 _
% \% M4 {! s# _# j- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
& v" Y; k/ K$ b0 W7 g - {
. W+ d$ q0 J: f; M - return 0xff;
8 U# ^: X) k* H m/ m( r2 V$ A1 } - }4 f: `, H' \! i% d) l
4 C9 g I% f3 e# N, L% ]- for(uint8_t k=0; k<3; k++)0 } l C" X" K6 M. @
- {$ T. K2 u8 o- u: W7 y1 W
- Sensor_Data[k] = 0x00;6 u( [% d7 W) P K! G; h# [9 x9 K& `
- # m- {( |1 {& S. f
- for(uint8_t i=0;i<8;i++)2 f3 z& {* w9 p0 B
- {1 m4 a4 f9 w4 u: i
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
/ m! }$ q d+ o9 y% a - HIGH_level_read_time = micros();) Z% o2 V4 m; X" ^ [; A, I! \
- LOW_level_read_time = micros() - LOW_level_read_time;
# K6 l2 Y* z3 Q - : [- N+ H: a! w- c) S& s9 {9 m- n
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )( b9 B- O; G% {4 C7 e* _
- {
- j- Z' K* n. ]! W! E. Q, J0 c - return 0xff;
5 }1 i5 Z3 A$ v$ e" e6 g - }
+ x( w9 k1 W0 H" d! E& B1 }
/ s4 ?. [% X4 N8 P# k2 n8 ^- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 q! v* N4 @& |# M4 z j, A ` - LOW_level_read_time = micros();' |# k5 K; a- P m6 U- a
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
7 J0 R7 R+ W7 y4 N/ [0 p S - - ?6 Z v/ R, v# x; H; W
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
8 K' Y7 ?5 f, M: O# F; J - {1 q* k: Z) y% b6 v8 L( }, p& K6 j
- Sensor_Data[k] |= (0x80 >> i);4 L" S( M/ K# I6 z& ^3 m6 ?* ]
- }
" g8 Q$ R3 N+ ?" ]" R# u - else if(HIGH_level_read_time >= 100)! p6 x& Z: s6 t) \" F8 ]. o
- {: g8 A6 u/ K& T: T7 t/ i4 i
- return 0xff;" c( D* P* r7 \6 W
- }
# F3 S; U5 Q, b/ q) \& p* c - & t8 G( G0 M0 `! q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out): R3 q3 k0 v$ |
- {: y( z2 H5 E% j9 K
- return 0xff;, N8 R3 U7 C- U _& D, t8 T
- }9 b: e! G; T8 m) |
- }
W2 u: O& A$ @ - }, T1 I) V9 T9 y, k7 h5 c. Z
& `* h% d. e3 E! K! v5 F1 ? L! J: f7 Z- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
! K* X3 E6 T2 v - HIGH_level_read_time = micros();
' T, Y1 ]3 U1 U, x9 `- ^ O ^" n - LOW_level_read_time = micros() - LOW_level_read_time;$ I3 n B5 x% n2 q/ B8 w3 W a
3 ~! P) X8 l: K @8 M, n- _/ ^- C- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
4 R* w: m! I/ r+ ^$ y8 [$ n% m - {
* j! R* l0 W1 l. p2 Z9 z1 z! x - return 0xff;
# X. o% m! K# h- v1 _3 o7 {: t - }$ B/ Q F0 E; s- }
4 _# P5 y6 ^# o" v- pinMode(_DataPin, OUTPUT);
; ]# f2 a/ N- R: L8 r6 { - digitalWrite(_DataPin, HIGH);
$ U5 u; ] Y: \- A( E w1 _1 ] - # S' |& g) U* u k8 o9 ^
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
# \. i! U7 @7 I) B - {7 a1 u4 B) g0 {" v* N; s, _
- old_data = Sensor_Data[0];. D: x; d* U1 o
- return Sensor_Data[0];+ {2 g% y) F* i
- }
& [" B1 q8 R; H8 ~ - else+ }# i. t8 v9 Y) e' d7 Z
- {; g9 y, {) r+ C' @; c. E
- return old_data;7 S9 F* ?7 t6 W4 M4 o# O1 d- k2 ^
- }
" Q' ?$ [- {- {! H. ? o* H. d5 L - }. x# i' R9 z* i) S4 a/ b* x, a8 B
複製代碼 3 u( P9 o. r! k- i
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
, y$ [- h" n: w1 X1 C, {) K" e, ?
: y9 |/ e) x: }4 ]/ A9 G |
|