|
|
8 w. n* C: v( V! f' o, n
- #include <Arduino.h>
! `+ T* q* L3 W) V9 |! c& w" e - #include <MeAuriga.h>
( M2 @! _1 h8 ?, _$ U- a/ m - #include "MeLineFollowerArray.h"
. E+ s6 v% W. \ - / W! {0 h, L* K1 R v
- MeEncoderOnBoard Encoder_1(SLOT1);
$ V; {$ N: b& r) ~. U - MeEncoderOnBoard Encoder_2(SLOT2);$ e/ g( E) g/ B/ t3 f7 _, j2 o
- MeLightSensor lightsensor_1(12);4 X2 H# f0 ~! t; E% b3 ^- L" m
- MeLightSensor lightsensor_2(11);& u$ }) b: [1 b. d7 \% p
- MeBuzzer buzzer;6 N, f/ A. }/ w) L, W( K6 x
- MeLineFollowerArray linefollower(PORT_6);
; t# \5 x. ]: }! f. n0 m - ; Q O. o9 ]9 M5 O8 Z
- #define Error1 1
* j3 Y* ^" S' q; A - #define Error2 25 i. ~" V8 z: b* I& L3 D5 S9 t
- #define Error3 33 W- @" E# z2 S# K" H% Q4 l
4 j; Z# @' w, a) G7 T- #define Kp 15
3 p0 U# L; d% F - #define Ki 0.15
8 s, c8 w+ C$ H9 L N$ ` - #define Kd 0.03
$ }! ?0 Q' o* V T# {" ^
; s3 ~- t0 i: A$ x- uint8_t sData;. c) f$ J R5 \! G7 ?1 O* g
- uint8_t D1;
. {0 Z3 b# x8 D' K* S5 _3 L$ x% @, u - uint8_t D2; x. K! W0 q! K7 J9 _
- uint8_t D3;! Q% J6 e9 s. e4 o% v4 L; U& Q
- uint8_t D4;
& c0 r$ F& J1 ?( V6 E: ? | - uint8_t D5;2 T0 m; e% F, z5 f
- uint8_t D6;- ~% p# H( X' O1 U& y6 t, K3 T$ c9 y) N
- ' I6 g w3 S# S1 _. x$ o
- float previous_error = 0;. X$ G. E9 |# e+ B# e
- float integral = 0;) e' U0 [& m+ p: H
- float derivative = 0;
2 R/ v) m; y) C2 g - int Speed = 160;' r( o7 Y# j' S; b. P8 k
- float output;( J$ D6 X/ O3 r3 `
6 v6 R) }0 n1 P& }/ I2 C) Q: p: s- byte Left;8 P( R5 F+ g( R; E
- 1 o' |, V2 w) N
- void setup() 2 E- Q: B, Y0 z4 T- F
- {
% k0 D; a7 e% J0 S - //Set PWM 8KHz
" O Z3 }% r& f0 O* m) c% A - TCCR1A = _BV(WGM10);9 [, Q1 u) F& X# Y# B B
- TCCR1B = _BV(CS11) | _BV(WGM12);: c* @- O! G8 M3 F! {* J3 Z
- TCCR2A = _BV(WGM21) | _BV(WGM20);
8 y3 e* m/ C4 u/ n8 M d7 d - TCCR2B = _BV(CS21);) W3 }9 y. d7 \$ g; U
- Serial.begin(9600);
! R0 Y. w6 C+ g# ~7 G, Q- @ - buzzer.setpin(45);
# J$ E0 X" e; ]4 p) k2 C - }
/ l; F$ X1 D N0 L: p
/ {0 p0 V6 L, f0 E1 @3 h- void loop()2 X5 s" r0 F6 p# W
- {
! r/ U+ Q1 a+ `, Y! a( C, Y. [4 [7 O - while(!((lightsensor_1.read()) < (10)));
! }; Y3 r, m+ E& |# T1 V4 c* Z$ _ - buzzer.tone(1047, 500);3 O1 ?% O3 W" o, G1 N6 y- C; s' `
- delay(500);
% X$ B8 j: \; G - do
1 O; s* ^( u/ L) V9 y$ A - {
- i3 {+ c& q* S+ \. w7 @ - int Err = getErr();' q0 m& w* A. C: ~9 W
- if(D1 == 1)
/ [" e5 k! o$ ~+ i; m/ `) S0 p) @ - {& o4 y, n6 \' U, Z* o
- Left = 1;
6 d4 B* x& f! e5 {" z5 G0 R0 ]8 \ - }
+ z. M6 M" [3 k3 _: q \4 q - if(D6 == 1), g3 i; N7 B' n+ b; J
- {
2 w1 L: z! _' C/ ^! N. } - Left = 0;
# Z; H5 ^- r" k, r - }9 X2 C6 v& v8 z0 |
- if(Err == 99)8 x K1 a9 P# U& b5 x9 K: e- I
- {$ b! w$ ?& ^6 j" ?4 I+ y) w
- if(Left == 1)
0 S0 g; i7 v8 ~( D ~& y - {; y' z; v) n+ K2 p/ ]
- Speed -= 5;
N0 w, V0 } p" R, l! V+ D/ z- i - moto(0,Speed);
' [9 O! x, u1 w& c$ s4 v$ [* |" ] - do0 T/ y1 t9 N2 |
- { O g' d: q* l9 ^
- Err = getErr();. V) K( V* l* W/ A6 `* }
- }while((D1+D6) == 0);; Q- [# v) L+ p% S
- }) F. X% c6 v2 i, o' I( e
- else \" Q2 s% s$ w
- {
: _' j+ ?1 L. Y7 C9 d+ J l1 c4 l - Speed -= 5;
& Q3 s+ C( i, h, k - moto(Speed,0);
8 \4 M3 C6 P; \5 A - do
7 `7 b+ k% O0 c - {( x* V: _+ c. @$ l" G
- Err = getErr();
, }8 L$ A% i( {# L, R5 X% I7 r# T: b - }while((D1+D6) == 0);
% W" P T4 g. y - }/ Q! o4 ^1 J! L+ O2 |+ ~& i% T
- }* q5 p6 K. P8 Q: w9 N' J4 _
- else
6 z: L3 g& Q# j8 z2 n: c; N6 E8 O - {
% \1 E& S3 _2 R; U - if((Speed < 160) & (Err < 2)) Speed+=1; 9 a+ u8 K0 t6 O- \
- if((Speed > 100) & (Err > 2)) Speed-=2; ( D+ m+ b6 X% L
- integral = integral + Err;
4 f; o6 O" D3 _% G* n - derivative = Err - previous_error;
% _: [2 c% B1 D( P( S% ?8 g - output = Kp*Err + Ki*integral + Kd*derivative;
. X! O# o% @ s, v; o - moto(int(Speed-output),int(Speed+output));' I8 e% g! V: t" a1 ]9 ~$ k
- previous_error = Err;
4 F) Y- |' k6 r* i; M" w H - }
" L+ d9 L1 v/ n+ [+ _ - }while(!((lightsensor_2.read()) < (10)));
. a# h6 F o4 t+ g) g - moto(0,0);
3 n& s- O$ M' m. s - delay(500);
$ Q; U& T5 u. I+ N- O - buzzer.tone(262, 500);% Q1 n7 R6 v8 ^ [. x2 q7 |) [
- }" O" [( z% _' w G: l6 p
- 0 {; |# {6 b- E; K& V7 x) X& y$ w
- int getErr()
# E$ M( B& [9 B \9 D8 R2 ?" r - { 1 r2 R: |2 P8 s3 T* }. [: r
- sData = linefollower.getValue();
7 J: t6 }$ s' b% k1 t3 {; r - D1 = ~(sData>>0)&1;
" O) p) F5 J4 o' \$ ]$ Y - D2 = ~(sData>>1)&1;
- e: b* S- E! P- N' u) n - D3 = ~(sData>>2)&1;. i4 } R9 }; C! w1 t4 b
- D4 = ~(sData>>3)&1;
O! c$ M0 O& s" X @- P6 E3 J - D5 = ~(sData>>4)&1;. y% I- P3 y! @9 T t; T4 [2 Q l
- D6 = ~(sData>>5)&1;
, T& s% X0 }* G+ } C* C% u - int downD = D1+D2+D3+D4+D5+D6;
5 b7 k% d5 |$ G& m - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);( o+ q E& G& K
- if(downD == 0)
z) d0 a+ v( w! M m' q1 ^, z - {$ D! M4 Y$ n8 y8 A& o' U
- return 99;+ }& V' i& g+ q& e5 ^( O2 d/ o$ A
- }
3 X* |6 V/ d& i9 d - else
, e) P+ }6 H* p8 _9 t& i - {
5 M' C" Q N o! u: ~9 N" d4 g - return upD/downD;
6 H, H I o# u$ \* ]3 M n( w - }
7 N/ Y" ^/ }+ w0 J7 u' c0 ^ - }) R2 P, b; [% x0 N# F( A( M
- , Q" o2 a5 x0 f' y2 O
- void moto(int Speed_L,int Speed_R); @5 ]9 \/ P" l7 T
- {6 m9 D- A0 V. C& k5 }( T" z
- Encoder_2.setMotorPwm(Speed_L);
* r& Z! p8 C M9 ? r/ w% Z - Encoder_1.setMotorPwm(-Speed_R);/ K, q$ J1 P) X4 ~7 y
- }
複製代碼
% `) n1 o" c/ M) u& `8 w* AMeLineFollowerArray.cpp
% x' R( K3 v- }( N& ?. _; J7 R1 w- #include "MeLineFollowerArray.h"
A# q$ B* d) m
* V; a# o0 Q4 q* u# u/ s- V {& y- #ifdef ME_PORT_DEFINED
& d% V& [. D4 s" i - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)' O" r6 o) J Z/ A) g
- {
3 x- A, H0 q @8 o: t' y% O, b+ P1 \
4 K( U9 Z T% S1 w* W" h2 Y$ ]4 W. e- }- z6 D. N) c! j3 T8 A
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)0 X" v. L# N6 [8 y% G% z9 O7 y
- {# a" z( N2 Z% Z6 _9 y0 E) @( X8 {
- _DataPin = mePort[port].s2; \9 C* i7 T: y% c9 Z
- pinMode(_DataPin, OUTPUT);
1 G# B" N: v7 |, K - digitalWrite(_DataPin, HIGH);" I/ _, R! ~) ^* O# M
- }5 R, _- h; u- H: M
- #else // ME_PORT_DEFINED8 j7 l" L* _5 Y: M
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
& ?6 u/ i1 k; d: K. L - {2 r& p, ]7 w% {2 v/ h. ^1 |/ y, A1 O. z
- _DataPin = pin;. |; B- B0 D: X) e+ T
- pinMode(_DataPin, OUTPUT); - s2 u& I# q# V# ?1 W
- digitalWrite(_DataPin, HIGH);
]1 X& J- O, M - }. S+ ]2 s) W/ A# C
- #endif // ME_PORT_DEFINED2 b' C1 M4 b: }& k& M
6 C, G# A) O) R$ Z
# X* f$ o9 @) e& X# T- void MeLineFollowerArray::setpin(uint8_t pin)* q4 e; }5 P. }
- {
& o) g" s6 o# C/ V1 l2 h6 | - _DataPin = pin;+ v$ c* D* f0 e
- pinMode(_DataPin, OUTPUT); 6 H" h/ a: f$ j: T4 |, N, @& A6 t
- digitalWrite(_DataPin, HIGH);
0 U3 w& `/ n2 h* a3 _ - . J9 |9 [+ C/ g8 R/ R
- #ifdef ME_PORT_DEFINED
; b! u# b& ^) L* w6 q+ D - s2 = pin;
* r2 _1 D O5 V3 t' f! }& q+ s - #endif
4 j" n/ b$ \4 V; V9 { - }. a) Q- h) l' K, ]3 N# G& j
# W y# a' @3 i* S- uint8_t MeLineFollowerArray::getValue()* z9 M% w6 _/ H& O
- {
8 T* [0 p5 ]" y. J/ | - uint32_t LOW_level_read_time;
! F' N6 l% Y3 b0 b - uint32_t HIGH_level_read_time;
3 S+ ^0 v: w7 I; E$ O5 I( Z - uint32_t time_out_flag;
) K/ y! S1 U& h" p' J9 P' z - uint8_t Sensor_Data[3];
" B- h0 \' m! @" e - static uint8_t old_data = 0xff;1 O8 V, V! ?2 r* F& U0 s( W
- U% y8 K2 ^$ S0 Y7 e
- pinMode(_DataPin, OUTPUT);
; g* m [$ @/ B! x* C, P8 x* j - digitalWrite(_DataPin, LOW);5 k: D) s/ o ~! z' g8 `4 B
- delayMicroseconds(980);1 Y8 f y% h; u
- digitalWrite(_DataPin, HIGH);
0 P% K$ i) t7 U9 Y1 j1 @( w
0 a0 M: k1 k0 o h( ]( C+ s- pinMode(_DataPin, INPUT_PULLUP);& B) e4 X3 r) u6 r8 A8 m
- delayMicroseconds(10);: V& Q& S1 Q! t
- 0 [$ f5 D3 ~9 h$ V6 a2 F0 l
- time_out_flag = millis();/ t. h: N% b0 n0 M3 h# o( [
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 Z6 T5 t% V' i' ~4 F( P2 i# P, Y
- ) Z) R1 b0 @- v& Y5 I. z; D, c; J: N$ _5 H2 r
- LOW_level_read_time = micros();
/ v; }( ^2 u6 N: Q% a# }# m2 d - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
' }1 a' q( \$ P a0 u - {
3 I- ]0 n! \& ~* V - return 0xff;' [3 y- z7 O) v. N9 Y1 T/ ~
- }
! C4 _- z& g; k5 X( e - / l# z: @! q+ v5 q" A* S3 i
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 j$ z. N3 w1 D. L
( K9 ^; r' ?( r: a- D5 Q1 T. H- HIGH_level_read_time = micros();
9 n" ?6 b% q: t - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
- U6 K; j3 f% r
3 D' [, P8 y5 l1 F- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
/ c! a) h8 C9 O; X - {4 _. o( G, E( b0 \8 d8 T
- return 0xff;+ J; J2 V/ j1 V2 `2 |2 @' `
- }
" b' I- |0 R# p. l; G - . h' i6 v) m d6 n, ~
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
+ d" m! L( Y1 |* @6 w: X# @, k - {
, w& i& r. I3 U# a9 D( _9 ] - return 0xff;
. ~- @) A$ w: v- z - }
! ?( @4 \ i5 `7 f! T: T - # r, s* t+ Y8 d+ z/ L1 R; n3 Z1 i
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );5 e' G: _3 O9 j8 c% }0 {
- LOW_level_read_time = micros();
2 g. }# E4 n d3 G/ i F' s - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
* p$ F/ m5 {: a9 Z/ f0 y# l - 9 U" ?2 {% T/ H/ r2 ~! d
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 |3 S3 D3 ^$ R. x# Z) N
- {
0 T1 ~" }. h- b; X% X+ j1 x$ u - return 0xff;
+ s0 A" I' m8 q. G$ W8 e - }
4 w" R# w) U' Q/ b0 \
& c: [, R& s8 K4 c- ?- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))" x5 X2 R7 ?5 x/ t0 p
- {
7 j: e- Q# I3 ^) t - return 0xff;( c! _$ N* C8 I0 `$ O
- }
7 G% `" `! K) q9 Z: V4 e& C8 T - ! [+ a9 Y/ a1 X/ }9 {( |/ ~
- for(uint8_t k=0; k<3; k++)
- }2 z, p9 r# ` - {
) ^$ P2 u; R8 l2 c, { - Sensor_Data[k] = 0x00;
4 {% Z( ^0 c* b/ i' a9 L$ p3 y) Z - 7 r( ~' [* ^5 b% z, x' {
- for(uint8_t i=0;i<8;i++)
5 V& d X& c( d - {, B, j6 _* s& G& p" N- V3 P
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level8 v1 |0 ~; Z6 h
- HIGH_level_read_time = micros();; S; B2 x$ \* ~
- LOW_level_read_time = micros() - LOW_level_read_time;
( |$ F/ C3 b, X5 e
' F& @. p9 C+ j7 z' p" Q- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
6 @4 f0 {1 G- x% L7 j1 w5 v - {) s x! @( T# h
- return 0xff;
" U" G1 h3 q& L. d; x - }
6 T3 b1 Y. G; w/ y - $ k6 g3 T* x5 J$ I. R+ c$ f1 L2 [
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );/ l1 p' e$ A. R. [/ P( m$ S
- LOW_level_read_time = micros();
3 r& \7 F2 O1 ?5 T5 w - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level- _4 L4 [4 Q b( _- C$ V5 n
' B7 ?3 T. X. v! J$ r" H* L- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
/ P0 Y8 F i! P7 b3 Y9 F8 O8 W8 q3 Q - {
9 Q9 w5 D% L/ I. ]3 I" F6 k - Sensor_Data[k] |= (0x80 >> i);! w9 W: f9 x5 r8 L0 K B; ^
- }
; C; g7 e& |! y, E4 T8 G - else if(HIGH_level_read_time >= 100)4 H! ~) P) z. O5 o
- {( z& w& m) }( f
- return 0xff;, U4 i. J5 n% ~6 j: X
- }
: Y# Y" K* u7 ^8 \' q- G2 S - 8 a+ L0 h- ]- ?. [ b0 ]7 R7 r
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) T" o. W, g8 j2 ^8 m2 V" L
- {
- Y1 U: t" P, D R, l4 X - return 0xff;/ p. q7 `" w, ]& S- _+ Q
- }# g7 e+ I; S1 B0 L
- }) u. b" Q# ~) V/ c4 q' w$ h9 v
- }6 I7 v( R k U) C2 A( B( S7 {
- ) v/ e/ T2 s& ^% C1 H3 U" k
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level9 Z$ h" H! k+ x3 a1 _* s
- HIGH_level_read_time = micros();4 X2 U _ R$ @* ^! Y% [) [
- LOW_level_read_time = micros() - LOW_level_read_time;1 ^* e( R7 v, Y+ ?2 T
- % B x0 X; Y2 ]. s/ w
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ); o# J, _$ |% m5 h7 U' E# B$ f
- {6 F. ~! |% g* a% Z
- return 0xff;
% P& _4 L( f+ h3 n. ` - } R: ~* y1 Y( B: `; g
- * n$ R; I7 j1 w3 z& _, n7 U
- pinMode(_DataPin, OUTPUT);7 k, ~: Z+ d) C1 k) `
- digitalWrite(_DataPin, HIGH);
% Z W ?, M; ^: `5 C1 j) V2 o/ v - * R R2 V6 x6 }
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0]))). V* [% ^$ J" ~1 W
- {7 Q- P3 I9 k* A; R- L* J: S7 J4 }
- old_data = Sensor_Data[0];5 r6 F3 ]0 P: E% \1 s; W
- return Sensor_Data[0];9 _9 w. \' W' C- o# o+ N+ A
- }) i5 B, M2 {/ t# K2 J1 y) [
- else
) Y) M, S4 a6 I- v% x8 f - {
$ }( D6 J5 x. t+ k- \ - return old_data;! Y; g3 c7 c0 f5 ^
- }4 D) ?/ w+ K5 ` e M
- }5 Q( e4 w/ e' B5 x7 L0 Y* D7 w! [
複製代碼 # y" j2 a8 E+ K% B3 Y# E) Q3 Z
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
1 {0 G9 a, B: {2 q( K
4 D2 m. t" R# N& F; X. l |
|