|
|
5 o- Z( h& X+ D' L- Q" w# v5 K- #include <Arduino.h>' K- d2 Z5 \9 U2 C
- #include <MeAuriga.h>
9 h, r0 l2 D" Y+ n - #include "MeLineFollowerArray.h"
5 u5 W- M& x$ h" w, p" X$ Y - ; N/ X! q* L- B
- MeEncoderOnBoard Encoder_1(SLOT1);+ P1 J# G; r1 S# C
- MeEncoderOnBoard Encoder_2(SLOT2);
( j) x, O4 T% W4 A( I/ b9 I6 A9 v - MeLightSensor lightsensor_1(12);
0 d0 x e* I/ m6 @ - MeLightSensor lightsensor_2(11);
" T" s# R( G) ^) I5 q - MeBuzzer buzzer;
6 K& Z# N7 d; i6 Y2 d3 S5 J; { - MeLineFollowerArray linefollower(PORT_6);3 ?1 W8 t6 a# j4 L3 Y. C& x$ s
- " z7 Y1 h9 l% N+ d5 Q' u d- v0 U
- #define Error1 1. D. ^4 b7 ]* `) `, g
- #define Error2 2
; e0 }: b8 u- B. ]7 Z( Y - #define Error3 35 O, u0 I. h# ~6 c5 @& k, O
" s" K; g2 i6 v1 W# _- i) W- #define Kp 15, a& k/ T# h; x% u2 N3 M4 _- S
- #define Ki 0.15
: w2 q' }8 h4 F, u f* B$ O) S5 `1 [ - #define Kd 0.03$ S1 @2 `9 X( z# M. h F
- : a/ Q& b! G- O$ y$ F
- uint8_t sData;# R+ u! `' @6 A
- uint8_t D1;
$ E/ K; m7 z% _& d$ ^ r; ] - uint8_t D2;0 j5 R' t% f( m7 e* u n1 k k
- uint8_t D3;7 \/ h5 A/ b0 \# S* A/ {/ N- M3 B
- uint8_t D4;: j2 u$ v' D$ W; T
- uint8_t D5;
/ n% _. ~& p* K( V4 C3 W0 Y( C - uint8_t D6;
' _) L9 ~+ O# \0 v5 M! F7 L7 F
9 b; X3 N U/ g {0 } ~7 E- float previous_error = 0;" A/ a: T% G7 V# s5 O0 P! [1 t5 S
- float integral = 0;
q6 v1 M' \; N# n - float derivative = 0;
/ t. W4 G, y6 ^' @" @: W N$ x( q - int Speed = 160;
# X! L# w& C9 e+ F; `. L2 h - float output;5 h& I) D! d$ L! P6 @& m' Y3 l5 _
- - O: h) o1 \5 q4 T+ b
- byte Left;2 ?6 j' ^9 Y+ q1 ^6 \5 u
& _+ l7 H& C3 ?+ W! E, L- void setup()
9 x- n* b# w) j' _4 _9 C' b- Z- T- C - {9 t! S% g3 W& d3 I
- //Set PWM 8KHz& L; H- m9 | z, Z! Q
- TCCR1A = _BV(WGM10);
' i1 x+ i+ Q5 Y9 O. D; M u4 Q! G9 o - TCCR1B = _BV(CS11) | _BV(WGM12);
4 j! q6 e, `" D - TCCR2A = _BV(WGM21) | _BV(WGM20);
, Q) X. U8 J8 U2 I- S - TCCR2B = _BV(CS21);& ], e2 v0 P$ S) @( ^
- Serial.begin(9600);
2 M5 l* e- H' q - buzzer.setpin(45);) j8 J b' f" j+ H# H2 ?
- }
7 n; J' a# _( C+ a1 C$ C - * P1 t! Q6 a' C1 _. J- | u
- void loop()
6 I+ w# _/ V8 `& p - {, L& e/ G; [* a1 q1 e( V. Q+ X9 B; a% X
- while(!((lightsensor_1.read()) < (10)));+ L- i* v# |/ c" x, N8 m
- buzzer.tone(1047, 500);7 S/ V" K% h4 E8 [7 ^
- delay(500);) |' D! M, L# `- C2 q7 o8 `' i0 i
- do9 A/ q! o5 e# N3 E; j y4 P
- {
7 t( _$ |6 [0 e0 o6 Q; ~9 B - int Err = getErr();
( b. ~# q0 [# m; E' i$ K - if(D1 == 1), t1 z* s6 ^4 K' X! I Y# U7 I6 ]
- {
9 I; H) b3 ] E5 R - Left = 1;* p3 B) m, K8 ` }
- }
7 n# f( c/ \7 v1 o- [1 F - if(D6 == 1)' x. |- {4 }" |
- {
9 n0 E2 c c' w* r; X - Left = 0; H$ q' y* `5 A+ J1 }
- }2 W- R2 J' N0 W4 J3 p( Q
- if(Err == 99)
! k* C( x. N- j% C/ l* i7 g - {- k! c( B: v0 Z8 t, m1 Z
- if(Left == 1)
0 r- ]$ T, e- a/ x" p" X3 o6 Z7 { - {5 @3 n. j1 O1 @; Z7 |+ g
- Speed -= 5;( |+ o" @' V# N) h4 y/ R) q( B
- moto(0,Speed);
7 D/ ]9 Q: v4 J0 V9 T - do
! y( z: H9 m S* L+ E0 Q* J - {
" f& l% E* V8 {+ ^ - Err = getErr();6 g1 t; u) d6 G8 l6 r c; o+ A
- }while((D1+D6) == 0);
4 M3 t& H3 D/ w( L! l - }
$ ]/ w9 e5 ^# G2 V$ B* x' J - else
+ t- U/ B! O4 _& p8 N; } - {
8 O. E" H4 R( g) U8 M$ ` - Speed -= 5;, t0 M) V7 T+ a5 {9 H8 ~
- moto(Speed,0); 2 {0 n2 M/ p* f6 }- f
- do: d3 J6 H" A9 p! ^
- {8 M9 A3 _) ^* N& d7 {% C
- Err = getErr();' n. Z) t% h. n2 ]: m, N/ s' ^" [4 I
- }while((D1+D6) == 0);
7 _, K# m' Y7 w4 i! j; ^ - }
" O0 @1 }* _! o! h - }7 t+ _6 O4 q' B, F1 s
- else- ^9 T# B' T, |. c, g
- {; w4 C3 A: F* I! |' \
- if((Speed < 160) & (Err < 2)) Speed+=1; . Q5 ?0 L+ b. k, W
- if((Speed > 100) & (Err > 2)) Speed-=2;
7 Y4 ^" O! o: V) U7 H - integral = integral + Err;
E" S: _2 O$ k$ }4 g5 o - derivative = Err - previous_error;: |- h/ l! z# B/ b) |
- output = Kp*Err + Ki*integral + Kd*derivative;, m6 {8 r0 Q! ?8 }, u% e$ D
- moto(int(Speed-output),int(Speed+output));
7 P: W; b. \ M$ h+ a9 K# |- r - previous_error = Err;
( o7 V+ I9 A' `' V t2 K' B9 t - }0 j+ Y5 X" ]7 X0 \+ G" o: I
- }while(!((lightsensor_2.read()) < (10)));
' N9 j" V9 @$ e4 e! d0 r7 N - moto(0,0);
9 g! G f2 c* h - delay(500);) z! o/ j. X* |/ B. I
- buzzer.tone(262, 500);
3 ]/ L8 Z2 x/ W% |: \ - }
& k! s$ f) o+ P - 2 m5 y4 V) x9 A# Y
- int getErr()% C) P1 }( J, W% m" d1 b# R
- {
0 q9 I0 u/ l8 G% r( J d" a0 I - sData = linefollower.getValue();4 j$ y% Q, \+ L7 {" \9 ~9 _
- D1 = ~(sData>>0)&1;
1 R( u" r! A6 k - D2 = ~(sData>>1)&1;
\/ K5 C, r. T" S/ t - D3 = ~(sData>>2)&1;
2 i' t, v- L8 y4 J - D4 = ~(sData>>3)&1;7 N W0 k: Q4 _' }* t7 y) g
- D5 = ~(sData>>4)&1;- `0 p. v8 O1 {
- D6 = ~(sData>>5)&1;
2 c" u0 N4 R5 i" i9 t - int downD = D1+D2+D3+D4+D5+D6;
9 D2 t1 D' A3 b8 f+ L+ N+ ]. ^7 E2 x - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
: F" ~ {) Z2 s9 F4 q1 l' K& o; N# T# i - if(downD == 0)
. Z1 S+ L' {2 F3 W3 Y' J: m - {
+ T: m+ P* @* v' I5 v# G - return 99;1 V4 t" `: p4 v. D- Y# N
- }
) ^" X1 }, x- b7 _. }0 D - else
% H7 R' v2 f+ @; c+ y. U - {) \8 z' g: C0 \- D# P! I
- return upD/downD;
# N* ^( y+ x) \0 n* s - }
2 l; s9 [# f4 _4 L$ G( q7 t - }( o0 R* ]9 Q, q- @5 d% `# b' b, y
- 5 I; M; ]3 g+ r7 N
- void moto(int Speed_L,int Speed_R)
6 R( y4 |/ X" e: D1 L6 L' Y - {3 m) h; I/ z$ k# F
- Encoder_2.setMotorPwm(Speed_L);, y" \- e: @# B2 j; R. o: _
- Encoder_1.setMotorPwm(-Speed_R);. a! R% J5 z7 q' I0 ]9 E/ Z% N- e
- }
複製代碼 . M* P! w: ~8 z F9 c% e6 g) V
MeLineFollowerArray.cpp
# b) D! T2 ^' E j) v$ E: V- #include "MeLineFollowerArray.h"& ~/ |$ |; p5 _! W0 O* h4 w r
5 a. N( X5 u1 ]! k! ?" I9 r! e% h- #ifdef ME_PORT_DEFINED
8 P) r' l+ L" w - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
2 h- \, f+ @# A8 Q# u - {7 U ~2 [* ~! [, W" F6 O; t) s9 {
; j5 [: [/ d4 p; h/ b- }- o& w! V6 W5 y4 `+ c
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)0 B# F3 T9 o+ J p
- {- V- J# a- y7 o; a8 [# h
- _DataPin = mePort[port].s2;
1 b5 C# _ S/ V; j3 a9 x' ` - pinMode(_DataPin, OUTPUT);
: [" v" Q& q# l( W; o' {5 T - digitalWrite(_DataPin, HIGH);" n! C; I& m- P! n0 ^+ K
- }+ k( X4 Y$ @4 R8 }# a8 i
- #else // ME_PORT_DEFINED3 Z6 C1 Q) W/ {3 u2 C
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin): D! v6 ~3 z/ E6 A+ `8 p- g
- {
% |- `3 u' x3 ~. A4 x4 o; o8 g - _DataPin = pin;2 b9 X' T6 K6 y" x3 L, Q& W* }8 h
- pinMode(_DataPin, OUTPUT);
5 B( ] K* B" { - digitalWrite(_DataPin, HIGH);
$ x2 N9 K V! [9 E1 D - }& ^; `+ D3 [5 l; L; I1 p
- #endif // ME_PORT_DEFINED- C6 \- A$ ~6 d! y& L0 u
- : @* [' W& U/ H W+ W/ Q
- 3 z9 {+ v1 U, J; i4 H( d
- void MeLineFollowerArray::setpin(uint8_t pin)! s8 Q0 E9 \+ p" r
- {5 F' M/ U0 r2 v, N t! B
- _DataPin = pin;
/ f# g4 x* Z' x* `, D+ M+ g - pinMode(_DataPin, OUTPUT); 1 }+ P- _6 U0 d
- digitalWrite(_DataPin, HIGH);; `# w! o: O9 A
- 4 U- v* v$ w2 N4 Q# R
- #ifdef ME_PORT_DEFINED( q: G7 a" u @6 L) Q' B* S# o% t
- s2 = pin;
B, `/ b8 r1 Q0 T - #endif
" t7 }6 [6 |/ [% J0 M - }
8 L: h$ F3 o% _) p( @* Q
5 K( I8 c# X8 ]$ s- uint8_t MeLineFollowerArray::getValue()
4 a: k% i( N X1 g - {6 ]8 J" ]# b! M. V* m" D' ^
- uint32_t LOW_level_read_time;
1 G! H- B- \' P/ \& I - uint32_t HIGH_level_read_time;8 ~) _! R' i2 A% L* Q, k$ r, t
- uint32_t time_out_flag;/ H2 r3 y D& N# y$ @
- uint8_t Sensor_Data[3];3 j' Q! u3 s: e% o% q0 u' Y) O8 \
- static uint8_t old_data = 0xff;! N) ^/ m3 [ E: |1 n
: h6 R0 }+ \+ a9 V) Y; Z- pinMode(_DataPin, OUTPUT);
" F% k& B, ^0 C* J+ C. g' g - digitalWrite(_DataPin, LOW);
/ K5 w- C1 J, q% \$ d R5 Y6 r6 | - delayMicroseconds(980);3 w* x4 o/ \, k2 V* o( l8 x- z
- digitalWrite(_DataPin, HIGH);
3 }5 @) l' h1 Y/ K& X/ f' h
- j/ r/ ?: P3 w$ H- pinMode(_DataPin, INPUT_PULLUP);' C5 `; F e! p8 k
- delayMicroseconds(10);, v% f S$ E, @( K; W0 @. O
- - H$ {+ \% K* Z2 K
- time_out_flag = millis();
% x0 K8 b' ]3 h - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
* w' }$ z$ Y+ z2 x - # G; _( ~8 H" q
- LOW_level_read_time = micros();
- Y9 z' B! v8 f0 M; A/ k. n+ f - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! b; w+ Z2 G, m
- {- M* R" N) x& @5 l( w, g0 ] ?
- return 0xff;3 D$ P0 o% L$ c7 c6 D
- }6 N" H+ r; e3 r: v& c% C# A
2 x1 `! x1 _) c4 W' g$ d- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 ^. R& k3 p! F) |( [% u; Z6 ?3 m - 6 |$ P8 q$ R5 e6 h* v% y. {
- HIGH_level_read_time = micros();2 ~& ?$ w" W+ ~- P
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level2 d7 f8 c. m+ Z; w* y
# u6 U2 d; S" h- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 O/ U# ^3 v1 @+ z6 |8 O* S b - {
( U" w9 A* e! H- S/ Y# J' N - return 0xff;
3 I/ n% H- g! D - }
' z0 v ~: t9 l! }: ]% y" W( C
6 v: U9 c H& _3 i V- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
* r# ^) v# N2 F5 a. b8 Q. v - {
2 W4 r3 g- e3 D! G+ z) R - return 0xff;
& ]5 h" m) Q, @7 L" `" M - }
d& I4 ]! k7 p9 ^ - ; ?1 E. C! r- f
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, X/ @, ^0 w8 r. |" ? t
- LOW_level_read_time = micros();
& P' w$ F, u- H- v1 ^ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
8 S ^! g/ P+ N9 s
( J+ C7 L. }0 i- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 z) N; W# A: @3 L, b! ]
- {
8 n6 Q7 d: U. z - return 0xff;. g( _" I6 r9 ~# q/ z
- }2 b1 ]4 i; e) Y" ?6 F9 {
1 o, f- v* U2 R7 N( M" c' V/ J" K- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
, @& d+ u! {2 L) \/ s7 _. h - {
* J t, p* z7 p2 j1 F - return 0xff;" T3 k) Z, [% z9 r$ l& T
- }
: D+ z2 E- o* Z3 z; ?' F/ o" e+ y
, h! b8 { |( L* G* C7 E- for(uint8_t k=0; k<3; k++)7 @* j% G2 _2 S' h
- {4 i7 ^! i/ z- U8 c; f1 U
- Sensor_Data[k] = 0x00;
9 @4 ~2 }/ l* |2 l) A
5 [7 Z7 X& z& T- for(uint8_t i=0;i<8;i++)
+ a1 X e3 q6 [2 p6 {( U! _ - {& _5 d2 h2 w" a3 Z3 Y7 E3 L4 B
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
% d7 h/ h" c- _4 `" s, _7 k - HIGH_level_read_time = micros();# K9 C" G0 D0 l* z# n" R! N2 n) l
- LOW_level_read_time = micros() - LOW_level_read_time;! J1 J0 c+ N$ |
+ c, Q, \# r; n8 j2 u1 z2 ~- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
' g- O4 q7 b/ W' G3 E1 g. A( T; U - {
% e! r6 s' M0 W/ J - return 0xff;
& j7 d% `' Q- r1 D, @$ U - }5 n( q. |3 A4 q9 W4 O% g. r
- 1 c6 i7 U$ `3 {- T' A
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
" r3 I9 z1 t" ^4 w q. {; _ - LOW_level_read_time = micros();
. z1 y& M% e' ?6 D - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level- Q+ y: Q6 w6 F6 t/ k1 L
- ?0 O5 D0 [( W1 n3 S, s5 ?- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
6 j4 ]/ U- Y6 ]2 Z# F - {+ I1 ]) p& e- }1 W1 z- l
- Sensor_Data[k] |= (0x80 >> i);
1 l7 ~' q" ]" Z - }
1 ~# Z: Q5 {0 ] Q) s' I - else if(HIGH_level_read_time >= 100)
3 d! g; W. D+ U E: R - {
6 s7 I( Z+ ]; I: T - return 0xff;
4 z: W4 f- t5 V) A% j - }
: L/ V- e+ N0 R* E: ? - 3 M+ c3 M8 ^% w
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)$ u* t6 O6 ]7 n0 _$ R& z7 o) s
- {% L" \7 M( t1 ?) A# s- {
- return 0xff;8 p7 y3 p/ ^) ]7 |, w
- }
7 a: y+ t- H, w) f3 o6 A - }. v* D7 A. Z. X4 l+ w+ R
- }
( r& A7 n/ M& J - + q3 ?: B4 _6 U
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
& |- j; W* G) H9 W. X- d% v @9 j/ ]4 D - HIGH_level_read_time = micros();' m$ \" ^3 ?: t$ C) {9 _* W5 B0 T
- LOW_level_read_time = micros() - LOW_level_read_time;# L, j3 v; j! D7 t* S9 E/ `
- 2 ~$ V1 e" s2 Z# s3 \- V T
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )( `6 I5 O8 \1 m6 }
- {3 f( z! |3 e' D+ s
- return 0xff;
3 n2 G7 V3 l) m! A' I3 A - }
) S% q1 Z! h7 s! J0 s& O
: Y& F9 d1 ?+ A# }; G- pinMode(_DataPin, OUTPUT);
0 a: q$ b" Q( _2 S- F. _, b y- _9 ] - digitalWrite(_DataPin, HIGH);2 i ~% a$ Y: O9 W+ ^9 P' N
- $ j6 u4 _3 [) q
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))2 Z" d4 E* F. F3 x0 `* H+ d
- {
& _1 g7 p4 i& X. i - old_data = Sensor_Data[0];+ f1 v( T0 r. e* q l7 o9 `% O
- return Sensor_Data[0];+ I! k4 u% \! ?( W1 ^
- }
$ h# ?3 u0 @: K: ?; w9 C0 w - else
9 E4 k/ a5 C1 p/ M2 ?8 l - {
& t' d/ @7 @$ \( s - return old_data;
' l, S* X- p: x; l# X9 K - }
, n0 j9 C4 C9 n/ C* G. f - }
* _( t! y/ [' X: `: r, u% U! @
複製代碼 , X2 {# Z1 E) G/ ~
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
* p3 p) U2 ?5 p" R
3 j. p. x7 F8 ?
|
|