|
|
- i8 k% U/ z# N- #include <Arduino.h>
% C+ g5 I( G- s- W4 p" h - #include <MeAuriga.h># D$ f* Z5 v! K+ r
- #include "MeLineFollowerArray.h" L% v `; C9 g
- # ]( |7 k6 ]7 Y6 z, x
- MeEncoderOnBoard Encoder_1(SLOT1);
2 B; l6 l( f! n3 S: X! [ - MeEncoderOnBoard Encoder_2(SLOT2);
& X7 `# D! }* a0 @+ M* z8 M1 U - MeLightSensor lightsensor_1(12);' g5 h& t. I& i# E
- MeLightSensor lightsensor_2(11);/ v; ?3 h* A% v
- MeBuzzer buzzer;
, |( [2 `- p1 F3 P0 ~2 r' }, H) s - MeLineFollowerArray linefollower(PORT_6);& [6 t6 e0 J9 L8 w' }" ?
K+ Q" E4 V0 s- F! `' X5 Z4 L- #define Error1 1" b+ c% u& P: F& V Y2 z: ^
- #define Error2 2
( w4 f; a" |7 ]5 M, z# e# q. F - #define Error3 3
* M$ t [% q, f# {& C- }! o" q - 8 m: w" A3 k$ M$ M k# n) j
- #define Kp 15
7 l5 L/ \7 x. k& V% S! G4 C - #define Ki 0.158 z* W) q( I1 }; Y2 v5 l- F
- #define Kd 0.034 F3 D; g. _6 M2 o1 k
/ E M/ M, B# N+ `- A- uint8_t sData;
3 T3 G- e; T# t6 I/ t1 S - uint8_t D1;* X' v7 h0 v9 D @' ^1 g
- uint8_t D2;
_$ \& K- E/ S$ G- _9 P - uint8_t D3;
! V9 q6 k5 b5 o. D - uint8_t D4;
: I! l( W1 u* t) E# d% w - uint8_t D5;' K, E! [! e% q1 L, ]1 z+ c U
- uint8_t D6;, V) J; H; [* ] Q2 W
- 0 N; b* I; J' v2 Z( q
- float previous_error = 0;
& l/ u% n* V4 w+ y; L( f - float integral = 0;
$ p' Y0 [/ x& t1 `- v! Q - float derivative = 0;
0 e( R9 e* b; {2 u" U# \7 F - int Speed = 160;
' Z9 l( O/ P$ ^" n) i1 q1 d- {2 ? - float output;7 I' d3 l7 v/ C' I/ E }: }
- / p, b4 }% i, {, i/ ]4 s% n
- byte Left;3 j i9 M3 ^% t) z# I# ^( n5 N# r
' V% N, Z; A2 u& W+ v$ g+ S" p- void setup() ' m" G+ j0 b8 r( z# u8 v0 Z+ [
- {
6 E+ \. t3 c; H" ]1 T4 F' t4 ^ - //Set PWM 8KHz `+ z$ a( n. H$ C$ X# G
- TCCR1A = _BV(WGM10);
5 e: j0 g0 L$ J2 V4 | - TCCR1B = _BV(CS11) | _BV(WGM12);5 ^3 h }' [+ W$ q( ]' U
- TCCR2A = _BV(WGM21) | _BV(WGM20);9 J- \& u/ Y+ s4 N) C: F. c
- TCCR2B = _BV(CS21);
C1 N& O% |. C3 ~& y/ {3 A4 f9 J T - Serial.begin(9600);) B4 l; H! D( e, }
- buzzer.setpin(45);
2 a2 b5 L6 m h2 W( \- T - }
8 Q) `- T5 Z% u& c: P# }6 J - / u) q! S4 o* S. f" R
- void loop()
) }: v1 y( O' N: k) o* W - {, w6 Q$ {# X. @+ s3 a5 `
- while(!((lightsensor_1.read()) < (10)));# q& y* W) z2 y7 i* B. d* X) G3 e V
- buzzer.tone(1047, 500);
( a7 X5 {8 y+ |0 p k' L - delay(500);1 G5 F! p$ o+ I ?
- do- A X5 f8 D4 V7 t: W
- {& o9 [. n+ M( ]4 H1 K7 W( J; s
- int Err = getErr();5 r1 N( W0 c1 Y( ^* ?0 ]
- if(D1 == 1)% r e: q9 w$ f! V+ w, w
- {
d+ x' K1 U1 R6 ~& z6 I - Left = 1;
, T3 Q4 ] B; _' p - }- j- b; \1 @/ U: \& b, d
- if(D6 == 1)6 g2 U$ [- S- n" T: P8 ^
- {
6 [' ]0 M$ a- d( R1 s' b9 d( h - Left = 0;
2 w# j) R% S5 } - }" Q# g, R2 i& }/ u u' L
- if(Err == 99); j0 [* `3 T+ W7 V; S/ Y2 A
- {* ` f9 Z7 f! C# w4 O2 E4 k- Y" Z
- if(Left == 1)
2 R. |& C, m& m - {7 x1 K5 M- H; B1 c' I
- Speed -= 5;
$ T: Z* a( B+ a7 e - moto(0,Speed);: A$ X) O5 r5 Q4 R6 v: ]
- do% x2 b( F$ A# E, P3 b% B
- {& q- b" K2 o+ H/ [/ O5 J- y
- Err = getErr();
% c& k- V& u9 u2 N6 s - }while((D1+D6) == 0);
( U* C) B. W+ A! ^2 t2 I( R# ? - }
) e; n: A* ~& z% p - else
/ S) p' e0 Y& s; G7 y3 K5 H U+ x - {* H( D) c y" M" y P
- Speed -= 5;+ \, V4 }8 G. ~
- moto(Speed,0);
' F5 t( @; M3 ^* S1 \% p - do6 t9 y+ R" K/ a1 n# E! E
- {
3 q# K0 X/ d) ~$ G, v; X - Err = getErr();- y9 D( [* Z- p" J7 j" V
- }while((D1+D6) == 0);
. D4 W% P5 X7 }0 A( }1 ~' b$ @ - }! f, e$ Y" x: R0 C1 \
- }% ~% N& u4 o& m& V+ C
- else7 {5 ?( f& s1 c6 ~) W5 {
- {/ [! @4 M8 I3 e- u& T0 h5 t( O% M
- if((Speed < 160) & (Err < 2)) Speed+=1;
8 [( E" u7 d- i2 S0 ? - if((Speed > 100) & (Err > 2)) Speed-=2;
3 }# p5 \" Y$ w" P! D/ e% i - integral = integral + Err;
7 _9 e% M! ^8 A - derivative = Err - previous_error;8 s/ v: v. h- r
- output = Kp*Err + Ki*integral + Kd*derivative;
" P# O) P! @! H' O, e3 ^ - moto(int(Speed-output),int(Speed+output));
- u( ?+ J; F- w, K - previous_error = Err;/ q6 y4 ^! Y/ e7 }4 V
- }$ ]4 G3 K% H0 E( x2 m* P2 C
- }while(!((lightsensor_2.read()) < (10)));2 u* D7 w" X, ^& C( p
- moto(0,0);
* |3 S4 I# @5 R& | - delay(500);
/ `5 d/ d( R# _% i( c - buzzer.tone(262, 500);
' E$ \9 i4 F& i$ b7 x) n% D - }
0 ~4 u. e0 P# `& x: y0 k! f$ b - $ g( C; l* c8 y6 k
- int getErr()
% Q4 o) `' y6 @ - { - m' x g& r: e5 k6 O7 E ?3 E+ K% F
- sData = linefollower.getValue();$ m) G3 b, l6 o
- D1 = ~(sData>>0)&1;8 B' v; X( ^* x L. S
- D2 = ~(sData>>1)&1;. s4 ?3 K( W. J. s& y6 V, @
- D3 = ~(sData>>2)&1; f# {$ D- |1 ~ m' B5 i& b7 b5 ?0 ~
- D4 = ~(sData>>3)&1;' z3 z0 w# |( s+ m) F! h
- D5 = ~(sData>>4)&1; H4 a& F! o* G V7 ~ T* O
- D6 = ~(sData>>5)&1;7 v Z" x" H" V+ ~: H
- int downD = D1+D2+D3+D4+D5+D6;8 D2 ?7 s, M& Z- ]1 H/ i& z$ f
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3); f- E" b- S: [3 o
- if(downD == 0)
( J$ a0 J) k) F, F* F" u1 o3 I - {
$ p: H$ n; z9 j2 C+ I9 _ - return 99;
6 |( Y" O' u8 j& C- [, U, p, g( F. Q - }
[( X3 F" G# h/ V& H - else
# m' t2 f# l. X" L, G5 Z6 @* m - {
( m- e" b& W" O" m - return upD/downD;" V% y5 p: z- P
- }4 V* {) ~: a1 _8 i+ k
- }
( v# L, U1 D" x- P - 6 E: H# k! `1 e! V( ^
- void moto(int Speed_L,int Speed_R)
2 [/ b& J. H5 P! C+ r - {
3 a7 i. q$ B* p( z5 J) u. J F) V - Encoder_2.setMotorPwm(Speed_L);4 r: U9 ?2 l! K2 r) t# y2 g" l# a
- Encoder_1.setMotorPwm(-Speed_R);
# ^! @ H8 Q! h5 c& ?9 S& F2 o - }
複製代碼
% \' t* I, a8 ^. A" E4 ]; }5 cMeLineFollowerArray.cpp1 W" [. x" v& b; }5 ?
- #include "MeLineFollowerArray.h"1 Z& {3 E$ _) T
- 3 a7 B, T3 @7 ?& a! r; f
- #ifdef ME_PORT_DEFINED7 e" m( I/ }: J# l
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
) d+ x4 q: Z4 q" _4 I4 ` - {
& c$ B; r4 U1 o3 }5 ]8 @/ \, y! F
* V2 k* z& r; a. I- }
; S# X8 V( g( W; k' h0 S7 D" ?& x - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
: F" j* h9 e# m, ~1 L9 x% A U - {/ \9 ~, P7 {% H9 ?, {5 b/ H. a
- _DataPin = mePort[port].s2;8 L ^( r- k, D, {/ d! a( j2 {4 r
- pinMode(_DataPin, OUTPUT); 1 g3 U* a: f8 d, Y& E# ?
- digitalWrite(_DataPin, HIGH);! P9 _/ u- n6 j4 [
- }
: u3 J& m+ w* |# ] - #else // ME_PORT_DEFINED- g9 p J# f5 l( `8 t! l
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
- x7 H' d; }. L0 X - {
, \. {4 \' f: c1 G8 z - _DataPin = pin;9 ^/ K2 s( j+ j$ A' x7 S' X V4 B
- pinMode(_DataPin, OUTPUT);
5 ~ V) b# f+ E* X - digitalWrite(_DataPin, HIGH);
: Y# w6 y# X, P1 X% {) Y) n. Q - }8 P* \; x: s N: M. b, a9 r
- #endif // ME_PORT_DEFINED
$ G: r9 o! v: s7 Y6 a2 C
# a* r! K+ F$ E- X: t
7 w3 P+ g. @- t% {2 i) o- void MeLineFollowerArray::setpin(uint8_t pin)
F% Y' B( d" s% e - {6 s2 [9 l! S Y- j, [) ~0 M& A
- _DataPin = pin;
% {1 v+ k5 @7 F% q8 D" U - pinMode(_DataPin, OUTPUT); 8 a# R+ K4 |- x( J
- digitalWrite(_DataPin, HIGH);5 b% g# L& a+ a! {' ` a
# O8 z- H$ t# w! U- #ifdef ME_PORT_DEFINED; N4 n2 u/ [$ R
- s2 = pin;
: g+ j) \4 n6 ~4 W5 i0 ~6 S - #endif4 w% z) G m6 M" a
- }
6 `; ?+ _. X: O2 m. W) h2 i! u. e - " v, h( a1 _4 Y
- uint8_t MeLineFollowerArray::getValue()1 [1 l2 X+ y" l- x7 z4 E7 U
- {
1 v, m" `8 ?0 E - uint32_t LOW_level_read_time;( y' n" g, W9 p) P/ `
- uint32_t HIGH_level_read_time;
7 u& g" s2 W" A - uint32_t time_out_flag;
: b2 {- U+ R4 L) S - uint8_t Sensor_Data[3];
) C, b4 t1 |5 W! |& U - static uint8_t old_data = 0xff;- f$ k9 E5 H/ i: T3 X/ k2 j
- ~) Q) J# x- s+ {
- pinMode(_DataPin, OUTPUT);
, l0 \7 r3 m. F1 N2 O4 B! [ - digitalWrite(_DataPin, LOW);
. Z1 ~ b9 O, o - delayMicroseconds(980); n3 X+ }+ `" o: r6 a& l
- digitalWrite(_DataPin, HIGH);
( ~& V, t- ~9 s# E - % R9 y+ q# k9 Z7 U# b/ Y- V; E3 v$ X
- pinMode(_DataPin, INPUT_PULLUP);' I. c1 r- E; U2 K
- delayMicroseconds(10);
" n7 x3 _$ u' k& _ w, U2 E( \
2 w {6 E) i5 w% W- time_out_flag = millis();
4 x2 n' }: c" k7 o- [% l! `; e# N - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); M, F" _8 \9 ~8 l, j9 }
- / v0 I0 F* ?6 f+ s) Q$ E5 D3 K
- LOW_level_read_time = micros();9 ]$ W. @8 `' X$ Z7 E# n/ E. ]7 M% Q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out' H0 G0 A( ~: n
- {
5 G. A$ A2 f @! \0 s0 z - return 0xff;
8 d, C: w2 t1 F$ [, \9 w4 ~5 G - }. |7 p; o0 Y) q7 Y* X/ a# p2 K* E' q5 }
5 m' Z# e- ^+ J6 v- S* T; p- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! S3 G0 u8 f6 P8 G
) y/ @( a Y G5 u6 d7 @' w9 K- HIGH_level_read_time = micros();
( w2 O" Z# O6 K& Q - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level" ~0 T% a% [% L2 G3 V, a2 @
- 3 K _* ^6 p+ @1 ~! n
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 {! [& \3 |% t4 M - {+ Q2 U2 m8 R6 |/ C3 ]
- return 0xff;
: |, F6 \( w+ @4 s/ m" L - }
+ ^, ?0 p6 J$ z; R+ \/ d
+ z0 C2 i, y( I, k% j2 {% L- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))9 d- }3 s( H$ h# b3 X L7 K
- {3 F) n$ }6 k8 K
- return 0xff;7 }5 m0 A, \6 k8 ?* b/ |: x8 E( j# y
- }+ F3 v1 a M# o/ |9 a
- ! p6 P) y. A+ Q% E2 f
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# u2 t; d/ l2 ?. H: F - LOW_level_read_time = micros();
/ P: d9 H0 D2 J. o7 D - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
+ E, i% P& s; r) H8 q. }1 d* C; f - 6 p" l. j- G, H E3 W7 P1 N1 t
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
8 [ D5 \+ r& m - {) ^6 L6 I5 U* K+ M$ W
- return 0xff;4 C$ v* }: r3 ^+ ?
- }* K b4 U+ i( W# O$ x+ R0 m
- 3 u. c' k; F0 K# F7 P, n& R. l
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)), J. w+ t4 O# {5 G* w7 D
- {% i5 }% v, o5 b D; ?6 L% Y6 M
- return 0xff;5 d" ` \" y+ i& O8 x$ `) b
- }
' C& u J- h+ Y; C8 X- }' z" G
0 C9 c& m# |' q2 `4 M+ [! j6 a- for(uint8_t k=0; k<3; k++)* v, [' k) A# u9 L, g
- {: I. i7 k* g+ F; [, g
- Sensor_Data[k] = 0x00;
( t3 X8 ~$ {0 O3 ]6 o - . |$ R. S" s" x0 ?' i
- for(uint8_t i=0;i<8;i++)$ [* S+ g2 j+ Y/ ^ p; ]9 w
- {! m) f$ v# b) P5 o3 y6 t0 o7 ^
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level7 W E! T; T9 Z6 G3 r+ _; d: j
- HIGH_level_read_time = micros();
: D! C8 M1 D% y, j( k - LOW_level_read_time = micros() - LOW_level_read_time;
k5 ~. b" G8 b* h# T - 9 Y" t2 p) a: i- n1 R
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
( S* s" |( ^& m2 d - {
3 b" {6 E; `: C! ^ n, A - return 0xff;
! G! Q# h' q' f9 v$ x) z( Q, R - }. P+ d# x' G0 N; v" w
- + m% \2 R4 G( T4 p$ _' Y& i" d3 X
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 t. t2 g; i3 u2 }8 F
- LOW_level_read_time = micros();' Q, O) @9 m/ `: G" l( F) e# ?4 v
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
, s# T1 m) I/ q0 z8 k6 i
' v; a2 Q# k4 J, ~* f# _! J- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1( Q* ]) r+ [" e3 K- U
- {
6 @) D, {" Z6 Y9 @: x) w6 P0 j- Y - Sensor_Data[k] |= (0x80 >> i);3 k( d- W F$ {. b) x
- }8 _: z$ k4 B- L8 X$ {3 h
- else if(HIGH_level_read_time >= 100)+ O$ w( n% `4 _
- {
5 I' [' N" }* T# T h - return 0xff;
. t4 Q _, x3 M/ b+ d9 X - }
3 Y5 d/ T1 O& s& e# N! P! c, a6 v9 b - 1 w; Q$ t1 z4 q X1 P
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
; V9 S; j3 P2 U, D7 k - {
6 R+ A- R2 q, [2 S" n% x - return 0xff;
- b, O5 z. V/ X! Q* o' c - }
8 o1 y+ j; L0 {6 z" \6 M+ N6 t - }
. r. {& G5 P3 m4 c Y4 ~ - }
6 {+ o( [. D* n, g4 ]0 Z
7 e* a2 e u& O$ a% v- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level5 e% N% e, G4 D4 l8 u+ }
- HIGH_level_read_time = micros();
: f. I( V! N* U4 p+ `) F) Y* d1 A - LOW_level_read_time = micros() - LOW_level_read_time;
5 U1 t" L) T2 h- d
6 s4 P4 S) _) Q+ _: n+ z- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
& a4 B$ F$ o- E c& K8 o - {
) B P+ ]8 \0 m. ?" } - return 0xff;6 ^1 |8 a1 S! g% M2 j
- }
* f( j+ O, U- w$ C- Z) H
# H* u+ ^0 v5 N* W0 h7 ^# A' o- pinMode(_DataPin, OUTPUT);1 B1 ]" ^* Y4 b
- digitalWrite(_DataPin, HIGH);6 W f. \+ v! @
- ( b- Z/ b* w* ` x0 r
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))7 D0 D! D/ L F' A9 W
- {9 D; U4 o a |- C! l5 f
- old_data = Sensor_Data[0];
8 y5 n+ S8 L% l* ?6 \ - return Sensor_Data[0];. b8 K. u# O! m+ [4 R' j' g4 N4 c5 n
- }: X' B! n+ T6 G7 N- d# w) q
- else) z6 g; X7 N# G
- {+ r" _, e: F' t# b7 z0 ]
- return old_data;2 y- ^2 w& X' a4 y/ B
- }
9 I+ M5 f3 p) J0 t# t2 b" V - }
) U- Z7 e; ?8 F9 c0 C" Y0 i
複製代碼 3 @" }3 X4 c' j
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
" u; V9 O) ^- v4 i3 b0 C) \' `! B# D% L4 D9 a5 [2 d
|
|