|
|
( e/ P- M) \5 F: g; A0 E3 u
- #include <Arduino.h>
/ c5 J/ ~# @* B% W' Q# ~5 A8 t - #include <MeAuriga.h>
; J% r3 [) o0 T7 g - #include "MeLineFollowerArray.h"8 V1 [( N' d( d: t* O" }
- ) E8 M4 p/ N5 O$ l2 ?- ]5 J" z
- MeEncoderOnBoard Encoder_1(SLOT1);9 x& H2 M( p" V$ f. b1 i# j
- MeEncoderOnBoard Encoder_2(SLOT2);2 i% c8 A6 I w* f7 | D9 R# E9 @
- MeLightSensor lightsensor_1(12);
6 T7 z8 I# W& B - MeLightSensor lightsensor_2(11);
4 x; g% j+ h1 h0 u% P - MeBuzzer buzzer;& {* s' x, R& P
- MeLineFollowerArray linefollower(PORT_6);4 L$ Z9 b; `+ H$ V
& Q) N, `& a, M" |6 h. _) V- #define Error1 1
$ |8 q& M. W. L, i - #define Error2 2
w- G0 u* k- \# N6 {; q6 @ - #define Error3 3
4 M! L" Z/ P/ N8 k M - 8 z- b4 ^; Y" U* `; Q8 H& Z
- #define Kp 15" p9 F l0 J7 r* e5 Y
- #define Ki 0.15
+ |+ N: J: g1 K' L3 a - #define Kd 0.039 w% R: [. z" j' @0 u2 ~" M; m$ R
- % g" W' E" t% t: m) l$ C
- uint8_t sData;
e* r7 W5 j7 N1 |4 A8 B - uint8_t D1;
8 M) e5 x+ R) ?- ~ - uint8_t D2;5 T& p0 i! x) q( C2 ^: h0 ?' f' `
- uint8_t D3;0 V' v! _+ w7 ^
- uint8_t D4;/ j& o; G3 y0 \- U6 K/ d3 U
- uint8_t D5;
. Q4 Z n+ U: g; T( y- t - uint8_t D6;
4 Z5 P, z, K$ q" Q8 n
# _' u2 _+ L( y. J& q- float previous_error = 0;
* D" L1 e7 \1 {5 l6 h, T - float integral = 0;' d- b9 M( P6 j4 m4 ^7 m1 O* J# m
- float derivative = 0;1 N; c6 {% Y$ ^) @
- int Speed = 160;& [) b8 r# \ J# v
- float output;' b, |4 ]( m/ L1 R) @+ q
- 5 |5 E, t) N# X) V* _, e
- byte Left;' s) C- K {3 M0 C$ @9 T
3 `* q" j. D t" s- void setup() 7 m7 |: f7 s/ w W0 M
- {/ O/ I( v' G- d2 q" b, ^& C. D
- //Set PWM 8KHz$ K- X% O& u$ C$ t
- TCCR1A = _BV(WGM10);+ a- _5 ~- L- S
- TCCR1B = _BV(CS11) | _BV(WGM12);
- B0 u" X$ Y6 u$ ]7 @3 s, B - TCCR2A = _BV(WGM21) | _BV(WGM20);( ?) X9 n& l/ L% p3 s& m
- TCCR2B = _BV(CS21);* y$ F7 X `1 z( f
- Serial.begin(9600);
1 N S4 n( i# d - buzzer.setpin(45);
% G, }8 {7 r$ x' [ - }' h' w% t. k6 J% k7 t
$ U/ M C0 Z4 v8 t: i* ^+ y- void loop()
6 `+ c+ A7 m# S - {& d, A% x. o8 i+ q z& l
- while(!((lightsensor_1.read()) < (10)));. d4 r+ Y* M' j4 G: y, H
- buzzer.tone(1047, 500);
- y+ F* X- Q1 _ - delay(500);
' u7 q1 B& S) R# Z# z: B - do
6 ]6 _/ |1 `: f$ [) ~! j) ?3 i) R - {
* j- h3 T( Y, n$ Y - int Err = getErr();9 O& F; x- y8 i8 o* r. Z9 A4 M
- if(D1 == 1)6 ]9 M) e! Y/ j5 K1 c7 \
- {
6 M8 k$ k) S3 A' B! b - Left = 1;
# K# a8 ~! y' k/ U+ `& i - }
$ r; Z& j" ?& d6 P - if(D6 == 1)- L9 j% @0 t& ^# ^
- {
! Y. S$ w8 N. D8 L& T- R' O - Left = 0;
* C2 e4 j% t, x. | - }$ W, _' I- c8 D+ r1 h' w8 C8 q W
- if(Err == 99)
3 v5 ~3 m' u' N9 P# [! E - {
' Y/ Z. a& P2 _0 v) f5 ? - if(Left == 1)
+ P& d0 t; v: \ - {
5 p$ j0 J* Q8 H& I9 T - Speed -= 5;+ S# j" ?, u) p) h
- moto(0,Speed);
5 O9 @( v$ w7 [' \8 Z3 r - do
$ s% P, L2 V% Y5 J5 I) j- ^ - {- T2 b, i- m' E( h- E! s z
- Err = getErr();
3 B+ l+ |, f, x5 X+ V M - }while((D1+D6) == 0);5 y' p$ Q J" C) J. t6 |
- }
i; E7 m& I& F - else
7 W6 F, B' |1 n1 X/ g" c - {- J- I& d- }% u T/ Y
- Speed -= 5;
0 y7 Y$ Z0 q& Q" d. Z - moto(Speed,0); * L0 g# ?6 P8 i) c; H G0 _& S( E
- do" N: ]4 h M; j
- {
' ?! i3 O4 U# [$ B; V - Err = getErr();, S4 N8 U: g5 \5 o) `
- }while((D1+D6) == 0);
' D+ q9 t- V9 {$ _9 q1 t - }, Z. a8 e8 F/ _" A$ `+ ~- {9 a9 A' ^
- }8 h6 z3 W& h! Q+ j' ^5 g
- else
0 A/ B# b& D/ @ - {6 \1 K; [% `/ ]* @% a
- if((Speed < 160) & (Err < 2)) Speed+=1;
6 w' l) w, d' [7 J3 E( z - if((Speed > 100) & (Err > 2)) Speed-=2; 4 H# L! ^, O" O# e
- integral = integral + Err;2 f4 B; j% K$ K2 l
- derivative = Err - previous_error;: g# O. K5 L9 N. ]1 |* ~' \
- output = Kp*Err + Ki*integral + Kd*derivative;
3 U+ Y x" {3 Z5 b4 c1 m - moto(int(Speed-output),int(Speed+output));
0 k$ g7 X9 r/ I; o& F - previous_error = Err;
5 e3 j: m8 k+ I; s - }( b: B9 g# O* ]$ p& i/ R
- }while(!((lightsensor_2.read()) < (10)));
. M& G+ ]: Q4 y% b2 u0 \& v7 ^! u - moto(0,0);
n* G- _4 l& `' @1 Q9 L6 q6 X - delay(500);5 k, R0 O& Z6 Q0 v) ?# x" `+ \
- buzzer.tone(262, 500);9 \/ ~) V1 x. u; c
- }1 @" `. @ R8 _, K( L2 U
- 5 r, S! r [- q; [7 T6 j& y+ R/ ]
- int getErr()" B1 e- d4 f8 J% ^+ B6 S/ R% o5 d
- { - G: r, ^3 h' c) U$ C$ l' f& A
- sData = linefollower.getValue();
% A) V* R+ s! q1 d - D1 = ~(sData>>0)&1;: u8 P) D0 H! p& G6 ?5 p
- D2 = ~(sData>>1)&1;$ R+ ^2 D: l' o4 x- j
- D3 = ~(sData>>2)&1;7 s3 `1 s7 B3 x; c0 _/ _
- D4 = ~(sData>>3)&1;7 l% ~+ X+ I' c! L0 F
- D5 = ~(sData>>4)&1;
0 H. X( N4 h2 Y$ @8 N# D, ^ - D6 = ~(sData>>5)&1;8 K8 ]6 O% U+ [* w2 K; @1 Y# E
- int downD = D1+D2+D3+D4+D5+D6;2 y* j( C, Q" |( r
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);- o8 b* u% N7 e6 o2 l
- if(downD == 0)
4 m" g! _% \: o6 D; W y - {6 ^# ?! \* M+ V5 ^, E0 C
- return 99;
, w2 M3 e1 E8 R4 `* {) ]4 k! R1 H - }
: S- _) d$ c3 N; ~& c1 b - else
4 o7 U; x3 ~# D# w - {8 U, w) Q8 K3 | Y: z! W1 W9 {" K7 M0 Y
- return upD/downD;5 E, v' @# v9 L% t% x4 ?; j
- }
/ Z" L! i8 ]+ U8 ] - }
, \' s* F% f5 I+ E5 T& K - " J' E! I2 l. m) ~
- void moto(int Speed_L,int Speed_R)$ [! J$ } |9 I$ Q+ j# g0 _& P/ ?/ }
- {1 P3 W: O' J; l- e o" ^7 z
- Encoder_2.setMotorPwm(Speed_L);
: x& A3 ^% ]& [0 }" t& E - Encoder_1.setMotorPwm(-Speed_R);
& u" [( p" \3 g3 g+ o - }
複製代碼 Y/ a4 j& X3 a" H1 w8 g6 g8 @% @1 o/ ^
MeLineFollowerArray.cpp- h8 X$ a3 b2 D4 E M# P4 d3 Y
- #include "MeLineFollowerArray.h"% {3 q6 u0 N. v+ r. {6 V& C0 j
- 5 H8 e" @6 u; k0 w
- #ifdef ME_PORT_DEFINED5 w2 g3 Z1 a( E" F- t( P
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)9 V3 s* P7 \. z4 t
- {2 Z( h3 f, S% M
0 t. h/ t6 U% Z( I8 `- }
. E0 P& |8 D# V) t% [; t A - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)! h: j g$ X: N4 _: t K9 t& l- o6 Q
- {$ \3 K) `6 [) C$ Y0 p( B/ W
- _DataPin = mePort[port].s2;
! N' x! C& f3 H/ D+ h! y - pinMode(_DataPin, OUTPUT);
2 Y$ Y4 P/ ?1 N - digitalWrite(_DataPin, HIGH);
+ W0 o4 a' R/ j- Q. Y2 x- u - }( r/ J R! r a0 x
- #else // ME_PORT_DEFINED3 V6 M$ z. G' @8 E+ I6 e% K
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
+ ~4 f+ I5 E Y - {
3 Q- J8 t% Z, s! y' h - _DataPin = pin;1 b! {/ N% \& P' M3 v6 z; f
- pinMode(_DataPin, OUTPUT);
2 G! ~% f; q9 v8 n4 N' M$ X - digitalWrite(_DataPin, HIGH);% I6 q# \+ Z' J; l, @4 S
- }
" }9 Z- X+ p. t2 [* |/ @ - #endif // ME_PORT_DEFINED9 q4 H* E$ ~: r- ^9 X& J
- 1 r7 P: A: _6 b7 ~1 G* X2 c
# O! T2 o- z. ]; k6 C- void MeLineFollowerArray::setpin(uint8_t pin)- p+ w7 |: \( a2 o2 c( m
- {8 z: q V& b- `4 d/ }
- _DataPin = pin;# p( I! W1 J8 E% ?- ^
- pinMode(_DataPin, OUTPUT);
, I# z/ ~9 S- M8 _- K - digitalWrite(_DataPin, HIGH);
1 h+ {9 f# H- l1 {. ~
- z) n% f8 N7 H0 V# S" {- #ifdef ME_PORT_DEFINED
, J [1 p5 M! D* u$ m. } - s2 = pin;
* \+ [2 t5 }7 A& L7 Q - #endif
7 p9 {9 E. e% r+ ^4 [: K0 k - }& ^' _6 T% H; E; O9 [& U2 D
- * Z+ f# J, ~" c& P
- uint8_t MeLineFollowerArray::getValue()- \# U/ [1 h2 E
- {8 k0 f) s" l% T& V- }
- uint32_t LOW_level_read_time;
% ^$ K5 m7 d: |1 Y - uint32_t HIGH_level_read_time;! e9 |0 I, N8 A O
- uint32_t time_out_flag;, U9 z% X: m- I0 J' I8 T
- uint8_t Sensor_Data[3];
, S- S) Q& E6 p - static uint8_t old_data = 0xff;- _/ I, t/ Z2 e
8 B7 D. Z s) B5 w. [4 p- pinMode(_DataPin, OUTPUT);; O4 l5 l3 e+ Z+ j8 r, W
- digitalWrite(_DataPin, LOW);# U9 r+ o9 O a/ r9 O. |
- delayMicroseconds(980);
! N8 o' H1 b; W) W1 q* K0 ] - digitalWrite(_DataPin, HIGH);# I( Q2 P8 _+ Z
# _4 o" q" Z2 H- pinMode(_DataPin, INPUT_PULLUP);
( k: V) Z/ D( ~. p - delayMicroseconds(10);6 a* F' S- z6 u- v. ?+ W
; v% O, e2 H5 C* G- f6 Y- time_out_flag = millis();
' c0 w, l9 C! D& E; V - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; w0 ] h- n7 S
# y0 `% {$ g8 Y. @* j$ {- LOW_level_read_time = micros();
9 ~2 K; E M, b+ R - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out8 P8 Q1 @( x* `$ f# W) F" p
- {! S4 e! J. N+ ?2 o
- return 0xff;1 O8 D) |( [6 @
- }" a: @5 k. f( P( }
- 5 f$ J0 {; E# l* c. \
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );7 o$ y( d3 e/ U v) ^
. Z" P5 K8 \* J" i3 {- HIGH_level_read_time = micros();7 O4 T6 h3 Z" f* _! J# q6 p6 C
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level% G( {0 A' j) D, U8 M4 p& U$ \
3 E1 d! u; \. O* ^! K; m, A0 V- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
p+ B3 W# c8 ~) R" Y" A - {
2 ]' Z& o) g' o+ b( _1 Y8 w - return 0xff;, X- U! o6 z+ f0 X5 e* y
- }
8 W; D, o- j4 r n - 8 ]; `* S5 p& z& _2 K
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
( J. @( O& b1 d - {/ ]4 |6 D: s s, b! @0 }
- return 0xff;! a& [5 o' P5 t4 ]2 Q- D% A T. d$ J3 Z6 S
- }; B \/ w" _- W0 L& }
% H% W3 N/ ]* @, M- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
) ~0 p) `# H4 W1 A( l0 R - LOW_level_read_time = micros();
1 ]+ X" S8 z- K/ [0 J) p" X& {% x - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
7 o' R% F- w3 ]2 @# x: q - 6 H. o% Z% ]5 t% ?' Q9 i: X+ z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ Z( T/ z( T: R% h
- {: ^# _& s( ]# [2 U% t2 o/ N7 x v0 e
- return 0xff;& j8 Q& h8 R3 q4 v1 L: U1 Z
- }7 ?+ e( {4 N4 w- G1 \2 {( |
- / T( h; y4 U! C
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))4 u9 D1 p& d9 i: r. C: V3 d
- {/ F# P, p; A& C6 |- c* B
- return 0xff;, M. E6 L5 G8 P! W
- }
5 R" r- @4 h6 ^9 e3 v - % C2 |" P, S5 r" ]
- for(uint8_t k=0; k<3; k++): ~/ e8 d/ l* ]0 z
- {
3 I9 g' |' n6 ^6 B& x: m3 C - Sensor_Data[k] = 0x00;$ p3 n, ~% ~* z1 D
- 7 m2 w+ U" s% v! @, [
- for(uint8_t i=0;i<8;i++)1 s3 F' G/ }& d9 G0 D2 w: T/ V7 @' [
- {( X$ d ]1 B* X2 }+ O5 z; U
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
% d4 A( P7 G% t7 r9 V - HIGH_level_read_time = micros();
6 i8 D" z" k8 b - LOW_level_read_time = micros() - LOW_level_read_time;4 f$ c' J% s6 ^" O! E% m3 [2 f. S
2 X6 X/ J! j$ X- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
' ]+ |# m: L b2 v2 L - {1 |* Q) W) ~8 a, r4 n, X. d
- return 0xff;
' v1 K1 m, }8 j1 t" y( R$ a7 v! M" _ - }
) D! g: F1 Y# l( D - 4 S. {4 e4 }& }/ K- R( J. M' J; g
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ l- x" X; H* u' u: N. N
- LOW_level_read_time = micros();% s, b8 Y" ~ c
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level# z. x; ?. V1 U$ P. j5 l
- ( Q7 {1 g4 B* @$ i7 R- K
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
+ R* V" R& W+ d4 e( k - {
; {, D! J A4 D) X ?9 o, v/ U - Sensor_Data[k] |= (0x80 >> i);
9 Z! C3 \; ]! e4 c0 |0 [ - }" A1 r- i& q8 C) F% _
- else if(HIGH_level_read_time >= 100)0 U7 y& O: l$ _: o9 K9 o+ j
- {- q& Q3 _% {+ p/ ~' l* z
- return 0xff;
: e# m: @5 _5 y: \ - }, Y& @' D; ]& ?$ M6 N5 u
- 1 Y# S, P( N, T3 e8 I2 g. @+ k- L
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
7 D; ~7 B" @7 F) t; o) E - {' ?$ R9 x1 J# `& w5 T1 `( v
- return 0xff;
1 o+ ?+ ]. h9 s+ t8 ]( ~2 C - }
6 H g; v; m9 }$ r- j( D - }8 R; ?" {, i/ u5 ?- N% p
- }* L/ ?; R, b0 N, A
) ]3 h- D" V5 l* y" i" Z! V& [- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level0 @0 O Q- Z3 s/ J# H8 p
- HIGH_level_read_time = micros();, Z( A* m$ V# Y. Z4 R# G
- LOW_level_read_time = micros() - LOW_level_read_time;* @* w5 q" E" x/ Q4 _
$ ^" i. t* Z, ~5 n- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) q5 S" S+ ~: l5 X, a
- {; ^5 M, Y' H# ^9 ~% P. h, E$ @
- return 0xff;: `5 p E$ F' A+ [
- }! x& o# [) @8 E: W4 v6 E
- + u# }6 Z) G. S& d
- pinMode(_DataPin, OUTPUT);
+ X9 {. w5 e! ^3 n* q- C ?! H - digitalWrite(_DataPin, HIGH);
" d; j( y+ c& z K6 k
5 [; f4 B4 C( D' b- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
# S% E% i% \1 g8 \1 S0 [) V - {
) w$ v1 L: s" Q. C6 M7 H; S - old_data = Sensor_Data[0];
5 P0 |4 Y" k K1 f - return Sensor_Data[0];5 n0 O" k6 c% q8 A Z. P
- }
" }$ \) g$ z9 @' R, I. ` - else: n3 w* T: ^- n/ S
- {
$ O' j( |8 t, n2 ? - return old_data;
$ `* ^. A1 d( t& O. g" a) v - }5 Y V4 K. e8 s- p2 f; Z) W% H
- }
6 T" J- T6 a6 ^* i) q# G0 I
複製代碼 * q. Z6 ]* b* M1 M
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
: p7 f4 v& l- ~7 P d
# T3 W' M3 }/ l: c7 u# v" s
|
|