|
|
4 r. c/ w0 A% Z/ V- #include <Arduino.h>, N5 r. a: Q* x3 M- j- [
- #include <MeAuriga.h>" L' u0 a! i2 ~4 p$ d: r; u8 E
- #include "MeLineFollowerArray.h"
9 N9 K0 B9 ]2 ]" z+ B: m, K4 @
. W, s( i# o9 |* l8 z- MeEncoderOnBoard Encoder_1(SLOT1);, T s+ `0 v9 [
- MeEncoderOnBoard Encoder_2(SLOT2);( _+ M$ z# h0 A0 O% s/ H# C$ g
- MeLightSensor lightsensor_1(12);
8 }+ L" a$ \9 u* w! e; @ - MeLightSensor lightsensor_2(11);/ z& d# {( K/ Y5 R
- MeBuzzer buzzer;
( l5 z$ M6 f, s) d! }2 }3 y - MeLineFollowerArray linefollower(PORT_6);" w4 R; {2 v" n/ X
. E% g" b3 k" Q( c; g" A$ Y9 W0 s- #define Error1 1" N! o7 l0 J j$ z u* M
- #define Error2 2- x% \* q+ ^' g; u2 M
- #define Error3 3' T8 _1 m1 r1 K8 F# p- P
4 \) C5 r: ~9 w: r- #define Kp 15
6 U$ {: z, G8 S+ E - #define Ki 0.152 f- K( x4 h( s
- #define Kd 0.03
0 s7 v0 S' Y( X5 R2 B0 K
) U7 R0 M0 s) o& @- T' b- u- uint8_t sData;$ p2 O3 | R4 h' O! Y- k
- uint8_t D1;
( M; Y3 Y% D: ]* {) D - uint8_t D2;
9 j) @/ `3 {/ K - uint8_t D3;
$ U( e$ o7 q% U% W - uint8_t D4;& F2 V1 C- R( o. c
- uint8_t D5;
8 p: Z0 _/ A3 x" k - uint8_t D6;
2 q/ H( T5 f0 w; N+ J3 T) R
/ Y2 B7 O- S; y3 v' O: [- float previous_error = 0;* [$ R- ?+ E* I7 n( Q2 `
- float integral = 0;8 L. Z" F' Y) B9 i1 B
- float derivative = 0;
6 l8 b) ~" {8 U, | F - int Speed = 160;' F/ ?! z) [ T# r5 |3 m6 k3 C
- float output;
9 K; y. ?; K* {6 X- R1 ` - * ^ R& o& h2 n' n9 ?: [
- byte Left;
/ F9 @# F% u8 c5 A; M: P/ N - ! j* ^. J3 j; V; U( l! s. E% q
- void setup()
# N' t) I5 M5 m+ A# h+ ? - {/ h) J/ w1 R) Z; @/ e
- //Set PWM 8KHz" S1 q1 [2 U; p6 G9 ^$ `
- TCCR1A = _BV(WGM10);" r* {. D, q' F6 [
- TCCR1B = _BV(CS11) | _BV(WGM12);
, U8 j7 ~! } z3 w' S( s - TCCR2A = _BV(WGM21) | _BV(WGM20);5 c8 Z' a$ r9 m. T w
- TCCR2B = _BV(CS21);
, P& J4 l! U/ H; c - Serial.begin(9600);
. v! D+ b7 C$ A4 @8 q, S) p0 H - buzzer.setpin(45);2 S6 e! i5 h s! S7 u, K8 }; S3 S7 m
- }& x" s7 w W+ n/ y' o' V0 q
- 9 L+ D" f! Q% W* C. y% ~
- void loop()# \. ` I8 E) W
- {8 P) B$ ^" O& G2 |9 } J
- while(!((lightsensor_1.read()) < (10)));
! u5 h5 q" E0 _) o3 y - buzzer.tone(1047, 500);
: P N& s; S. q6 x - delay(500);8 n: N8 i" q+ @. P k0 L8 s" p
- do1 o% {# s5 C- F5 ?
- {: H5 y7 Z& ^+ l0 J% A4 W1 C7 b
- int Err = getErr();
9 ?! [& S5 F, q - if(D1 == 1). ?& R' B. d2 X8 s" t2 l5 g2 U/ D
- {
% x' X& h0 I8 O+ Y% H' D( H - Left = 1;) m' p0 R2 _& J" _
- }# H) w$ m8 [) Q; {; K5 P) L/ e" c
- if(D6 == 1)3 y) j9 }$ T2 { H2 q, p0 [: W
- {) \# I' E. H# X1 }
- Left = 0;# x |: P) ~; ]8 P
- }8 d2 @' ~, J( \" [: }4 ~
- if(Err == 99)/ T8 L6 r P4 ~0 {! l& q
- {
+ A5 ^% X! v! R8 O( ~- O& \ - if(Left == 1)
0 x: q' x+ p( G+ t( p - {+ F7 g& u- e$ m: ]: T# P
- Speed -= 5;
% o+ J& L$ _$ B/ W6 R W - moto(0,Speed);& M3 H5 b' [' |! }4 {% {" g
- do
7 o3 }! l5 q( [( k - {
0 C1 m% ?( r( u# u, K2 d4 x! N - Err = getErr();) E. n u+ w0 U9 D+ G/ i
- }while((D1+D6) == 0);
, U* O I) N( b v0 v - }0 T( a8 A+ m/ v- n+ h* x
- else
% c% `# f/ Y- N' b - {
( y N% K0 w9 x' r - Speed -= 5;
6 h# ~( W+ m3 g - moto(Speed,0);
/ E( d7 Y9 ^/ q+ p- [ - do) |( U% _5 B3 x% M5 ]' \7 n3 T. y
- {
8 I& H+ m( l' v9 n3 g; K O9 m k) F% T - Err = getErr();. Q5 I9 G$ m& S) Y" s
- }while((D1+D6) == 0);$ l0 C0 O- ^& O6 Q
- }5 w+ G. N/ h: o$ G. o4 _1 _
- }7 m% H) n2 ~5 \$ j" e
- else
( ~0 r) V6 K8 |! @& W. x% d - {8 m v$ P5 A& B
- if((Speed < 160) & (Err < 2)) Speed+=1; " N& g2 Y$ X) L$ Y( K- `
- if((Speed > 100) & (Err > 2)) Speed-=2;
4 ?. D% ?$ x& r0 `# a) u3 \6 g. \1 \: W - integral = integral + Err;3 c& B M7 }! o* m# v3 U, E
- derivative = Err - previous_error;
4 [" y, R1 V: M8 }: D/ Y2 K - output = Kp*Err + Ki*integral + Kd*derivative;6 i5 Q$ H6 L1 e! I/ n1 A; L' ^
- moto(int(Speed-output),int(Speed+output));8 \# T+ }3 e, G. }. u$ ~
- previous_error = Err;
6 a9 J, G+ w4 h5 | - }
3 X3 g7 N% h* _ C/ o - }while(!((lightsensor_2.read()) < (10))); x& w3 Z/ Y0 ]
- moto(0,0);5 w# W' M4 j: B. U1 c4 F2 i- O
- delay(500);
" i, n) J: U7 [+ H* A# z3 O2 T - buzzer.tone(262, 500);
& H: f4 i& X' x - }
: h6 X- K v% n( G% X3 I1 ?
1 p8 W4 C4 j' ~6 m) `* c% L6 c) \- int getErr()
) S. X- J" x! m" O - { 7 h5 G0 E: C' n5 B5 R. }
- sData = linefollower.getValue();
( _- G! ~- L5 e, o8 E - D1 = ~(sData>>0)&1;: {# ?; x% ^5 }* S% Y$ I" l0 g/ s
- D2 = ~(sData>>1)&1;
$ g$ C' Z8 m0 O3 ^ - D3 = ~(sData>>2)&1;7 K# o4 B/ o# p: h
- D4 = ~(sData>>3)&1;
) V" L0 H9 ]( i! o0 p8 Q - D5 = ~(sData>>4)&1;
' e$ m9 v5 Y2 A3 w( Q - D6 = ~(sData>>5)&1;9 h, B$ `$ k1 K
- int downD = D1+D2+D3+D4+D5+D6;# Q9 s& x0 Q: E/ c0 d* C3 _/ V
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);1 X8 T8 Q8 G8 ~
- if(downD == 0)8 f; d: a" [0 B9 z9 S% l
- {
5 c' Z, L# L& S# k" V' k+ d - return 99;+ `5 D6 o* H' x# _9 ^0 ~6 Y! L
- }
8 ~. c; p% a8 n2 n5 D1 w/ v - else
p$ L( [# P2 j - {
5 U% \3 C. a6 S1 O3 O* y% Y - return upD/downD;" y) x( W Y7 }4 Q$ m
- }
+ O9 w% _6 S8 T2 b+ o; |, a - }
! u! u9 B$ V+ b: g$ `$ h7 a. \, S6 D - 2 @/ M& @) s. c' Y( @
- void moto(int Speed_L,int Speed_R); _. K) f t/ z! j
- {
7 F: \& j' A$ ]( r - Encoder_2.setMotorPwm(Speed_L);
: r) C4 X5 p% T5 ~ - Encoder_1.setMotorPwm(-Speed_R);: I! _. y" h4 b- v+ S7 S. C
- }
複製代碼
0 [3 h/ A8 i0 k1 p) }MeLineFollowerArray.cpp, B& i$ [# }" O. L c& U$ `+ t
- #include "MeLineFollowerArray.h"" A& f: s+ K( C8 ?
9 K( e. O. ^( s6 _6 W- #ifdef ME_PORT_DEFINED4 n6 H# W y9 \+ A5 o! {
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
6 |( a3 g2 B3 |' F( n/ a; h8 |* I - {! w8 V9 O% _% _; s/ O6 @
- 3 P1 h8 N) k0 V" u. D
- }
; D4 C3 _, f v9 i U7 }8 m - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)9 Q1 l; t& K; u, o: b, |
- {& f$ d9 W( v- [
- _DataPin = mePort[port].s2;$ z9 W% g" N4 j0 y& S' k
- pinMode(_DataPin, OUTPUT);
, c6 P' x5 y/ t7 u) e9 { - digitalWrite(_DataPin, HIGH);- h& S' b" Y& u% v7 d% ]
- }
" E/ v1 g) J6 r- L - #else // ME_PORT_DEFINED
p& I5 X- l: A/ M* Z - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin), F4 \; e: f8 U, ?
- {
( ?* M# A0 p1 @/ I9 c* c$ H - _DataPin = pin;+ t8 c( g% m& ^0 c: \
- pinMode(_DataPin, OUTPUT);
8 C3 t9 h+ _4 O; j - digitalWrite(_DataPin, HIGH);
8 q5 M" Y W5 g: s - }
+ S, G9 f7 W: Q! q - #endif // ME_PORT_DEFINED
8 Z+ v/ }. E/ Z; g' d' Z- J" U
3 U. z5 L3 L; c; d; @# u- 5 z0 b* b. ?- r8 V6 c; s8 w
- void MeLineFollowerArray::setpin(uint8_t pin)6 D( n+ E) F8 H
- {
+ h2 G% \, i* u" x$ K1 b+ w5 T4 k - _DataPin = pin;
% x, K7 p7 o/ ~- y3 c - pinMode(_DataPin, OUTPUT);
/ T/ G9 d& c( w4 T- f - digitalWrite(_DataPin, HIGH);: \$ h# y H% k2 Z
4 L5 h# ^# X4 T5 K- #ifdef ME_PORT_DEFINED3 d; t! l1 w, s0 G- }5 q- i# ?6 j, O+ {5 c
- s2 = pin;
/ F7 @' O; g2 R0 _ - #endif3 H2 T9 q: X" j" ~1 |* q& R Q0 B' p
- }
' H- k6 [# |2 b1 ]- |. [
& k& ?" u* O y. k5 T) `4 R- uint8_t MeLineFollowerArray::getValue()
9 V7 F2 o( @( q& z( f4 ^2 a - {! _8 e4 \3 A7 S3 I4 ~' c( G0 K
- uint32_t LOW_level_read_time;
% O/ d% I- u# a* } - uint32_t HIGH_level_read_time;8 M, y7 {* \: H, s
- uint32_t time_out_flag;% N- ]1 y' I9 P; ]7 p0 K: C
- uint8_t Sensor_Data[3];0 |- D; ~4 B2 I7 ~* V- M
- static uint8_t old_data = 0xff;
5 p R, x# M: R3 C, J" u0 \
: k4 j: d& x7 \6 j) d( u- \1 h* Z1 }- pinMode(_DataPin, OUTPUT); y* M" n) a2 n s4 l* l) `/ F
- digitalWrite(_DataPin, LOW);9 C* q* s3 L2 F7 h+ c+ f) ?/ C! V
- delayMicroseconds(980);
8 L7 n2 Q2 ~% o4 A) [) t- u - digitalWrite(_DataPin, HIGH);
* l: K3 J/ e/ }0 G$ Z - ; x! G) @; D- { X% A. z- z- ~4 e
- pinMode(_DataPin, INPUT_PULLUP);
) g& }* b' l( V3 I% f5 P$ @ - delayMicroseconds(10);
8 O1 x3 |$ S" K. r! \( ^: a
/ M$ P" J9 \1 L# E2 z- time_out_flag = millis();
- Y/ `2 M4 f3 L1 O: |, B; w7 v - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
f" _- p; e3 A5 e - 7 r' M* N8 a- { c6 }1 _
- LOW_level_read_time = micros();
. E) F* d# y0 A8 x" ^1 G, Y" Q' U - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
, C& A& Q' @" o# Z' c - {
+ y) a% i, }* G" ]& y. |/ [! ^2 a - return 0xff;
( r6 n' {7 Y; p - }
% e2 O9 z- |7 V' d# ?5 F
- ]* V9 e7 J/ D- Z7 @! |- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
~6 L" e% \( U2 e/ r% f
" F) u4 H$ `; Q- l- HIGH_level_read_time = micros();, _( x9 R; s; V# l j& s: @6 z
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level* f d r4 Q" C' \2 `
- ( h2 Z( g: ~9 I
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out) v1 \- I2 b- r' @& f9 s- B
- {6 [, B h6 X G* z1 j
- return 0xff;$ ^* S4 ~0 r# a$ k' O) W- Z
- }
U& \) }' `* ^ - ( X) W! H7 r$ X5 |( m' N7 K
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))' l% t8 B7 R5 a) f2 V
- {6 V1 ? A7 p0 Z! {. V7 c5 C5 i) A
- return 0xff;( }' ^/ t3 x4 F- d& r! h
- }
3 Y; A% W+ l3 l9 `$ |, \$ J - 8 q9 W3 J- O- }( V4 j- B
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
8 z3 m4 r3 ]7 }: t S5 o4 ^ - LOW_level_read_time = micros();, D* r+ y" G& [. h# c0 G
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
- U! s# f4 ^8 t0 {$ A3 B4 i& \8 K0 E4 ~
1 Q( g1 q- |( b5 B7 L/ O- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
# G# ?- m2 M4 t& y) t! [* Z" e - {
! y, F# A6 }. T! z! A0 c: h( E& Q# k - return 0xff;
5 C l. E4 F! L/ q& ?+ d - }
$ [1 ^. N5 P; I$ Y/ N& A0 X - 4 C' L1 T# V1 ?$ C* k; x$ F7 \' Z! \
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))8 ? G7 C( {, a- c/ U
- {
& P' z! B7 F5 A+ s$ C - return 0xff;
5 u( a# W+ D, F$ g; p9 x1 a - }4 a! M& L- f, p5 x' r6 L$ a6 R
3 i6 A- |7 |: b1 G Q- for(uint8_t k=0; k<3; k++)
_; t' r2 P9 S3 M, q& B9 M. Z - {
2 |8 V+ o, b! e& q$ q. L% { - Sensor_Data[k] = 0x00;
$ R, u, j& i3 R. M: R0 \0 T - b$ I \* } `2 F E+ Y, Z
- for(uint8_t i=0;i<8;i++)# _) G) c: P" O0 a2 @8 I7 S
- {
8 e( [5 t; H$ Y, c - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level4 O+ @3 G" L) P3 }
- HIGH_level_read_time = micros();
# t7 Z% e4 H# j* S3 ] - LOW_level_read_time = micros() - LOW_level_read_time;' i3 ^7 ]. v. {6 Y1 ]% [' }
& d! Y, O9 l* o" G" N( T) w- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
% S' Q: d: ~- W1 a - {
- S* U8 J8 L7 s/ o/ T - return 0xff;9 L( v& ?- m! }2 M
- }
6 x$ Z8 [9 u: S- t t; o- Q - 0 N9 @2 M1 I( L% {5 `* M
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );! l0 z) B( G* D
- LOW_level_read_time = micros();, K6 R* t. N0 I% p: [
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level) `. t& C' V4 k6 J( X! m
- ( e4 x* Q: F* p4 {' c6 E. R
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
+ Q; o# a+ ?8 J - {
& v& `0 q/ j' J- U3 c, e - Sensor_Data[k] |= (0x80 >> i);5 ?0 L) W+ c* d& X4 i2 a- l
- }* {% o7 ]& K1 U3 Q4 _2 Z* l
- else if(HIGH_level_read_time >= 100)! [5 s) X b; y& a
- {% a* {, w( q7 m' A. m" ?+ V
- return 0xff;
x7 _# W) @: }1 o* v3 a' U - }
; K; x/ i& p- u
' j1 l& ]8 V( ?6 C# x$ E- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)) Q7 @' o6 s) r4 I0 W5 ^ _
- {
& |0 w% x* ~3 h5 H Q8 \4 u4 f - return 0xff;6 \; O1 u8 L: L. ~
- }4 c( P7 w; I, r
- }
) L: G( e. H) O, [ - }
' V% q( [7 H5 N5 l% g
) X; T& W* X8 c- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
& C* F) N8 a4 l, b* T" }9 Z: T - HIGH_level_read_time = micros();: c$ Z. P9 {/ v$ t; d
- LOW_level_read_time = micros() - LOW_level_read_time;
, V: H/ w. b8 W' n' C C) |
, D% [7 t4 |5 W* @) V# A- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
. m4 \$ m4 I! U5 _/ j( s - {
! S. l8 M2 E8 v8 [4 U - return 0xff;2 B9 U5 B7 C; i# @6 i3 o
- }8 c( x0 l9 W3 Z# o2 w
- 6 x$ [5 p: @& Z# L6 `
- pinMode(_DataPin, OUTPUT);
9 G$ I, g" u" w% P) e# d7 c - digitalWrite(_DataPin, HIGH);
. h5 J4 d t; [, m6 H3 [ - / v+ [, w7 `" D% a" B/ q
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))5 `" \% t/ I& [7 n
- {( q6 \5 s* E( [ @" k
- old_data = Sensor_Data[0];2 {/ n; t. k X
- return Sensor_Data[0];) [0 H- s" \6 A G' H( K) o2 h- E
- }. H J7 [5 H# m6 p, `' F/ R1 B
- else
* m M0 q8 ]$ f5 |6 y - {
0 y( F: g; R1 } - return old_data;9 O2 v1 z: H( t! d
- }& ?5 V- ^/ q, }2 W# ~: t
- }
6 |( j% v9 M" ~( e. z" l. b; l3 S
複製代碼
9 Q; N9 Z4 [4 {, T- @1 g# W+ E; i! J6 Z3 DMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
4 c5 I1 [6 _/ ]2 A% @+ k7 d
6 w2 B1 @4 R1 o, l) ]5 r
|
|