|
|
$ B2 b! R- r6 r; l. T6 U
- #include <Arduino.h>! y- e* W5 s$ ]1 t4 Q* Y( {
- #include <MeAuriga.h>
~, X' H T p& H; V8 ?! |+ S - #include "MeLineFollowerArray.h"
3 d- S. z! H/ y6 P" Q- B4 I
; K* ~) I8 }- n: w- MeEncoderOnBoard Encoder_1(SLOT1);
5 X! I/ A( W9 G+ m! j - MeEncoderOnBoard Encoder_2(SLOT2);
0 a$ W6 f& `& ]0 ]8 u - MeLightSensor lightsensor_1(12);1 w7 q! b+ V, p5 ?" m
- MeLightSensor lightsensor_2(11);0 D& m* d2 T7 h- \/ O8 Z! Y6 m
- MeBuzzer buzzer;
. T) d! @1 W) r& ^: ]3 K - MeLineFollowerArray linefollower(PORT_6);4 ]. | q8 k; E. ?4 [! }
- ! E6 p! B# D: @( ~
- #define Error1 1
3 {$ o8 c8 b0 @3 z0 `6 Y - #define Error2 2
% h9 w" \( R+ [! s( K5 F/ Z5 M2 i - #define Error3 3& h' Y9 Q) r+ E0 x
- ' S% g8 ~, s6 d' _4 x
- #define Kp 151 ^$ N. _& i+ Q
- #define Ki 0.15, Q* S& @0 F2 C0 \2 S! B
- #define Kd 0.03* M& I- ~9 E- s6 T- n
- % T& G0 x( y N% a! r. Y
- uint8_t sData;: x; V6 H- i: t3 Q1 h. }
- uint8_t D1;1 B+ f9 }- i w& p
- uint8_t D2;$ o5 S M0 j* C8 g; t3 E {* u
- uint8_t D3;# Q) S" T# @! f& P1 I. j
- uint8_t D4;/ n; ]3 @& C" D* T% M a
- uint8_t D5;
# k: w2 d9 b m6 W2 ^5 v - uint8_t D6;( D4 {+ q; f$ T* }
- # O3 T0 }$ I4 ]8 c
- float previous_error = 0;
- l# \8 Y. m1 ^8 ]# { - float integral = 0;
. W( M8 v4 I) s5 {$ D i+ ? - float derivative = 0;
) ?1 h3 A3 L' e8 Q8 v: c0 E - int Speed = 160;! n/ ^3 h9 J5 d; K/ g# @
- float output;
) L7 z' F5 y0 w+ i3 y# @
, F0 D* a! h( s: C0 a3 D- byte Left;
- D9 C, K, q! P; X. N
4 X/ U5 i8 O1 Z7 C7 x9 Y+ j- void setup()
: w5 Q9 \" ?! |8 x; l! f+ ` - {- l* |( X0 {! v
- //Set PWM 8KHz" J" Y( r1 H( |
- TCCR1A = _BV(WGM10);
, J) w1 \" ]+ [" Z - TCCR1B = _BV(CS11) | _BV(WGM12);2 H$ d6 i2 E/ h- X9 s& I
- TCCR2A = _BV(WGM21) | _BV(WGM20);
, q b6 V+ i% ]4 H/ Y# F% Y) S - TCCR2B = _BV(CS21);4 h" I5 Z9 h% W; W1 @! h
- Serial.begin(9600);' M5 F6 `, D' ?$ K
- buzzer.setpin(45);5 W R$ h$ N4 T5 S0 M9 L* |
- }
8 q" w& X$ I/ Z# y - 6 W2 L, x4 i) e" v" ^3 O
- void loop()
) c8 ?" q5 z4 C - {; h- k+ c' y# j! s
- while(!((lightsensor_1.read()) < (10)));& w& r. A: \9 A7 [
- buzzer.tone(1047, 500);; ~+ b; r5 ?' B/ T; u
- delay(500);' X7 P: K, C+ b9 j' `& R9 m
- do
8 ^+ S6 l7 r5 a7 C: r, S$ o - {1 T3 _ k- h+ e& E |! I d
- int Err = getErr();6 p6 I$ l! k2 I
- if(D1 == 1)
/ j1 U# M+ O: K2 w$ L2 w - {
. c$ y4 A) [6 z1 [8 E' P$ s# _ - Left = 1; }7 q" u' J ~2 V" \9 m
- }5 }, X6 W, q3 s2 g' X$ S
- if(D6 == 1)
* B- S* Z1 @ ` - {* E* V. O) D' q, q7 R" i7 T/ ]* v
- Left = 0;
6 E# E) d+ F5 F8 W: h8 G$ g$ n - }
1 ^7 Z4 D! T& B: f; j3 c% r% m - if(Err == 99)
+ `/ E0 ~8 s3 G4 W. E, P( ~- _ - {. V6 r3 f+ o1 Q4 k& d
- if(Left == 1)4 b) S9 W9 a/ F; _) W, a
- {; A1 t# V. Y" i9 z8 Q9 p
- Speed -= 5;7 a9 g4 l. I4 y! e4 e- J* x
- moto(0,Speed);
: A$ g, y% |! |8 r: j7 [/ v - do4 {# ^2 L3 j ^ {: ^
- {
# q- q# p- `1 E& o X - Err = getErr();
' O/ ^8 O _4 h8 u' v - }while((D1+D6) == 0);+ p' s, f7 u* r1 r1 x3 C
- }( U: Q- ^3 o: h0 S
- else
9 x9 I( n$ C' m- u; S - {
- W6 }6 Q+ {7 d0 I; J! n! }& T - Speed -= 5;
/ }7 U* g( y. @7 h( K+ G L* x7 H - moto(Speed,0);
0 f+ v) j: g0 b$ {0 D - do
/ C, p+ U6 h& D& K2 I - {
; D X0 B# n2 d( c; X5 J+ J - Err = getErr();
7 |: v+ M* E# ]. s* Y - }while((D1+D6) == 0);( ]2 c7 Z x4 Y: |2 Z( v9 k% W
- }
3 H( U& k# ^; |+ Q4 ?8 f1 a- e - }
: e+ W1 p: d! H - else
$ D- }8 H7 N* d* D b0 i, W% l - {
% U& d, R3 U6 t8 U' Y9 w - if((Speed < 160) & (Err < 2)) Speed+=1; 3 M5 W* D+ b" i! c) A9 ?: @
- if((Speed > 100) & (Err > 2)) Speed-=2; & S! t) o3 ~% ^! h) w( k$ W
- integral = integral + Err;4 R- p2 X! ]4 L% Z5 t6 Q2 X) a0 L6 _
- derivative = Err - previous_error;
0 N( z, m- G7 X - output = Kp*Err + Ki*integral + Kd*derivative;
( E8 I$ u/ ~& w& K8 h8 M - moto(int(Speed-output),int(Speed+output));) w/ c( E( V& ]8 ~% ?& I' {
- previous_error = Err;
, l8 ?/ C/ U& |+ S+ q; S - }
2 x6 R1 J( c; S1 M, U. M# D! O& Y- f - }while(!((lightsensor_2.read()) < (10)));
6 z |# V6 z2 T( `& | - moto(0,0);
) Q2 O: x# a7 c3 X - delay(500);
$ X6 c5 E# j' l% k2 P - buzzer.tone(262, 500);! y7 c ?$ R, |1 }6 O1 U# o
- }
- W+ F) F5 V+ z. f, F8 ]
! q' D: k1 a, b% y b' H7 R- int getErr()" u+ f' H: F( @/ w1 a) M) Z y( ?
- {
) g* I0 {, p9 `2 t" f2 O8 e - sData = linefollower.getValue();; P6 a: C6 g* g) N, \* p
- D1 = ~(sData>>0)&1;
8 S/ G3 ^7 f0 L. _& `9 f/ H I - D2 = ~(sData>>1)&1;: X$ F* `7 L* W
- D3 = ~(sData>>2)&1;
- r& f% j' T% X! o! C3 T# n' @ - D4 = ~(sData>>3)&1;! D) D) d! `) S2 m, P+ {) Q
- D5 = ~(sData>>4)&1;2 e0 A9 T0 S; _+ e7 @. }
- D6 = ~(sData>>5)&1;
* j* \8 V% j. j; n) j - int downD = D1+D2+D3+D4+D5+D6;
" E. P* _' h7 |5 T, n% q+ k" m! f9 ?) P# J - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
$ _' E& ~9 C8 A: d - if(downD == 0)
' K+ u* A4 U* u# I) i; H - {
2 Z& i9 _; r! C" n! E - return 99;1 ?2 j8 j+ C4 T& Q2 j5 H x6 l
- }* o$ p) J; ~! [% m' B
- else
% b. i5 ]( Q% s# N1 E - {1 s& p/ ], z1 W
- return upD/downD;/ t( b/ _$ b0 K: Z3 X
- }
' p) b% t5 ]( K/ `! ~ - }0 A" d5 E C9 D
- + w% Z) K; ?. A/ @* R5 D- D% }9 p5 r
- void moto(int Speed_L,int Speed_R)7 ?# ]/ W: x# o
- {# f/ ]. s* p4 l6 E
- Encoder_2.setMotorPwm(Speed_L);
7 g( S: u+ }* X7 D" X9 r- }) e0 R9 J( E - Encoder_1.setMotorPwm(-Speed_R);
+ |/ t/ l6 {) X2 R - }
複製代碼 6 U% m) L. P7 e1 E7 Z
MeLineFollowerArray.cpp0 x; d+ G0 Y$ I) g5 C! h
- #include "MeLineFollowerArray.h"; _2 J1 I1 V' G9 l7 p3 z
- % K' f6 F- i. e- t5 E p1 [5 B
- #ifdef ME_PORT_DEFINED! d, s! m$ u) H* F
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)& k$ k$ n& ]& x+ l( P
- {! i4 q( m4 D: c6 R) w3 w; `* Q
3 O* K i0 P! S! y% ]1 Q0 n- }
& {/ ?% q8 @- f8 H0 Z - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)7 z8 e+ y& a; s& I* b+ b; g
- {: {' u, l9 H! n# V# I$ @5 P; ~; R
- _DataPin = mePort[port].s2;
' q( F$ h( ^: n6 o" z# R/ R - pinMode(_DataPin, OUTPUT);
7 ~6 {6 _9 j! f% T3 T& l% Q+ m - digitalWrite(_DataPin, HIGH);# Y8 n* H/ a6 I+ F- [1 I
- }. z+ l1 i4 X$ ^) _2 a$ d
- #else // ME_PORT_DEFINED/ x% P2 S5 d5 S, n% _9 b1 p
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)$ C5 N( E) k6 @8 N
- {
2 \6 I" R$ v" `6 F) |! { - _DataPin = pin;+ T. h# y# K, S V. A C
- pinMode(_DataPin, OUTPUT); 1 ?4 @+ X& f- i* G
- digitalWrite(_DataPin, HIGH);+ K8 t* K3 B8 n, z9 W6 o1 K; _
- }
_2 U$ V* a; z3 h( I) p; n# w R G - #endif // ME_PORT_DEFINED
. n( {- d4 i/ K - 4 T" a4 m- L0 W5 }9 `0 L
2 ]3 @9 O) @3 B- void MeLineFollowerArray::setpin(uint8_t pin)' ^/ j/ C' X0 H
- {2 l# n- w% j A( b
- _DataPin = pin;
: |! h. r# J; h! r - pinMode(_DataPin, OUTPUT); 5 r* r# `6 i4 O7 B- L, {' f) H) l" }
- digitalWrite(_DataPin, HIGH);" I& ?3 p- o$ L n
9 a( n; c+ k9 G8 y" _3 R- #ifdef ME_PORT_DEFINED
9 H8 v# j- s: K/ J2 S - s2 = pin;( B+ @& M1 h* a# h
- #endif
- r6 @& v V8 X# l& y, l - }7 H+ e) e9 d9 b) U3 q
- / Q y+ E- A- A
- uint8_t MeLineFollowerArray::getValue()0 [" X, t2 Z: N8 M6 O$ H. V) L
- {5 b( U- Y- w' w4 G- F4 H
- uint32_t LOW_level_read_time;, }/ q( A4 f7 Q E: \
- uint32_t HIGH_level_read_time;) q6 s1 r0 W9 m% ]2 {
- uint32_t time_out_flag;
: V W& ]/ y* ~: l3 {6 t$ l - uint8_t Sensor_Data[3];
. J4 e0 B. J; w - static uint8_t old_data = 0xff; h2 h0 j* _4 H! b6 @
- [2 x+ w1 T0 I" n/ V9 ?- pinMode(_DataPin, OUTPUT);
$ v1 g+ y% r- b3 O+ t - digitalWrite(_DataPin, LOW);: C/ x. Y4 N0 N; w% L6 ?5 F
- delayMicroseconds(980);# A' U" q* u- ]1 k$ l* x
- digitalWrite(_DataPin, HIGH);0 a" a$ Q- |3 O+ `
/ d' ~7 j. c, b8 t: I- M l- pinMode(_DataPin, INPUT_PULLUP);+ a. t! f. O% s
- delayMicroseconds(10);! `) a0 Z k6 n
! `: c' T; N) ^0 Q* c- time_out_flag = millis();8 @* `3 T( l4 |8 K; B9 ^
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- g: a' I9 K1 a7 P& a2 i6 n' w - 4 C$ u9 u c- ]* z6 M# q
- LOW_level_read_time = micros();
7 c9 p- s- S! {6 `- ] - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out' M2 s4 w: _( G! |
- {
2 F8 g v3 y! `" _0 p - return 0xff;
0 \5 i0 @$ R: c3 W& G. c5 ? - }
9 t, Q! f/ C( Z$ m. P- y& C - ; [) {% d6 h: [( u
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, ~* d) A) ^& i
& R4 ?& V0 H7 K& C. o- y* `& h- HIGH_level_read_time = micros();
, G( D6 ]9 b5 K8 l - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level0 [6 l& ^1 y2 F" J2 `1 v
- % L+ s# g+ `, ~
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out5 N* h3 x4 p7 P7 m, T
- {
9 x8 R9 {* w5 p4 M6 X& `* D - return 0xff;
! H- y9 z* m8 j& V - }
y, U; M% h8 K w) a7 x - 3 _" `5 ]& {( Y! ~
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))! S1 [7 j( z, ]+ L) |0 W
- {' D3 s, G; G7 ]( y+ `2 L+ A
- return 0xff;5 m2 E% D. y) Y( C/ d" h
- }/ {1 z6 N7 U" X4 V8 K: _0 M* Y0 {
- + ?/ R, E1 E; h9 i/ o5 @
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
" `( J0 X+ t2 d/ G' l - LOW_level_read_time = micros();
" O+ B0 ?4 ^7 s, [8 v, t - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level/ ?2 H, y: i0 `! s) O1 |
- - \* V5 V7 A9 m) T, j% ~1 p
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out+ D Y5 a: k% t$ ?" O' f
- {
$ T8 e$ ^# e, g- V3 K - return 0xff;1 |( ^' n, u7 C3 h; M" Y/ m2 `3 n' F
- }( X3 ~3 j7 a3 W) w! N/ P
- 6 L& T. o* S2 ?' h# T( [. o
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))/ e' m/ A7 Q8 D6 l3 r6 n
- {/ j$ d; j7 O, U2 h. K
- return 0xff;3 H0 U) ?! H5 y' u
- }1 i+ `1 V/ g {. v& z
- 4 x% O) N# p; n" H0 {0 H
- for(uint8_t k=0; k<3; k++)( O3 f7 w3 H. n, r
- {& R2 S, z, i, q
- Sensor_Data[k] = 0x00;9 {! ? p% m, u/ {
( s: H" K4 R+ l( V' V- for(uint8_t i=0;i<8;i++)
+ P6 Q h8 I0 z' i9 g - {4 I$ ]) J0 o9 H M5 P1 L
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level* [ p( t* B/ W; E+ _: p: a
- HIGH_level_read_time = micros();: g1 j$ c1 ~ ~4 I" f$ @2 f5 }
- LOW_level_read_time = micros() - LOW_level_read_time;
1 y z9 o7 [+ K+ w- \ - 9 y/ d1 A/ K6 S) O, h: O
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
; c7 Q$ _; l* B+ T4 F& L - {
3 ^! S. ]: H) q$ h9 S - return 0xff;
( n3 j4 U, H. E - }4 w, q1 F7 {" J) F, U
- : ` J: v- Q9 T4 P' z$ S; a) r" B* o; {
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- ?& {; Z- z$ B( n6 S - LOW_level_read_time = micros();# y( d3 U+ r9 r8 O, g
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level% J2 X5 f; k' N) H( s2 H
- $ l w b9 Y$ ?, C/ X9 r- l
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
7 S5 z5 H+ r) _1 ]4 {. Z - {4 f; V2 o$ Z; d/ O/ Y% \
- Sensor_Data[k] |= (0x80 >> i);3 q# p( N! ?9 I
- }
% |; `6 k8 Q+ q) d' f2 p5 j - else if(HIGH_level_read_time >= 100)
9 _# g# x6 L4 ]# S i) |- e - {
" n- P: }+ a( z' R; b: X7 N+ @ - return 0xff;
( O* p8 D! g- e& E - }
. A/ p7 Y% {1 N
' h! N; U$ d6 D- }" G6 l+ e- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)5 G$ q( ]/ E5 I7 B, _& O* P+ y
- {) U2 T* ^% C' F! u" D, o) ~- } @" X6 y
- return 0xff;% s# a, Y- B! m; |9 V; a$ ^' X
- }
! W1 l! W( x' k3 d9 F - }% [1 N5 C% `* \7 M8 ?' f
- }2 _7 {+ Y' X2 b- _, U0 i- L3 l
8 w- k; b6 ]$ ~3 ~3 Z+ \2 Y- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
6 Z6 S$ o) \8 X" p* A - HIGH_level_read_time = micros();* L: i$ M: @+ C5 w
- LOW_level_read_time = micros() - LOW_level_read_time;- t9 N+ w$ a. Y( @
- 9 _7 w) [) r6 n- q/ H) k
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )- A& n% ?0 T& H& N% o
- {" p0 {% d. M3 w2 \8 ]6 S
- return 0xff;
+ Z( R# r4 i; W: K& e' ^ - }
! z# ^6 a- t' s) Z' F! }
: \5 `% B/ v, w# I: ~- v) I8 j- pinMode(_DataPin, OUTPUT);- ^+ K/ J) W) x" M3 ]: d+ n
- digitalWrite(_DataPin, HIGH);7 S8 ~+ w% q3 B8 ?; J, M
- 1 Y& `$ p: f9 c% d
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))9 o2 r* H) U- P
- {
, Q. p' L. S: y ^! m% P2 y' S - old_data = Sensor_Data[0];
- I# D( t+ j5 E% R - return Sensor_Data[0];/ l. n4 B8 Z9 ?9 w( J0 Q
- }
. {* k# M- t0 U - else
4 ~* Q8 T2 B, o8 ` - {, }$ u! I. ~ W( w/ N. k
- return old_data;3 ~. ?+ L, h* v+ y% U1 x
- }2 ~8 T& K2 c% j7 F$ d8 }
- }! ^3 H; E% E0 [" K, |1 e5 Q4 F- p
複製代碼
4 y$ e, ~) l. q. q2 A& m( h5 IMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
( T2 E. e) @, j1 k/ H! ^
6 R# f" O; g9 R# o( g* W2 j |
|