|
|
: A, t" E% `. O& Q- #include <Arduino.h>
9 i. r; b6 \( W - #include <MeAuriga.h>
# u, ?. ~8 `: C% a+ f3 C - #include "MeLineFollowerArray.h"4 G5 S% b! x8 r( e
- 6 x1 g* s1 c+ M; G8 d* s* ^/ M
- MeEncoderOnBoard Encoder_1(SLOT1);; F8 [$ m$ C* v2 l3 |
- MeEncoderOnBoard Encoder_2(SLOT2);, S* W9 J+ h" V+ O- T
- MeLightSensor lightsensor_1(12);
M) ]7 G5 a8 n9 f% R - MeLightSensor lightsensor_2(11);, j$ }8 }0 K$ i% }/ G
- MeBuzzer buzzer;
7 r6 b8 r% Q0 x! o7 c3 Z7 O6 q - MeLineFollowerArray linefollower(PORT_6);, W/ l& U6 p9 U2 d' M' O: Z7 ?
& g* ?6 f2 R( ~7 B# {- #define Error1 1
" H5 f* Q3 ?2 n3 U. ? - #define Error2 2
7 B: S- I: Z) Q" ` - #define Error3 3
: X4 G# F( @0 _5 d
9 |# k$ ?9 |4 o1 }3 t) o. Y- #define Kp 15% }9 p7 `* @0 J$ H5 e' E3 a
- #define Ki 0.15* N: M. Y$ T$ r" B1 e8 h* E5 m* S A
- #define Kd 0.03$ g2 g; f0 q N* e
- ' B5 J! w* F5 T+ }0 t8 R
- uint8_t sData;
+ n I/ J2 a, M% | - uint8_t D1;9 m1 N1 J# N) V5 _
- uint8_t D2;4 b; z3 I2 D$ C7 [4 ]& P
- uint8_t D3; M9 {) `( t( E$ T
- uint8_t D4;
6 d+ t6 y0 u# t - uint8_t D5;) i. m- Z3 d, ~, y. ]: H
- uint8_t D6;: F1 B3 Y3 ~1 v& l \
- 5 `/ D8 m* f7 B* a5 ?
- float previous_error = 0;
7 n2 E' ?/ E! V3 x T6 m1 u - float integral = 0;- W8 d1 M* L0 l3 W' C
- float derivative = 0;
+ ~; N# P7 d' a) z9 j( P - int Speed = 160;
; G6 |/ s) L6 T# f4 \3 v U - float output;
% ~. i2 `$ \* Z
" N$ {4 Y O8 N4 J |- byte Left;
4 q9 g, w4 L: m" h& T
# G% F2 Q! t: ~+ F( i- void setup() . E$ P6 t" J+ n9 M6 e5 O
- {
& S5 x/ {0 }: m; v; n |8 `& h - //Set PWM 8KHz. ^9 i o; h" \, t& O
- TCCR1A = _BV(WGM10);+ C r6 n3 f( t# n: }/ W
- TCCR1B = _BV(CS11) | _BV(WGM12);& \5 T+ y2 P" ]3 x( @4 H) y/ y
- TCCR2A = _BV(WGM21) | _BV(WGM20);0 K8 L1 Y, N2 F: Y; K) p% @
- TCCR2B = _BV(CS21);
4 S% I! `3 S1 Q8 \ - Serial.begin(9600);
7 O3 e5 r# k) @7 ^0 l5 w, h - buzzer.setpin(45);
) L$ F$ l; O1 q# Q - }
0 n5 m0 Q) J7 T( H! x% G
* R, P9 T3 A( H) d- void loop(): T6 N- u3 `" L
- {+ w( ^, B# r6 ^. L4 ~$ }! t
- while(!((lightsensor_1.read()) < (10)));* w: p3 ?. z7 t
- buzzer.tone(1047, 500);8 F% e4 e1 M6 H
- delay(500);
; t& A. D3 o4 a& O5 u - do
' ]& h/ q6 D, z1 X4 W - {
! f: w' h f' w7 c; J - int Err = getErr();5 Y; i/ `4 r) M8 c7 M% W6 l* K
- if(D1 == 1)3 u: u; a* Z, J
- {2 g: g, G& ^% y) H3 K
- Left = 1;8 O* F6 k9 Z q) s& B. D3 D% k
- }7 s' ]% _7 @ _( R- Z$ b) Q
- if(D6 == 1)
9 c4 S# H3 M9 y+ O1 s5 h, N - {4 N' x" ~, J3 l0 r F: ?7 P: X+ ~% }
- Left = 0;
E9 x; J+ O$ F6 {4 D* U1 |6 R - }
$ j* T l I* U6 \ - if(Err == 99)* S$ Q+ \- g6 m% |- I
- {$ p6 q8 t* N- [: T; Z
- if(Left == 1)
l3 L, P7 O% Y2 ]3 V0 l - {
& p4 j" a1 G( x - Speed -= 5;* ?6 N8 V6 u- t9 N3 C
- moto(0,Speed);
* D6 Z9 R. u. J1 w) H5 {. t - do
$ ` s) J4 D1 ]7 { - {
# @, y7 U& c# R2 p( m - Err = getErr();+ W& G& T7 x# k L K% s& o
- }while((D1+D6) == 0);8 y) |3 a# e) C7 y/ _: O' N
- }# j+ Y \. e/ J* C8 E2 l' V
- else9 F/ o2 {7 e" H+ f2 m
- {$ |; L4 M! m7 y; ^7 W' U9 h
- Speed -= 5;+ L5 L' {. \# L' B5 i! r7 f
- moto(Speed,0); 9 W$ Y. k0 I5 } c: A4 u2 L7 c2 p
- do
# r9 a" Q, j4 v; G - {
0 B _/ C w, \3 U2 t) B - Err = getErr();5 {! F v/ p0 w# t2 v6 K
- }while((D1+D6) == 0);
, O; f! I2 Y, L- i+ S$ N* \ - }
' r& S: ?) I$ ?; { - }
) w3 Z1 D" `" d. ] - else
+ p9 o! }* g, |% { - {
& J/ e% b- q9 K& S7 M* C) | - if((Speed < 160) & (Err < 2)) Speed+=1; ( A, _; i, ~4 `/ U7 f+ T
- if((Speed > 100) & (Err > 2)) Speed-=2;
% a# c: ] j& P% b- s- F/ X8 v8 M - integral = integral + Err;( p, ], t* U0 h8 `$ H3 P8 ~8 y
- derivative = Err - previous_error;
. N" h$ ~5 q& q i - output = Kp*Err + Ki*integral + Kd*derivative;
- t$ t6 t8 r. D7 S& J- V; y - moto(int(Speed-output),int(Speed+output));
$ u) J) @8 c; M - previous_error = Err;+ x/ K4 a: z8 i4 x
- }
+ h e& R* Q: a- T6 R - }while(!((lightsensor_2.read()) < (10)));- U$ z8 @ u8 s4 ?
- moto(0,0);
7 T/ Z& |; d# r - delay(500);# y* U: ^! M/ R5 h7 o
- buzzer.tone(262, 500);+ Z" Z) A9 K( [6 E* ~( k
- }
, Q) [+ i! N b - 5 e* }* g2 y7 }
- int getErr()
$ X' U) v2 d7 b - { ( c) j0 y9 l) ]% w
- sData = linefollower.getValue();
2 A; E: P! [+ q - D1 = ~(sData>>0)&1;
# T2 l1 i% O& m) n/ ?' q - D2 = ~(sData>>1)&1;
; A1 W; N$ \. o2 g0 Y - D3 = ~(sData>>2)&1;9 s3 q: R) v+ b" Q8 _( ~
- D4 = ~(sData>>3)&1;
* `5 P- c2 g. c6 u1 `& Q - D5 = ~(sData>>4)&1;
* E7 A$ W% a% a - D6 = ~(sData>>5)&1;, g! L# ]4 f/ C* }& y
- int downD = D1+D2+D3+D4+D5+D6;
8 q' j8 V0 z H - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
. M/ f1 v8 P! N3 b - if(downD == 0)- i% ?" C0 F/ H& @8 ^0 n1 `
- {
* a4 I9 [- J7 D - return 99;
! v8 S7 e3 Z$ b8 U: h - }) r# C3 ?& ~' R* P
- else x, [& C/ o0 h, U4 p
- {
' F# i9 B: E, N" J - return upD/downD;
- C$ f1 x' m$ C- W9 ]# k# r' ~ - }
' H. p: a8 y! ^" T4 V - }
- }( n/ A r. r8 j1 s: C/ e4 |
5 e) |4 W' o$ @' T2 }# d# D- void moto(int Speed_L,int Speed_R)) [$ ]1 O* k9 r' f3 n
- {. r: c) T q! E+ i a m4 h& A" J
- Encoder_2.setMotorPwm(Speed_L);
|' A7 `5 S# H4 A7 e6 v, ?% x; e' M - Encoder_1.setMotorPwm(-Speed_R);
8 w) E5 c/ {5 D+ @) L - }
複製代碼
& ~- z3 k# b5 f7 HMeLineFollowerArray.cpp
. u f* ~8 j: j9 R7 @# L( A" g( X- #include "MeLineFollowerArray.h"! A& q" b c+ t/ S& C% I
$ z, d6 d" b# H" G" e( k5 n- @8 Z- #ifdef ME_PORT_DEFINED2 a. y! C4 Y! V4 O
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0): I- t! ^2 ?" h+ v2 O. x5 Z
- {7 x1 h1 z) o; \" T. b% w# v$ f ~
# ]. f. \8 X! B z" g. h( B# }- }
6 ?: u( s! h$ A7 K0 y1 G - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port); ^2 U! F7 G1 k$ N$ Z4 }
- {$ W) F9 q: J+ }: {8 N
- _DataPin = mePort[port].s2;
; Y- ?6 U. i. J* k( K" A - pinMode(_DataPin, OUTPUT);
4 A6 X$ |7 a, [- j( y3 z: E: p - digitalWrite(_DataPin, HIGH);1 S: |% ~( s6 O p5 z
- }
L; x; R# S! p) s; C - #else // ME_PORT_DEFINED7 ]! ]2 K9 D* G7 P) b" `9 r3 H
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
+ Y, g' Q2 p5 ~1 w; i+ H - {# ~! f9 k5 Q: e+ d* t2 S
- _DataPin = pin;
w- ?' ]7 \6 C0 @8 S l! v - pinMode(_DataPin, OUTPUT);
H7 ?: R3 G8 _% a - digitalWrite(_DataPin, HIGH);
/ ]! y: W6 B6 b9 ^; A+ I - }# H3 t8 @( T0 [. E! v; j( `9 X
- #endif // ME_PORT_DEFINED
% R2 }1 T3 `' u( Q6 n8 l) s
P, [0 N) z! x* m" i: |; e. M
) S6 }2 J* \8 i- void MeLineFollowerArray::setpin(uint8_t pin)3 w3 ^3 y# h: C+ f- ~* ]) M2 b. U
- {
0 A( g: e* T: k& w - _DataPin = pin;
( E8 u0 E+ E# d" s' _8 \ - pinMode(_DataPin, OUTPUT); : r9 n2 |3 ^- G' e8 w4 H# k- b/ d
- digitalWrite(_DataPin, HIGH);
/ \7 R: T' Y4 X6 Z/ M
7 O& |6 i) D! S3 r; g7 t0 b- #ifdef ME_PORT_DEFINED
+ y- m6 g6 o* a5 q3 @ - s2 = pin;
$ r3 J# d. `% X$ A- O" X! n - #endif
) K! Y1 U& X) F# M6 S - }
9 a+ _; H5 X' @/ d7 N/ N/ f - ! A1 [' n# j0 v6 j& b; Q. |
- uint8_t MeLineFollowerArray::getValue()
9 C- }$ ] g: P- x6 T1 S8 Y) y - {2 h, y7 M1 O: ?4 ~( S9 C
- uint32_t LOW_level_read_time; A& D3 T' v% I6 D4 |
- uint32_t HIGH_level_read_time;
' g% w/ R' [2 ]5 k- ~0 c - uint32_t time_out_flag;5 P8 Q' i- ~1 K9 F
- uint8_t Sensor_Data[3];" J0 w f' O0 l4 Q4 E8 M
- static uint8_t old_data = 0xff;
# p s6 J: p7 Q0 g0 d) ? - * E5 @% ^) Q5 |( l6 q( V
- pinMode(_DataPin, OUTPUT);; k# P5 P* L, {
- digitalWrite(_DataPin, LOW);
; ]8 j5 S7 u. C( {- f" c5 A( f - delayMicroseconds(980);
- f1 N; z1 v3 Z0 G: l - digitalWrite(_DataPin, HIGH);
9 x- u6 i* m# F+ M! B2 q( S3 w
0 f! r4 R' v: @' H: w- pinMode(_DataPin, INPUT_PULLUP);
& Z" c/ A' w" C4 F - delayMicroseconds(10);/ M& H5 L6 V! `7 t8 |4 ~8 k- l) C
- " b5 z, g, G/ W2 q6 R( D
- time_out_flag = millis();
5 ]- m5 u0 s) {0 @ - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ u: c( k. S! N; A* P* e' k - - u& V3 f0 @$ F' n9 q( ?% p
- LOW_level_read_time = micros();
2 {, J1 y9 X8 Z# V - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 e: F/ [$ w0 i - {
( t, O) `* Q8 `* l* ^ - return 0xff;1 U' ^9 j" I e
- }6 I6 I2 M' e: T' A
, {2 P6 |1 b/ U& {1 G* R- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );; l+ {. k+ Y8 A4 ]0 q9 I
/ I- d+ `# Z* v/ [) [" Q- HIGH_level_read_time = micros();8 c7 o& \3 S3 N' z6 a8 B6 `9 ]
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
. K- s) u2 u1 j* a2 x1 N0 _
- _& p9 H# h5 d, l# g- H- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out- L; {2 A7 D J, j2 U0 q7 X$ \
- {& K1 p# t! _3 x3 s4 ^
- return 0xff;8 B! X# R. g+ `7 v5 R4 i
- }
" w: z$ P8 x, k% J& V1 x
! F% K4 G% z9 |/ H9 S9 }- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
1 `9 A1 e( R8 o' Y" ^ - {
5 i7 H2 x- E0 j; Y3 ~% A6 ^ - return 0xff;0 ]" Q8 _0 b3 g
- }1 v- }* C* o; `4 P
- N( s! n, |+ J- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
) y0 J1 o4 ~5 ^: i% l, b$ A - LOW_level_read_time = micros();
6 w3 S! Y/ X7 E. R4 q: S - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
' Q, M0 g2 W. E( t$ o$ i$ M - 9 t4 U2 O( U5 `* ?) R6 d
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* h/ }6 i& ~, }9 S
- {, ^: @7 P$ K3 `- [ s5 l n
- return 0xff;
6 g( r% R; {/ @* | - }
8 U% L. }$ j* X2 X
0 p+ ]2 ]5 ^5 J6 p- L- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
9 T+ K' g: ^3 G0 I$ @( N - {
, M& _6 {. W( C( z2 d: G - return 0xff;
( k s& G4 X- c- o- r - }
. Q$ J6 p; D3 K0 ]9 F& d7 V - : ]( _( s5 i/ y* W
- for(uint8_t k=0; k<3; k++)
$ Z, U7 Z Q" V - {
# ?; y7 [9 k+ n) f: a - Sensor_Data[k] = 0x00;
0 ^! T& u! g( ?3 ~; h. j5 ~ - 4 \$ |4 E+ W. f7 s! j+ k: a3 l- r
- for(uint8_t i=0;i<8;i++)
/ S+ A) C8 D. p* c, e - {$ z! z" K: D- f6 \3 ^ t! J
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level! T+ j2 ^9 _3 I0 u
- HIGH_level_read_time = micros();
' \9 i2 K& J& [3 \1 ~* m- O1 m - LOW_level_read_time = micros() - LOW_level_read_time;( m8 s" e7 y! x1 \) Z
0 l Z t& W; {, r+ @1 o- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
8 p% _$ V6 P. B5 f - {$ a9 c, S; b& ~% X4 f
- return 0xff;
8 v; H2 T( g Z b! D" l) ~$ g - } \; m6 H4 Z2 k3 G
! q! Y; C5 n* H- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ A- `# {; n6 n1 u. n/ W, n Z
- LOW_level_read_time = micros();1 c5 x, J' G. X# J9 f
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level; o6 @% e2 W' x" [7 g8 r3 K' D
- 4 `3 m) N5 }3 m1 ~
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
8 F& j, H3 Y( _/ }& @* b8 D/ z - {
* G+ U# h% c" ^: j) m" A+ e - Sensor_Data[k] |= (0x80 >> i);: t o D; l1 k6 y4 @. q
- }+ j! q$ c2 S' b( |
- else if(HIGH_level_read_time >= 100)
3 f6 V1 E: b1 m0 _0 o7 } - {) P7 e/ z% {& }. q, c# P
- return 0xff;0 q% C; t" N0 E+ ]3 f, B
- }7 m& g- H& W7 C. f- W X
7 s5 L7 }) Y* L. D) P- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)/ s. v( P2 u" T0 f: n1 b$ K
- {
0 _7 a* H; b* i4 {, U" T u - return 0xff;
/ H+ C' z- A+ l0 O2 X t5 y - }8 z7 A: L8 w/ M' c( ~
- }
$ c* G( a7 S7 k w8 p) Q2 h - }; Z4 X* C3 K( ]& C$ l
- / Z7 ~! C. [ S8 ?2 e
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
% m7 {2 { M4 @ - HIGH_level_read_time = micros(); Y/ ^1 @+ Q! t4 K- R
- LOW_level_read_time = micros() - LOW_level_read_time;; i9 i3 X/ h2 |4 S( w5 i
- , c( I& b; e) c7 ], o0 d
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
, Z1 ~ p3 n5 c8 P" f) Y3 z - {/ W$ W* W$ i) s
- return 0xff;% }3 [; n1 g& i3 u5 \
- }
. _, Z* n( I4 d4 k M1 ?% T2 e2 r - # a5 s+ d4 a/ v. [5 Y4 Q
- pinMode(_DataPin, OUTPUT);
7 q% M. k7 w1 L ] - digitalWrite(_DataPin, HIGH);4 }8 C- \( q( B; d9 m. o' \3 o8 m2 F
- 4 ] y2 W) D& D; K
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))& b# |- b* e! j) s* X$ p9 }' L% P
- {
1 t. N+ H( T- K8 F* } - old_data = Sensor_Data[0];$ `* l; _" R; Y5 Z. B
- return Sensor_Data[0];
! a9 i3 }: d; a7 S& Y- A - }2 ^, u# ^: b# _2 H+ o5 p9 e
- else
* x& b0 t3 x, ^( j! ^8 F6 B1 D - {
4 v( F5 J+ v" [" Q: v& S3 ^ - return old_data;% T6 b9 _( s* }8 W4 g
- }; ]' }2 @# k1 H! M5 v
- }' q y. A8 G1 `
複製代碼
' \- [1 S8 {! B' u& ^5 WMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
5 u5 s2 [( T; i% M
5 W/ f& Y1 y5 S! A2 s4 J, r2 }7 B+ z
|
|