|
|
$ h- F0 L2 M. t+ U6 e/ |6 T( D/ K
- #include <Arduino.h>& V* ?# B% I: \- `. i. e
- #include <MeAuriga.h>4 J7 R; _) p9 \) F
- #include "MeLineFollowerArray.h"8 |( A8 P: D- e3 \. ^
- 8 i5 L, ]* h+ r, I$ b: h8 `; F( ?( Q
- MeEncoderOnBoard Encoder_1(SLOT1);) w. b( [1 w& C {! K2 T. X
- MeEncoderOnBoard Encoder_2(SLOT2);
4 @+ ~3 ?) Z' [7 C7 }8 T - MeLightSensor lightsensor_1(12);
2 ^/ `/ x$ W" H4 F - MeLightSensor lightsensor_2(11);
p, p& s& U# i+ z0 v$ D7 Q0 z - MeBuzzer buzzer;
8 h, u6 r; }5 n: Q - MeLineFollowerArray linefollower(PORT_6);
+ x: R t3 Z# {4 i' `7 c1 d4 P
% Q- r+ ~7 W! H2 y6 E# a. u( Z, P B- #define Error1 1% ?0 \1 {: g; L, X; X. n
- #define Error2 24 }# b! w& i- F c
- #define Error3 3
5 j0 i3 k; G7 K1 a( E* I
" k1 s) G7 s5 ]# M" K5 _+ |- #define Kp 15* B# G1 D! ?* t3 a. H
- #define Ki 0.15
7 O4 K8 E# _7 n6 Q, h9 W+ k - #define Kd 0.03
6 t2 B; X) y A. I5 F# D
5 B0 ~7 T1 l% u# C" B- uint8_t sData;
# l8 g5 L# \1 V+ V - uint8_t D1;
6 F! c# N; k4 X! v1 l; T1 V# a - uint8_t D2;+ E+ ~( j& R V- w) J$ |
- uint8_t D3;
: W5 Y! k3 M- e( Y4 X7 H9 O1 R/ d) C - uint8_t D4;6 v- g' G% `' O/ ^8 Y) E
- uint8_t D5;
$ ~+ ?* o" R2 }# w - uint8_t D6;+ x7 x- C( v" x ]
; F+ Q4 }* N( W0 } e! R- float previous_error = 0;
0 v$ h3 R/ B) B+ u; C: X6 e( v - float integral = 0;- Q* F, x) r2 i( C# Q- }
- float derivative = 0;
5 i: P% y: t8 q+ b( G7 X3 \ @1 Z+ v) X; s - int Speed = 160;1 D( H# t* H$ @) l
- float output;
- a( u7 O d9 J# f$ w( Z - / S. v. {: `2 U J; y
- byte Left;
8 R, P! O1 b4 f- ~ G3 p
# l: I5 m0 E9 ^0 i# e. _- void setup() 6 X( G" |8 K ?2 l; n5 Y+ g
- {0 C" [0 ]# P3 s! _
- //Set PWM 8KHz
+ Q1 a7 d% r6 Q- i9 B - TCCR1A = _BV(WGM10);. F ]. \; K! M: E; H
- TCCR1B = _BV(CS11) | _BV(WGM12);
( x5 N1 \2 s0 S5 i' H - TCCR2A = _BV(WGM21) | _BV(WGM20);0 q6 F/ a) ?. M; Z0 m
- TCCR2B = _BV(CS21);& A! e3 @+ g6 N$ s% K
- Serial.begin(9600);
" y& R* Q8 ^/ \) G C& X - buzzer.setpin(45);
$ a: T7 T6 A) W: d O - }& D) ~5 `) N. D
- ( c, y% M" P- C
- void loop()
4 K, ~5 @- x+ i" O - {' m+ W7 y; L! e5 Y
- while(!((lightsensor_1.read()) < (10)));
5 K* A) |5 J; |3 O j" f - buzzer.tone(1047, 500);
. [/ g2 ?% p: ~* \ - delay(500);' @( J0 j$ k2 ]% U% O
- do5 E. J# I! r* R8 l
- {
# j( o O) E) Q$ f9 x( I2 a - int Err = getErr();+ V5 o3 O; M8 T8 j: \0 _4 q8 H
- if(D1 == 1)2 g7 _9 W8 c' {+ ^! L8 F* Z
- {
: V+ H- z7 P @ - Left = 1;
( ~/ w, F. ^9 c* U. ` - }' |) J/ j# \: s, v1 P! G# O
- if(D6 == 1)- ~( R4 Q0 G; f* [4 R& n# T5 L
- {
5 ]$ G2 o0 B- h, V; {7 m - Left = 0;
& _, M+ L- [0 D0 E! K; \ | - }) v6 B( H8 f" s; k; o6 A& \
- if(Err == 99)
4 ]9 ?4 H) K1 q# q. v6 \7 P - {
^1 [( x: I. R, C* z9 a - if(Left == 1)9 c3 j! q0 c! c9 \
- {! ~8 E4 w, K1 D6 i& a2 l
- Speed -= 5;
( m: x$ C7 D, I# h& R4 J L - moto(0,Speed);" L" u& {! o6 s, b g$ z
- do
: ^1 s, c o5 |8 m- f, `& F5 p - {
/ |: T2 [3 h A% {. N - Err = getErr();9 u$ ]4 W2 h1 `2 v
- }while((D1+D6) == 0); s0 m5 F) X' L6 U' H$ F: D" n
- }
" e+ P/ k" P$ D9 D. z - else9 Z+ h4 q& a/ V8 }. ]
- {
+ s+ X9 n, t D1 M: {3 \: _' \7 `( u - Speed -= 5;+ g& r' y9 x& Y
- moto(Speed,0);
9 y6 C- F7 e6 l( B8 H8 r - do' k' U4 `0 ?+ Q) M! ^
- {
* a- l: `5 c" g2 ]8 b - Err = getErr();
2 x) G$ u; g6 _1 ] - }while((D1+D6) == 0);
8 ]9 e& c- B9 R M+ h* K, ^3 t - }! O! Z9 |$ B( K
- }$ F& ]2 |- v6 H, L4 ^; G' q2 |
- else
! A3 F# h2 m1 c1 A: A - {; w3 x, r: Z6 K& ?% |5 Y- N% C5 P
- if((Speed < 160) & (Err < 2)) Speed+=1; , D! L5 n6 M: P7 @/ y' `7 ]' F! }
- if((Speed > 100) & (Err > 2)) Speed-=2; 3 w$ p" T4 j: p( m
- integral = integral + Err;# Y& r7 ^" ]" p4 p/ s- H: ]
- derivative = Err - previous_error;8 t8 Q D. I- B) L; o8 \
- output = Kp*Err + Ki*integral + Kd*derivative;
: D' S9 B; ?: S& B5 t6 ?0 t ]7 w - moto(int(Speed-output),int(Speed+output));
4 i* X' @8 [4 S& C - previous_error = Err;0 J% V, b5 ]9 X6 |
- }) @( D, [2 m& a% W
- }while(!((lightsensor_2.read()) < (10)));/ |8 G1 v4 E% P- j' E8 `) r* G
- moto(0,0);
/ U4 k1 @0 t# B/ I - delay(500);
( f- I1 v/ ~) n; w' G! q - buzzer.tone(262, 500);
6 a& z& r1 U9 ]( I, D8 I - }. V. r0 t" {' t+ U9 d: \; N7 ^
; n& E. k$ x7 j" y8 x$ A; p- int getErr()) R- h6 D% r- H, V' K
- { $ O1 @" m' P6 p; C3 [
- sData = linefollower.getValue();! y; n1 S7 k1 X6 K( x# G6 b
- D1 = ~(sData>>0)&1;
6 K8 q! ]5 P' d3 x; N - D2 = ~(sData>>1)&1;
4 t/ c$ N) d# ~( L - D3 = ~(sData>>2)&1;
/ _, U. o* P! \7 A: [/ h7 D% I/ L2 J - D4 = ~(sData>>3)&1;, c: ]$ l2 W4 i! c" a
- D5 = ~(sData>>4)&1;
6 y/ A; Q# I" Q: M' [6 Y - D6 = ~(sData>>5)&1;
. E: q. w- Q/ A% ^ - int downD = D1+D2+D3+D4+D5+D6;
1 j4 d, A% d1 A2 o% E& z - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);6 y. N, q8 L6 l- u# Y& X: e
- if(downD == 0)
2 f" U1 u) J) j/ m8 c: `; B; q3 Q - {
. S* t* d% C) B) c3 X9 f' F. x - return 99;
8 \' x' t' u( h+ ^8 N" c2 v- } - }1 T/ {% {7 k2 A; r' V
- else
6 x" Y7 W. _" i! t/ w1 i - {
1 |9 K2 V1 o# \. h - return upD/downD;
" p# I5 f0 @4 B: i* G1 f% y; S - }+ [4 x5 f6 x) N. H* S' }- A
- }3 H0 t/ i9 p1 o1 n: ~
1 e" Q2 V# }; e% Q1 |6 l- void moto(int Speed_L,int Speed_R)
; C5 e1 y }# u& h" a. ] - {) c" o* X" K1 n' _" m
- Encoder_2.setMotorPwm(Speed_L);9 y9 z' A c2 z# f. n: w
- Encoder_1.setMotorPwm(-Speed_R);
' d: k! i7 D, s( M( |/ @ - }
複製代碼
. S; [$ E" ?! k5 g; C5 t) YMeLineFollowerArray.cpp
3 p4 j: W+ o. t5 S- #include "MeLineFollowerArray.h"
9 B- |* K* `5 i0 U
2 ]) o! f1 z+ ?, [- #ifdef ME_PORT_DEFINED2 L- ^& x2 B( Z/ J' `. p3 |
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)4 d; T& @% @7 f6 t9 x* y1 a& @4 }
- {1 |8 y# `% @5 G; \+ c. k
( D; f, ~9 ^$ X2 w- }5 m* a% F. U/ j( k$ R8 r9 l( K' M
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)" f3 E3 y1 n, l- y: x
- {% k1 V; d+ b0 @' w" f3 Y
- _DataPin = mePort[port].s2;$ v) j! t! Y4 b" q
- pinMode(_DataPin, OUTPUT); $ K4 u% D) S$ M. I( d
- digitalWrite(_DataPin, HIGH);
% G4 o) c. B1 S5 `) s) c' a( Z - }
7 W7 m$ ?. J' Q9 C# b - #else // ME_PORT_DEFINED# j2 ^! @4 w2 d" \) V
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)) i0 a% ]3 I. P& I0 I
- {* U1 @$ t9 }8 [
- _DataPin = pin;
2 E0 w6 z. W% _1 P) K& C - pinMode(_DataPin, OUTPUT); 2 n' _8 b) o) f
- digitalWrite(_DataPin, HIGH);$ Z7 ~2 h: n% f! d; m) G
- }% B) j M8 P- \% V
- #endif // ME_PORT_DEFINED
' H T8 o5 V8 O4 [" T' O
4 ^* P" i4 s+ K. s' u1 I
/ s; U% U% S3 O4 k+ @- void MeLineFollowerArray::setpin(uint8_t pin)
2 ^. x4 A5 k* V7 g9 c - {
0 G" K! c. R! Z5 d$ _ - _DataPin = pin;
$ r! j6 D, w5 N" N0 V* [' Y: j - pinMode(_DataPin, OUTPUT); 5 l. J( i D) I6 k, B3 H
- digitalWrite(_DataPin, HIGH);9 j; Y# L- f$ u- d
- . T" Y' B7 A4 [
- #ifdef ME_PORT_DEFINED' V+ V9 y( o4 F5 U. a$ m
- s2 = pin;
/ p: V8 }- z3 Z; |% a - #endif
3 ^$ b8 ^5 ?* w; o+ k+ Z, W - }; [- b& M) {" u3 K" `5 [
- 3 u9 H) X3 m1 ^# o+ k# i* f
- uint8_t MeLineFollowerArray::getValue()+ u: {' i9 W' {9 A T. L5 m3 k' ^
- {, {5 H6 U. `; X0 }) _
- uint32_t LOW_level_read_time;
+ ^; W! c' f3 N1 x! s# f% h% Y - uint32_t HIGH_level_read_time;
2 i* f) s, j8 G6 a7 n- m - uint32_t time_out_flag;& p1 C' J. F/ d4 g: b. c% o
- uint8_t Sensor_Data[3];+ C1 m+ X6 l: T6 z1 F1 X3 n
- static uint8_t old_data = 0xff;
! B( w) {& v& }( n, Q0 \
% X4 x: A# Y- K$ r+ t- pinMode(_DataPin, OUTPUT);! R" @; r& y o" a* C
- digitalWrite(_DataPin, LOW);
" U3 D6 X: r7 ^3 c - delayMicroseconds(980);8 [% N4 q* G2 U1 ~( R" b2 t
- digitalWrite(_DataPin, HIGH);
- `3 o7 Z: a9 U: S" s - 0 u/ b- ^& [4 b3 A
- pinMode(_DataPin, INPUT_PULLUP);
: O% k% L. M. k# b" g, |* w - delayMicroseconds(10);
: d" Y% _1 Q" D! O8 F
# I. F8 x: W8 x" B- time_out_flag = millis();" D4 Q; |( |" M% @
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; Y# D" C8 v0 v7 C, T- Q5 O - 6 K9 D, K6 L. W
- LOW_level_read_time = micros();
5 Y' [( K! B+ I! e( F. Q' b - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
% @! G ?% d" ?, C7 a& N0 c5 \6 r - {
6 \0 P" y$ N- [& |5 A* L$ h - return 0xff;
' E' c8 @7 E' A - }
$ S# M. s. J% y5 z2 G2 X - ' K: x! O' L" K' d) e7 X, l1 w
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( [ ?" l8 Y# r3 `/ n
- . w# I0 R4 U1 m* r
- HIGH_level_read_time = micros();" Q, X% A3 S+ x/ q$ e
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
. J7 t/ Q+ ^) A; A$ \3 H5 W - 6 v2 ~0 K/ S* `" k" w L f
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* s' N/ ~; m! {9 s8 y
- {
0 V- C: \) j- U; n! K/ t - return 0xff;' A; D) L4 z+ |9 D9 z
- }# E& N* _+ ~2 d: t t& k( D
1 j5 Y6 d& j- I# N- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
9 v; D: z, e4 ?" i4 Q/ T6 b Y - {
5 u5 Y" n% Z" E7 B' t1 @5 h( I5 R - return 0xff;: t, P g- c" [9 Q1 P
- }
3 [, e8 v/ H8 N) N1 T* u
: O8 X. o8 P0 m3 Q- h. C+ L- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( ^' v. m- u/ l1 H' @
- LOW_level_read_time = micros();
" y7 Q3 n2 [/ x9 L8 m% u- j - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
8 v) ~7 g2 ^; p+ {' y - 0 z" p8 u/ I, x" P
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
% p' f. P- v, ^: A% t' O - {7 q# V5 X* s( v' w
- return 0xff;5 {8 f5 ]/ }0 X J9 E3 l- ]
- }
4 ]! i/ l1 a/ e$ Y, d - 3 x2 `. p: t! V: G: D
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)) r- b' u5 |6 J# d# g
- {7 T+ k, M: |9 u5 u' S1 y
- return 0xff;: {9 x; X d7 [& B$ w, i
- }; j4 n% c( {2 t, { F i
- 1 y. T P) f, Y6 x5 l8 V4 k+ h
- for(uint8_t k=0; k<3; k++)
7 |/ T; s. ~# p" V9 z - {5 x$ M. X8 o2 F h8 y
- Sensor_Data[k] = 0x00;# B+ |: z5 F7 ^ Q/ f: _' N& W( F7 N
* j% i9 z. E2 l$ d' d- for(uint8_t i=0;i<8;i++)1 s" ^) j$ `* h' Z% _$ g7 M* D+ `
- {8 ~1 {1 z% h8 X4 B" b' ]
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level- y9 a! _9 v0 d' P! M& J% [' E
- HIGH_level_read_time = micros();
( v& }) R4 _7 \$ v" g( ?5 U6 R) t4 b - LOW_level_read_time = micros() - LOW_level_read_time;; l" g% g3 n1 \8 R# X% {. `
- + K9 @7 Z# K( o% ~3 Z
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
& y; N: K% B" I0 |: w2 e - {# T1 i; p/ a, l& P9 T v% o
- return 0xff;3 x _- C, Q, u4 s4 X$ k) D6 d x' W: Y
- }( J' B6 f: N% \1 f% b. T/ z7 g
% z0 B U3 l5 G) U. D, |- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );; v0 n5 J; m/ V$ L3 l
- LOW_level_read_time = micros();
# S; ~( }# g' N/ x - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level$ n; j" _5 J8 t) v
7 B# w/ G2 N7 q3 t- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1; Z* f5 S( [ x& i9 |
- {! K- R# K1 E" ~. Y4 f
- Sensor_Data[k] |= (0x80 >> i);
( X6 C v+ O& ~: t - }
( U/ E" A9 _; [& a# U* }. J - else if(HIGH_level_read_time >= 100)
# ?5 U$ U0 y, C3 j4 A" p( v - {' g' K4 f' K1 C$ A
- return 0xff;
8 W. d, o0 j( g - }2 _" N j& _. ^
- $ _9 }/ M J0 \, z% ~
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)+ C Q5 p! I0 [/ S
- {
8 C. @0 ]! _8 v - return 0xff;
2 g7 u; {5 v, R1 _ - }; p& \9 o9 G3 o+ \
- }5 g4 @; u( U* O( p
- }
+ r+ F2 z N2 `( h. o- F9 S6 P1 | - ( D+ I0 ^2 j' F, `7 K0 F) c+ E7 F
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
+ e y! v9 b4 X - HIGH_level_read_time = micros();
8 o6 v! W) U# c1 J0 H - LOW_level_read_time = micros() - LOW_level_read_time;- C6 |- ^* p& [% |" F C% V( w$ X
# ~; \& ~1 A. `' ]7 k! b- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ). _+ O2 n/ G8 A' K8 ~% u5 `+ Z
- {+ \! @: K* O. K, e+ Y3 V
- return 0xff;4 v" M: t3 v" K4 y
- }
( T6 u# K+ `, ^3 l# A - ' A1 T4 G. z& n) E4 F& q
- pinMode(_DataPin, OUTPUT);2 @5 V. C, v1 n; X7 V) [
- digitalWrite(_DataPin, HIGH);
: c4 x P$ C- s# S: |* w* Q, B( r7 y
: q1 A$ I/ o9 w4 J+ F* V) k- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))/ E1 x% H) s7 i( X/ I3 x
- {+ ?$ ?# t! r/ u# {
- old_data = Sensor_Data[0];0 r' Q( n9 G* j* m( i8 F
- return Sensor_Data[0];. \) N; h) M/ H0 Q
- }( r) H1 F7 ^7 a+ o$ w0 d" d: y
- else2 m# H6 g" ^4 g1 P( H6 t
- {
& {( T8 d& N( |9 {( B- ]. J - return old_data;* [% M8 ], h6 {3 u3 F, z
- }8 Q$ z* L# T5 h3 N' d
- }2 [3 z1 T& m( X \ }. O; T
複製代碼
) X; |- H+ I% B( s a$ k C$ F% QMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
! t# f3 }; a: H$ }% n, w; h! K" V6 j2 v; v7 R3 w
|
|