|
|
) K! q9 n) H) E4 o6 y) M- #include <Arduino.h>
8 q) b4 b+ l" ?- U g F/ ` - #include <MeAuriga.h>
. Z, p! ^ m( G0 e$ m$ _3 Y0 a# U9 R - #include "MeLineFollowerArray.h". f& M- B5 k) l8 U% h. E
- ; }) |3 q8 p4 P# L5 m
- MeEncoderOnBoard Encoder_1(SLOT1);
0 o2 [9 f" k; g6 l2 j - MeEncoderOnBoard Encoder_2(SLOT2);1 p8 y2 U& c$ B1 F6 H4 b2 {
- MeLightSensor lightsensor_1(12);7 j9 O# r4 |7 @. ]% k2 \" Z0 S6 J
- MeLightSensor lightsensor_2(11);1 K5 m. t, A$ h, C9 ?
- MeBuzzer buzzer;
$ f, ?' N$ f, w - MeLineFollowerArray linefollower(PORT_6);
1 i' |1 Q3 B3 N( s/ g- u& q
8 ~1 B# H; W& T; ~' }2 v7 G0 S5 L- #define Error1 18 }6 C9 ]+ [1 M, h$ m4 h
- #define Error2 21 ~- o6 Q! ]4 x0 D
- #define Error3 3
- a0 X1 W8 j/ Y! J7 O - ! \1 c3 Z: A- s8 u% e! b0 y
- #define Kp 15
: v; c3 \4 V) D" }/ | - #define Ki 0.15
# V* b# |. c- z6 I - #define Kd 0.03! v% t. {/ [: I$ V* B( C
- & U1 P, P4 z. \% u2 u# N# c
- uint8_t sData;
c, g" d+ c9 y( S+ ]( d/ _ - uint8_t D1;
1 W, Q+ j1 W$ C$ X: h8 i - uint8_t D2;
( k7 s" f0 {. |8 { b/ Q6 C - uint8_t D3;
. S. o6 t& }7 |3 k - uint8_t D4;) y4 |6 {# r. e9 f, Y `
- uint8_t D5;
3 l3 x G1 u V7 s/ q - uint8_t D6;
* O' o9 ]# R7 f* |3 w
2 a) d H" k8 ^) G9 |; u- float previous_error = 0;1 Y) i2 A2 ?( d7 R r
- float integral = 0;
* A3 a1 X! H; g* }% { - float derivative = 0;
& I. I+ {: B! m5 R f- _ - int Speed = 160;; ~7 g) ~( f* Y2 i1 l% }
- float output;4 ]# x! y- r' k4 D( I4 y9 P9 }# D
& I7 D4 B& @! ~2 \; f/ [$ S- byte Left; { Q% I" {! a1 }3 p h/ G
- , R* p9 z/ O$ I5 U3 {
- void setup()
7 ~& M4 C r& J: ~ - {
; F* u9 S) X( S! c* H, ?* C - //Set PWM 8KHz
, J" A: L' [2 @4 S& d - TCCR1A = _BV(WGM10);, S; L z8 w# W! f
- TCCR1B = _BV(CS11) | _BV(WGM12);0 B6 P: t/ k9 ^5 p/ ?8 a: c# Q
- TCCR2A = _BV(WGM21) | _BV(WGM20);8 D/ k" a- @, {! Y0 i$ a7 D
- TCCR2B = _BV(CS21);
/ H' q# _' s$ Q8 j( q0 V - Serial.begin(9600);
/ ?8 D6 C$ g7 K$ i$ k - buzzer.setpin(45);
% S+ Z* Z& }. J8 w& R2 u0 n - }
1 U" o+ @4 A7 E5 ~* x
: Q) D; J; E ^8 L2 }. z' b* Q+ }- void loop()
3 o, H2 |5 P8 s6 L$ ~* c* u - {
& T J. L7 y: L6 v* V2 | - while(!((lightsensor_1.read()) < (10)));
- [* U, b* U4 |6 f' o, e4 A9 V - buzzer.tone(1047, 500);. T/ X+ \3 a; L; B/ T
- delay(500);
0 P6 E. ~% O2 u* ~8 ^- R! q - do
" @+ `2 B6 z. F - {' t5 ^6 |3 Z$ d1 a) e% O D/ M
- int Err = getErr();) V) Z3 l. \. G5 \ m3 |# _) @
- if(D1 == 1)& |+ m7 M3 e/ L4 M [/ e2 S
- {2 ?4 n/ a$ f8 Y' j+ ~2 ^
- Left = 1;9 U/ u, C7 r# I9 B9 ~
- }/ L& H5 r% T) f: d/ L
- if(D6 == 1)
6 N6 b: u# {. t* b+ f. a4 b( q: f - {
. } D8 Q5 M/ c - Left = 0;# J: b7 m; y& Z) A) L% Y2 ~
- }
+ P$ \# D# I) k3 K2 e7 }. J - if(Err == 99)# X; u$ i) E1 V: U0 N3 j1 n/ M
- {3 B% N9 `* q7 q* ]: V' U
- if(Left == 1)1 \: l( M; m; B; h0 j
- {
; y3 r$ t' h5 y6 v3 d( A - Speed -= 5;6 b* M! i" n; k8 y- W
- moto(0,Speed);3 X+ }5 l8 C2 k8 E6 B$ J
- do4 j1 T( ?5 ~& ^8 _/ z
- {
" v2 i( z, x3 q" G" O% {$ q - Err = getErr();/ r) Q G8 h8 {6 ^
- }while((D1+D6) == 0);
* m& S' [" B% H2 L# d8 y3 s - }
9 w, M# T h- ?3 R# o - else4 A- `4 M7 s3 {0 C' _7 r5 w
- {# M& S+ X# [% T4 ~6 i3 j, @
- Speed -= 5;1 }/ Z5 a1 r( x) }$ d
- moto(Speed,0);
3 `) L+ v2 d" c' s# B3 f - do
; r& u0 B( d( B* j; c - {2 l! y; i$ y6 i0 F5 M- G
- Err = getErr();8 C4 _8 K) k' n7 f6 [
- }while((D1+D6) == 0);/ S7 v3 Y! G2 p. R! i
- }% ?! Q7 Q# }. }3 @1 X
- }2 c- O4 c1 h% y: y
- else
9 d$ i- E; x. b, B - {
5 }. g8 O) }3 ^, I - if((Speed < 160) & (Err < 2)) Speed+=1; ' }3 F+ _% ~; w$ I% _
- if((Speed > 100) & (Err > 2)) Speed-=2;
. w- j8 ^/ y1 {5 | - integral = integral + Err;
& k5 G2 K( ?. |, S - derivative = Err - previous_error;) G, e0 o$ N8 \7 [& ~! Z$ p4 Y) l
- output = Kp*Err + Ki*integral + Kd*derivative;
# N1 h4 C$ K/ N+ |. T8 u* } - moto(int(Speed-output),int(Speed+output));0 F$ R' q. R$ V, Q
- previous_error = Err;- P) ^/ T6 A& J* z
- }
" w# l7 T( g. C& _5 T& | - }while(!((lightsensor_2.read()) < (10)));
' N1 S/ o |' o5 g- u1 q - moto(0,0);& y! k" l1 r, }" x4 G+ p4 z+ E `
- delay(500);3 J) X9 m$ ~! b) Y
- buzzer.tone(262, 500);& I) J4 o: Q ~- l) a I, p
- }9 N& o4 }, q# y* _$ I! w
- / E+ E/ R" e" b( E% m
- int getErr()
/ R, y, h3 y+ s$ i8 N - { 5 a& M" H& W A' b, {3 p+ [) E
- sData = linefollower.getValue();9 n; N' o3 O8 @- @, e
- D1 = ~(sData>>0)&1;
& T& i( [4 Z4 b) K7 V, ^" } - D2 = ~(sData>>1)&1;8 {) u4 e8 Z, }4 ]
- D3 = ~(sData>>2)&1;! V: `( l% g& i+ T3 Q6 Q) T* I6 H
- D4 = ~(sData>>3)&1;
# U( q* B$ J5 Q; u# N& ~ - D5 = ~(sData>>4)&1;
- z, k3 r. H# I: A* W# B! I - D6 = ~(sData>>5)&1;
# ~ [! D3 b) e' ^, ~- ], c1 v - int downD = D1+D2+D3+D4+D5+D6;; Z9 l4 g& g0 j: _7 C
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);2 {9 a# Z# a2 E% O
- if(downD == 0)
7 j* d: H" ]6 t6 _* W! B - {# X& T* R3 o0 }
- return 99;
* K) Q1 u- Q- |* @2 N: q# S - }+ k$ c! M% M0 n3 x
- else0 X$ t; u. J0 Q" k
- {/ j1 G/ M% Q' v' Y2 {6 ]: x
- return upD/downD;( W( i4 g5 S7 _
- }
/ p8 H9 Z" r1 n( ]5 G+ ` - }' G8 }# d6 i, W
- 7 P8 g! K3 g4 R; v
- void moto(int Speed_L,int Speed_R)# h* S, |- l* G9 p3 Y
- {" g! t4 t$ M. n4 I
- Encoder_2.setMotorPwm(Speed_L);
3 o+ X ]) q' t) c* P4 h - Encoder_1.setMotorPwm(-Speed_R);6 r' e* ]* F% J- J3 _& M+ @% A
- }
複製代碼
! Y V9 [2 S# O+ lMeLineFollowerArray.cpp
9 H: T( m5 C2 Q2 z; Y) b* }- #include "MeLineFollowerArray.h"/ h( _ t; K; j" L5 \+ A
- 6 ]% B' \4 M' Q1 {) t3 ^
- #ifdef ME_PORT_DEFINED6 p; e$ q) a/ V* q2 q
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
) @) g" G( o2 C# H& m: G - {4 u5 |" }, L7 n, u
- / d1 P1 X, l" W
- }" i' `7 q) h$ F
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
- t8 N0 ?, h- v - {
, c" O- J; l* \ - _DataPin = mePort[port].s2;0 }: C! l% z1 }4 ~( ]; z
- pinMode(_DataPin, OUTPUT); |/ [+ |& c4 {( u+ e3 g
- digitalWrite(_DataPin, HIGH);7 b `3 d+ Y8 @+ ]; }' g1 |" \
- }5 `# [6 u& |& d! H+ U& Z. |
- #else // ME_PORT_DEFINED
- p. z5 H* W3 |+ ~ - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
+ Z5 e& }3 V. g - {. J* N; h j, ^$ v& L5 Q# B' J
- _DataPin = pin;$ d( M. H3 T9 e! g% r& Q# U O
- pinMode(_DataPin, OUTPUT);
! q0 O, j4 Q* d3 c - digitalWrite(_DataPin, HIGH);' L. ?! q5 Y/ m0 ~ m/ f% g4 M
- }" O1 l5 r8 _) ^
- #endif // ME_PORT_DEFINED) F& u9 @/ E$ ~
- $ a2 I ~% e% Q2 n% p7 h7 j
- $ l5 m. I+ K. y0 S4 P. s/ t
- void MeLineFollowerArray::setpin(uint8_t pin)
) {: Q) J2 ]1 w$ U - {" o& W$ P+ l% K4 [; K6 p+ H
- _DataPin = pin;
, D8 F. v6 G7 A - pinMode(_DataPin, OUTPUT);
- t+ \2 n" ]+ w. \ x - digitalWrite(_DataPin, HIGH);
2 P+ T4 H3 Q* _) a2 A
6 E @1 w: F5 Y1 A, `, q- #ifdef ME_PORT_DEFINED2 Y* H6 o1 p3 W" |
- s2 = pin;
2 w2 u% g$ J+ F6 |, E0 W1 F - #endif
/ y7 s6 z* w+ X5 p! F5 B - }
2 G; f6 _# b8 n' c - - B( L( s* N" f7 Y, w$ p! f: U
- uint8_t MeLineFollowerArray::getValue()5 K/ H3 F" i; }& l5 S2 D- }% a
- {4 g4 u# G ~) n
- uint32_t LOW_level_read_time;
# I$ `! J2 i% e7 A* n! q - uint32_t HIGH_level_read_time;
" E9 r" [, p# M4 v) {- d ~+ E - uint32_t time_out_flag;
, u# K+ d5 D4 p5 F1 g - uint8_t Sensor_Data[3];' Y; ~9 C% ]5 a* G
- static uint8_t old_data = 0xff;6 E7 Q+ U7 R) `+ l
" l& c- }. I. h" O& L7 f! ^ {# Y4 U- pinMode(_DataPin, OUTPUT);* ]- U% I5 E/ j4 P! z
- digitalWrite(_DataPin, LOW);7 f' e- J; a9 w/ a* \6 k; ]! {: W
- delayMicroseconds(980);
- F; E- r$ |; Y8 d) I, c N0 u - digitalWrite(_DataPin, HIGH);1 [: R* o( }7 ~, U$ ^* u
- 3 T, Z; i3 l. F
- pinMode(_DataPin, INPUT_PULLUP);
+ n0 ?3 R7 V! ^4 u7 Y+ |, r% d - delayMicroseconds(10);
4 I8 J$ c; p& t' k9 [/ k) d& V - ! a4 l5 e# D9 h& `4 ^
- time_out_flag = millis();- l2 p5 i2 s! U" q- c
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );! M s, b( v& X6 S
$ n& c0 U3 Q+ H( o' X8 v- LOW_level_read_time = micros();
; S" P/ H+ A& T+ J. j - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out" K$ v ?7 H+ p# @& k+ x+ `
- {
* @$ ^6 {5 {) P" w; H/ J4 K2 T - return 0xff;
5 B; q" t! Z. H9 v - }* j/ p# X7 n9 N( w
- ! i0 E9 c$ V% z; g! K2 _# h
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 ~6 K3 e0 n& r/ X
- u6 L: c. k4 w ~; v- W5 u- HIGH_level_read_time = micros();
* b2 c9 r' }2 T5 x( W, a - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level- O/ A/ A5 a: k! J
- ! Q4 @2 I! @2 L! b
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out. s/ Q n Q0 y! N$ O' f0 K
- {
- @7 Y- U8 D; e$ u; c - return 0xff;+ F0 m6 V: P6 J
- }
! F9 y3 {. b, H$ h5 L/ U4 n2 \ - " y5 f+ u2 J8 M! d1 S6 G
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))! e+ f" C/ R$ F& ~% k0 b9 ]
- {
2 t6 l/ @9 V. {; E5 \4 X - return 0xff;
* j+ Z9 G I1 j - }
9 n" S% z+ N; `5 [8 w
0 t/ n: L" T1 w1 A/ {- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
W8 z% T& P- |7 a3 F - LOW_level_read_time = micros();4 F; Z! k3 K. @, V$ k8 W6 ~
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
) S! V8 E0 D) c7 @) F - 5 D' i' j( U5 ?2 B: L0 `
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out' T$ ]4 K" c1 a$ N: `. y4 b4 ?
- {9 o! \0 I1 k! q# v, w
- return 0xff;
: y) s6 Z2 X4 `" \ - }% N. ?% H: _7 J' Z) m
, a# F; ~0 A$ D$ n( G- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))% r8 ?; L( _2 c! e
- {
* S2 u7 N* J9 \# x0 T7 O - return 0xff;/ ^' B) [" Q% Y* M O
- }2 D+ n' j2 `/ ~/ @" ?
; q0 ?! ^5 _) o# o. B) f- for(uint8_t k=0; k<3; k++)( J8 a9 U2 }- T
- {
% I* | C1 q7 ^ - Sensor_Data[k] = 0x00;
1 ]4 m1 C0 G x6 Y q
5 l b+ A' @* F# m9 d1 V- for(uint8_t i=0;i<8;i++)
0 ?. ^$ r" d. k/ k4 b5 y - {
C" C; C( ]* w; D - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
- N: _8 h6 M. h7 ]: j - HIGH_level_read_time = micros();; `: O! d' D2 Y
- LOW_level_read_time = micros() - LOW_level_read_time;" {& s$ M8 [: g9 r9 q4 i
' l. U0 J) Y/ B! p8 m: `- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )+ r8 e5 q. w9 Z; ?
- {
+ d/ E$ A5 d( t5 u! c - return 0xff;5 N ^( u9 e2 K5 ~2 Y! I
- }! |4 A) i, s& X% J
- ' T6 q/ l6 `- Q
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- |; N' i2 X8 n x5 @ - LOW_level_read_time = micros();
) n( Q+ Y- P& v2 L - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level8 a; y3 i8 G4 I# @$ q k7 @
- " `5 m( Y/ f. ]3 f, A# A( v
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1( k1 e- q) P- Z- b- _& i0 Q3 f0 T' o8 d
- {
c; B. P. ^- u- e! P& M, {. u - Sensor_Data[k] |= (0x80 >> i);
, j# Z7 b8 }" {% \# A - }: @! C/ H6 e, m) B; | h! h$ w
- else if(HIGH_level_read_time >= 100)6 d" w# O2 }4 p4 Y$ e: l( }+ ?
- {1 T9 e r& `9 r) V1 Z. q) e' p
- return 0xff;: s$ ?$ f! n# L2 u
- }
- I" {, v$ s7 i$ a3 _
$ Y/ x0 W. v( }+ |5 S6 I- [- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
8 O/ Y; N" a" B( g1 e - {$ }5 y0 r4 n# n, |. ~
- return 0xff;
4 C$ j) ]( ^! f a9 _$ P9 H" B - }8 ~0 I2 O. E$ k8 W7 _" m
- }( M6 c& U' A: F& S$ p+ k. Q( @
- }
( f e% U9 _3 J+ V: m - 0 E# E. W& l& [( Y6 I
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level. z& N2 C. Z! r4 e7 |
- HIGH_level_read_time = micros();# F7 `# V0 M0 f8 e; J/ \
- LOW_level_read_time = micros() - LOW_level_read_time;
8 t" I0 r* i* ?1 ~6 U - & a2 S/ y" h: _
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ) N/ h+ [+ `* r& T5 P
- {! C: n3 S! F. r/ M: w6 g9 f4 ?) @( O
- return 0xff;
; f1 i7 v& m7 ~% E( b* ^4 u - }
0 _# ?$ m9 p2 q* W
4 g% i( Z4 ~% _$ M$ V# Q6 M- pinMode(_DataPin, OUTPUT);8 J9 `9 C9 V* V% d
- digitalWrite(_DataPin, HIGH);
! {- t3 c, f0 k - % B+ |/ `( m' N) }1 L+ n
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
O t/ J1 V0 t - {) h; J9 |0 Y, e/ f# o5 Y% w) j
- old_data = Sensor_Data[0];$ u5 o: j2 D2 a1 m9 _; L
- return Sensor_Data[0];
2 ~8 j7 }/ O O1 M) c$ |5 L( I+ N- j, g# S - }" A6 I c+ _* [) i4 D
- else
4 y* L! d# g% w - {0 F1 ]" d! E- j. w, a. R6 L
- return old_data;! V5 @2 K1 y$ Z/ F/ G$ F. `
- }
4 |, e: o* N3 |$ Y - }' g* [+ U( r; d! {7 ~8 N7 X
複製代碼 * ~4 ]; v7 U- u: _* | h
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
/ n- I. l/ \# F) a8 w$ \8 @( }6 H1 u
|
|