|
|
$ b& u+ B' w2 s( }, R' {8 y- #include <Arduino.h>2 R/ G% `( ^. q. f+ ]% B0 O
- #include <MeAuriga.h>% l6 A+ w x6 |9 V# d
- #include "MeLineFollowerArray.h"+ Y- t" K; o" Y* d0 V/ M5 F2 b
- 5 {1 }; B6 h3 l f0 E1 P
- MeEncoderOnBoard Encoder_1(SLOT1);2 @. E- D0 ^1 O/ U! j4 i1 ]1 Z
- MeEncoderOnBoard Encoder_2(SLOT2);5 m3 l% u* \& A, V8 A* K1 W; q* w
- MeLightSensor lightsensor_1(12);0 {# q2 e0 v! A
- MeLightSensor lightsensor_2(11);+ U4 h' g% Q6 Y
- MeBuzzer buzzer;# F( v! Q$ j$ o5 B" Y& T) l* a
- MeLineFollowerArray linefollower(PORT_6); f. V1 g$ M, r/ {
0 H* l' m' `( \4 \/ }+ K- k1 c9 c- #define Error1 1
3 T5 G% [& F4 w8 u0 ~ - #define Error2 2
0 y; @: P8 C% ?6 V/ I2 `" l - #define Error3 3
! Y8 I+ w! b" ]5 S) C0 V
8 p4 N! m7 j; w) R- #define Kp 151 A b% L: M q- N6 ^/ ?. V# X
- #define Ki 0.159 ]' @; a* j! F; e. r% i7 S( N' B& R
- #define Kd 0.03
' o5 P; W+ o7 Y( }
s# D! W" W! k- uint8_t sData;" c3 y. e! Z& m, x* p
- uint8_t D1;
2 n) r2 s& k0 P7 P. ^1 `' J - uint8_t D2;
2 x: h' N3 S. D3 A+ @ - uint8_t D3;
! x# S5 x( u4 g. x/ n6 R7 L - uint8_t D4;
+ _* W% j; O# u) { ]; A - uint8_t D5;
. V3 Z$ ^5 W$ N" N6 h - uint8_t D6;- w2 D ~ N3 f: \3 R( {
- 5 m# p& I$ z3 [$ y6 h1 Q7 k, b/ v" @6 B
- float previous_error = 0;
. V8 a9 X/ ~# T/ H - float integral = 0;
3 b0 z; I* f, P& } - float derivative = 0;: i" J- X- e0 p2 p" ^4 D5 @
- int Speed = 160;: h: b+ p6 A J5 v! a5 }* T
- float output;) G6 q4 }/ b/ \4 r' R2 x3 m
. J" o8 X: j# w2 c- byte Left;2 l9 O4 ^+ z/ i+ ?2 j) L/ d
- ( D* D$ {6 v1 [/ g T6 a
- void setup()
: h& P1 V9 R+ r% x# d - {
6 G7 O. ]* H$ K6 f* e - //Set PWM 8KHz1 D4 f* s I# b# w( c$ f1 F
- TCCR1A = _BV(WGM10);
2 `- n* t# C t% F1 I( k% O! h - TCCR1B = _BV(CS11) | _BV(WGM12);8 D& P' c7 \1 u/ x4 W+ A) u
- TCCR2A = _BV(WGM21) | _BV(WGM20);
: b* d0 y, l- J+ y9 N: R- L8 [ - TCCR2B = _BV(CS21);; k: u6 c A8 Z; Z" h1 Y/ r
- Serial.begin(9600);; C5 A$ Q5 l3 r/ r" e
- buzzer.setpin(45);4 C7 E+ g1 W, K) X5 w* n
- }
; k$ n% e* k' G3 a4 o
: _+ F, _4 x, O/ m$ J8 }. [+ c- void loop()# i3 R* l1 V0 |1 X: I! K
- {4 Q9 U. T( K6 F& y; i
- while(!((lightsensor_1.read()) < (10)));
$ G- u+ D, X3 L) V - buzzer.tone(1047, 500);
0 c) j; W3 _5 ^& k! n- ]& w* L - delay(500);/ L7 T* ^/ k$ \9 y+ ~" @
- do& M9 p o6 z8 s1 @( K
- {3 [! S9 o" j, v
- int Err = getErr();/ u3 v' g: ]6 C `% e* x! h( ]
- if(D1 == 1)* p! g" C9 e5 M
- {
9 i, r& D( J" j8 g- m; y - Left = 1;8 ^5 {$ T. O; D( \
- }
7 g$ B, Z" Y1 Q - if(D6 == 1)
# K z% r/ H/ J1 P - {
: }/ v8 z6 C, H! u& F' b - Left = 0;
6 Y6 q+ x, L \1 p: J - }& @* y9 f0 T+ z+ ^' H. ?0 a
- if(Err == 99)
: [7 X! D' _) C1 d4 q- p - {
+ [6 D& B& d4 n5 j2 U5 i3 k$ N4 F - if(Left == 1)
, S3 y. N* O. @# d! x - {4 Z# v* t Y" o7 n3 n" |
- Speed -= 5;
: b. `5 }# ?( u2 X+ S' K - moto(0,Speed);6 h& R" Q) e6 H t
- do, L0 m) e% u. `
- {
7 A* F, K* P" g& w3 i$ j - Err = getErr();
$ G2 t- l0 N$ b! q0 I, Z6 z - }while((D1+D6) == 0);
/ D5 M# L+ [% }9 x - }- |1 h! v& K1 s! J) c. U2 t/ h
- else
; \5 `/ J; D2 e - {
& G7 i- `' [- w0 A4 z - Speed -= 5;: o9 F2 f6 s) M, \& t# |9 b
- moto(Speed,0);
7 {$ E) Y# L2 y* t5 O1 i& d - do
" l" _" r% y) q - {) d) c# v7 Q% S
- Err = getErr();6 [" E6 s P* Q' a: `/ Y
- }while((D1+D6) == 0);# U# H/ K( `8 B! u" \9 T& n" e
- }
7 ^; M `( Q$ r. o - }
6 K1 t' L8 H# V/ |7 Y - else: }7 I' W$ A) f# x! n# z2 N
- {5 s7 I% b- y' J$ Q, l
- if((Speed < 160) & (Err < 2)) Speed+=1;
7 s' x! t1 C! Y* J - if((Speed > 100) & (Err > 2)) Speed-=2; , N/ b9 T$ w1 p+ @- M- D. |! e; G
- integral = integral + Err;! H0 m8 @+ @3 b: I0 c9 X
- derivative = Err - previous_error;
0 p7 H5 w& c7 E# W7 \/ ]7 ~; F: s - output = Kp*Err + Ki*integral + Kd*derivative;
6 _1 j7 S+ c* U f- n. Y - moto(int(Speed-output),int(Speed+output));
1 r( P) y- w1 j& L6 G" ?! ] - previous_error = Err;5 a% w/ [) ~) e8 \, r3 [
- }& N9 y* }3 w3 e3 S0 b
- }while(!((lightsensor_2.read()) < (10)));
0 ]7 ~7 [$ c: t0 R# |& Q8 o - moto(0,0);
5 p# T; K' g8 X - delay(500);
+ A) U; g' d; @/ z1 G! x( L - buzzer.tone(262, 500);- u" [) f7 L* N& u
- }
1 N. t( Q3 R" t* e - / i, G9 z8 L3 _: t- f) F: Z( N5 ~) z
- int getErr()
( c j& p M! p6 h! ?2 ? - { 4 L; _5 R: ~: Y, ]5 H9 o
- sData = linefollower.getValue(); I1 c+ S' i1 N% b* q) R8 N
- D1 = ~(sData>>0)&1;6 K9 k& {8 O" u r2 X2 S
- D2 = ~(sData>>1)&1;& Z; _$ v' e# X# s
- D3 = ~(sData>>2)&1;
' R) s" s& }6 L& | - D4 = ~(sData>>3)&1;1 ?' ~5 J& p# W* A- ^/ a1 G. F
- D5 = ~(sData>>4)&1;
: A( g0 Y2 O1 w - D6 = ~(sData>>5)&1;
! w5 k' F: i% M$ z - int downD = D1+D2+D3+D4+D5+D6;( J; Z6 l) J" B9 L- M
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
4 D$ b0 L9 R' r; D5 P" v; N - if(downD == 0)
0 b( w! j' o( g4 }4 o6 g8 M0 x, k - {
2 W2 g& S1 w; T3 n+ Q% N1 s, X - return 99;6 I: l0 E$ Z' r% Q* ]2 F
- }
! Z6 S2 y. f' [$ O A - else
' B6 q, S( M% f1 ^; k9 r0 o4 Z - { y5 W% \" e7 G) D7 e
- return upD/downD;5 l G6 R% q2 M l
- }
2 i1 a. T7 } L: F5 U" g - }
1 N, R7 T! A% [/ v; _8 R
% Y, r, N* _( N- void moto(int Speed_L,int Speed_R)
7 H- p) x K2 l! u5 p - {
% Y" `" q, _$ l, \) T7 k0 _ - Encoder_2.setMotorPwm(Speed_L);
c. d* B4 v+ X# P- y0 L. b) P - Encoder_1.setMotorPwm(-Speed_R);
# L% z) u# Q( A- x4 P* o5 c - }
複製代碼
0 t5 ]6 G+ p& s1 v* g, j1 e$ BMeLineFollowerArray.cpp
4 F& @( F2 k5 u6 }- z- #include "MeLineFollowerArray.h"
2 h2 G6 `6 U% @ b& \5 B
+ @8 d1 R; P- d- #ifdef ME_PORT_DEFINED
" S9 k- E0 M1 p2 ~) H" Z, a! y - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
5 z+ l4 \6 E6 G - {8 l+ H9 T* }) L9 w/ J7 j
7 r! V9 M& B1 ]" M' k# Q1 S- }
3 |- t0 r0 S4 Q, T) ]( [: a0 T3 O; w - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)' R+ y* c4 i7 P/ `8 n
- {$ @8 P# g+ q& _+ G$ d
- _DataPin = mePort[port].s2;
' ~9 r% r) X& b1 ? - pinMode(_DataPin, OUTPUT);
! U6 N1 C& |+ f. }/ L* s4 w$ W - digitalWrite(_DataPin, HIGH);9 L0 c9 ~: e" W$ F1 _
- }; j1 h/ T- V7 {. j; C
- #else // ME_PORT_DEFINED' @+ K! q. }- n' y# p& J2 o
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)0 C: g2 [# x7 i' E; Y2 \
- {9 L. t5 |! w2 x; @' I
- _DataPin = pin;
' p0 `( d+ z- i" | - pinMode(_DataPin, OUTPUT); 6 _. X+ `% f! b; T8 o
- digitalWrite(_DataPin, HIGH);
, {. Z* Q ]$ Z1 G5 A- L9 d - }8 z r4 R9 c4 W' P! u; z& q
- #endif // ME_PORT_DEFINED3 e/ i6 P+ c0 A. B
! m9 t ~9 l5 b/ h" }0 q& A5 ?
) S% W8 I G, B( ?( p- void MeLineFollowerArray::setpin(uint8_t pin)
/ f+ G. ]$ W/ v, E! f - {" \. F" d# v* n7 y7 ?
- _DataPin = pin;9 {/ @; k% J5 ~3 B$ j7 G5 [
- pinMode(_DataPin, OUTPUT);
' ? q, S. e; Q7 N - digitalWrite(_DataPin, HIGH);8 s8 v" F) X4 g3 f, r
- % N! ?/ a. C( e: R' U* Y& f7 d8 Y
- #ifdef ME_PORT_DEFINED
2 i0 L: y, T g; o4 E5 g' d2 X - s2 = pin;
, ^( i' j5 U, D7 l% r - #endif1 o& ~8 D9 C6 V! s8 T' f. I, d
- }
% g6 C! b8 ?+ C - . z( G0 ~# l8 ]2 P: W' q$ @9 H& G
- uint8_t MeLineFollowerArray::getValue()
x$ y$ O9 Y/ z0 ]0 S& p6 W - {8 {* b% ^" c4 [+ T& }9 t8 J
- uint32_t LOW_level_read_time;8 @& G0 ]) l8 H, F. u
- uint32_t HIGH_level_read_time;
4 P# F" e0 m: |% l; L7 G; m - uint32_t time_out_flag;1 S7 M' @" k S7 I7 u
- uint8_t Sensor_Data[3];& W) p" \5 `: \/ S/ F& d4 P
- static uint8_t old_data = 0xff;
6 P9 }3 e' k) f3 Z4 M' k - 3 C# D0 J' e& {, D
- pinMode(_DataPin, OUTPUT);
9 B1 ]4 |: h, l% x( p - digitalWrite(_DataPin, LOW);
" {5 D! J2 _$ M' k$ X - delayMicroseconds(980);
+ ^5 s$ J; `& L( J4 x9 o+ N! `. M - digitalWrite(_DataPin, HIGH);
4 ?' q" y* {' ^9 l/ z
$ f) Z# f+ p# m: [( |& U- pinMode(_DataPin, INPUT_PULLUP);9 x/ P" u/ X% F$ S$ X: S
- delayMicroseconds(10);
1 x. o- o4 m& K% e
% |7 | S) D3 i- time_out_flag = millis();
6 X& i& \& d, M3 s. } - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" Z- w0 P. }% L, ~( _) \7 T
- . L0 n# P5 ?7 C+ E4 w7 `7 p( t1 Q3 c; t: y
- LOW_level_read_time = micros();
% \- |0 p1 ]( t9 c' @# m( h# u6 c - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
: q4 }( @/ o) g7 G% G! h - {
; j& ^/ @( J+ A' C5 ]) w - return 0xff;
/ ]" J4 Q" W4 _* U. u O3 ^ - }
7 @ ~' \# H. v
! X1 n9 o, _ R/ k+ F# d- h- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 F. M- g8 H$ _8 L& ^( b - ' l6 y$ V( Y& M, T+ [
- HIGH_level_read_time = micros();0 d- a6 h. w$ J9 n5 M
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level& S' w! V- ]4 G% F
) _ M' A: t& u: ]* z4 r+ w- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
* q5 O3 [+ L8 Y4 ^. R( e' l( P, ^ - {
: i* H5 y. D: E$ ]. L+ p1 i5 [ - return 0xff;/ c6 _! @* O3 N& |! Z* {" F. V) N
- }# b% J h; j- c
- 9 L" b: l: X7 H& R0 z( `% |. t8 U) Z
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))9 ~: Z1 ^. {9 z8 j/ u7 W
- {
* |) n8 ~ F; n' i0 O - return 0xff;: C7 T' i0 Y9 [9 Z
- }
; w/ x' s% z( U5 b, a
6 b& R% \: Q; @4 T; h" N6 z- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 x% \% w1 I% h
- LOW_level_read_time = micros();) Q( `4 R( B( _* ?% g
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level' O' a9 p8 C, b- h
- " J' R( X8 y2 V
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
T9 |! Q" |& N' x - {
3 J2 t9 _9 |& r W( W7 d1 S - return 0xff;5 B' S1 f! b1 h% t- R9 m8 V3 ^( n
- }; ~: V, @+ m4 x, g Y8 O
- 3 L! j, [! H; F6 U1 b3 l4 }! f- p
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
8 U; m- b; \* i/ B3 T1 I# P - {
1 r0 {. q6 V% m' _ - return 0xff;
/ x4 @, _& o3 x0 T1 k1 u5 V - }
. {) g: Q% K2 u' |
" o" O+ h W4 ^0 t- for(uint8_t k=0; k<3; k++)
* W+ \" k; `8 g* O9 ` - {$ b7 c! G2 Q* _
- Sensor_Data[k] = 0x00;
" d- n! K" g( \1 K - $ `, c% f' J0 p3 F; m
- for(uint8_t i=0;i<8;i++) P6 u, a: Q3 }6 W" k; J
- {
+ B3 T& n( f9 f/ Y, F+ p3 Q& R0 F - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level8 L% d7 x) o6 r! \( {
- HIGH_level_read_time = micros();/ I w5 H: |# N |# R
- LOW_level_read_time = micros() - LOW_level_read_time;% l* r, T, t1 U1 R! _) x t4 q
- , L9 J6 t7 P0 Q
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )3 ^& c+ W5 M6 a
- {' j: n$ Q4 ?2 ]" Q0 X/ h. P
- return 0xff;
9 N* p0 F6 Z5 c - }
9 m' E- H& H: x% D' K2 ]7 T( U - ! O6 U# p k5 c) H8 H3 T
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 ?8 o; N! o( j. w8 L. d; x5 r0 s - LOW_level_read_time = micros();, { h% |2 ^2 s& v. _* M9 u9 Z
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
# W1 y2 X x" Z - ; ~% a. f$ _5 H$ c
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 12 J @$ H: ]# Z5 q7 h) Z( ~
- {
& [! l$ O! w7 N2 H; c% @ - Sensor_Data[k] |= (0x80 >> i);
$ G4 w) }5 X3 P# q2 S7 Y- o - }
4 T4 e$ @, m ^, J, R1 ~ - else if(HIGH_level_read_time >= 100)
y5 {3 m8 l6 Z5 |# a - {6 n, `* X/ g$ D; o+ A3 y# Q, X& M
- return 0xff;
* X# n# Y9 j# E - }3 I; R0 V Q# y5 N
5 F* d+ Y4 s$ n9 A8 l/ g9 l6 ?- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)# [$ l# Y& o* [; O# v8 c1 X4 n9 P% ]
- {
+ j1 w- v& {4 E) } B - return 0xff;
1 [/ s; J4 M. x1 z% H& w - }1 I U! U7 f% M8 U$ Q6 P
- }9 n$ W6 |6 e( r! ~
- }" a+ I3 P) t" l3 z. R9 }2 z1 z( q2 S% e
- 9 f( z8 j( x! m+ p" S2 [
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
) g1 ~4 E9 m/ ~' |' V1 f( f6 D/ } - HIGH_level_read_time = micros();6 Z$ M& q1 U' W; i& p
- LOW_level_read_time = micros() - LOW_level_read_time;& j- @" L# Z7 \$ G4 ?4 t1 h
- t" h0 o' _ d- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
; V" E4 r2 w: s8 l/ A- `, L' C4 P - {
7 m; U8 G4 r( K% @4 e - return 0xff;! J. r9 w$ C i5 @# [
- }
]" u4 \- T5 Y4 n) H6 x
H. ^6 z9 S7 \4 z" [* a' W- pinMode(_DataPin, OUTPUT);
2 [( ]" C7 [4 K C/ I - digitalWrite(_DataPin, HIGH);
( e+ V8 u2 Q5 m6 h1 l
, z9 t- S+ Z4 R- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
5 q) V) c3 `0 V - {# j8 ~& ?# M% }% n( E
- old_data = Sensor_Data[0];6 D" Z6 i" f x w5 G( J5 V& Q
- return Sensor_Data[0];# X/ w. e0 N8 F: @: m
- }
$ [& F8 @4 n; W' k - else
/ A. ?+ S |; _2 \# M4 c7 F( t8 O3 u - {
2 u0 G2 Z( {9 [- q! d - return old_data;, R2 S% h& Z4 v
- }
3 l* U3 \: P8 F5 A - }3 ^2 N/ X% H: A N/ c g
複製代碼
4 {: Q! M# ~$ k4 j" e- ^MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
- x! w/ M6 Z! ?
# L0 }* J- k5 N( B5 F |
|