|
: w7 q; M: L# c1 s) ^ I
- #include <Arduino.h>$ O) F9 {" M5 G
- #include <MeAuriga.h>% W3 H2 X D7 h8 ~- E1 R
- #include "MeLineFollowerArray.h"
( X/ \4 Q2 U0 P% o* c - 5 c2 W& Q* V+ r* e$ v% J
- MeEncoderOnBoard Encoder_1(SLOT1);# u8 V7 |% y) V% [; |
- MeEncoderOnBoard Encoder_2(SLOT2);
' a/ d) a& K# h' `; c8 F - MeLightSensor lightsensor_1(12);
+ E& O& t. P g2 u6 a" V - MeLightSensor lightsensor_2(11);
% @3 e1 Y9 F8 ?. c4 Y - MeBuzzer buzzer;
; I! Z/ r3 q1 [9 e0 j3 T - MeLineFollowerArray linefollower(PORT_6);7 |+ L5 V& u$ p' V8 R) ~4 A
- - v# o$ {+ X& C3 c; B$ r- K, n$ d
- #define Error1 1# a4 C7 V5 M8 b% j3 B2 W, S7 ]: d
- #define Error2 29 B0 T& [7 h1 h( P( ~3 @
- #define Error3 3
* ~, w! |- |0 N: s) f; e
; {9 o# U( x& t# z" u- #define Kp 15" V7 [0 E. h7 B) a% d/ }& |! G
- #define Ki 0.15
. \; l9 d: R4 S$ G( Y - #define Kd 0.03" ]- N+ g1 D' E* I, h
: x/ [& D! y1 l) {/ q! ]" t- uint8_t sData;/ _8 ^4 N6 q2 L7 k: m& U
- uint8_t D1;+ \) a. T Z. ^6 U
- uint8_t D2;
6 U' i3 Z1 _9 A% y( r' Q, y% } - uint8_t D3;
. m& ~- b5 g6 [* I - uint8_t D4;: E- l2 ^% c- h( o, W& ^
- uint8_t D5;
- w* S# g' q1 {. b$ h% U - uint8_t D6;
; F6 i& O( f( i - + j- H# {, y$ o9 ^
- float previous_error = 0;" h; ^ d A* w; I' U6 h4 `- {
- float integral = 0;' ?9 ]# v4 d: ]. b
- float derivative = 0;
! i3 f# \6 \1 @ `- p - int Speed = 160;
+ H; `; @% L2 A - float output;/ u3 Q! U7 ]6 M1 L' X! u
5 y! S) v* L O) I0 i" t- byte Left;- L% [8 {/ U0 ~/ ]. Y% z
- ; @; P( X3 A, `+ H9 y% p1 H
- void setup() , L, N! S9 ^: K7 F) L
- {: p- O3 b2 c/ |/ ~+ p
- //Set PWM 8KHz% n5 Q2 @1 A* [ j3 D7 w' a; h! {9 {
- TCCR1A = _BV(WGM10);1 Q5 P9 {' K6 L8 u5 a3 w$ g
- TCCR1B = _BV(CS11) | _BV(WGM12);
0 a4 T3 K$ M' H% R% Z$ i# V - TCCR2A = _BV(WGM21) | _BV(WGM20);
: T& ?) F1 Y/ a V5 C1 M1 t8 r - TCCR2B = _BV(CS21);. e, A( G6 h8 V, r6 \
- Serial.begin(9600);
& g, X1 l' L7 P+ d7 a1 g# Z) g/ N* j$ z - buzzer.setpin(45);
9 m! O0 ~1 h/ ]/ Y( P - }) x; d4 s! q. _
- # t, ], [2 Z; ]0 V) P
- void loop()3 ` a9 u+ t) C
- {
9 Q+ N/ h, t2 S7 s2 ~4 T1 J/ ]- J - while(!((lightsensor_1.read()) < (10)));
7 ?' r$ R" M0 `0 C2 o - buzzer.tone(1047, 500);1 _, n( b2 \$ `" S& X
- delay(500);
$ b8 D' I. Q; u- E6 T. |" A: | - do5 j: c& U/ @' p8 t
- {
9 h7 M0 S- f1 u - int Err = getErr();
9 H4 C3 A. C9 I6 |, Z - if(D1 == 1)
4 R* z9 g5 N! R2 {0 ?0 Q - {! a8 A! B- _4 G
- Left = 1;+ s& k( s- Q" W# Q/ {. j
- }9 v" U8 a; M& h# D
- if(D6 == 1) ~" z+ ^5 j# j' S/ O4 L
- {( S1 v# X& o" t- t
- Left = 0;0 I- {; F! A* d
- }6 W8 f6 ~: |" O8 E: ]
- if(Err == 99)
9 U4 A3 Q9 Z' f8 W' ? - {( Q# ?& e" }% V$ `1 c
- if(Left == 1)9 C# r2 B5 ~8 B. J( Z/ o0 x
- {" x% O6 X$ i+ B, J" k6 m- w1 G0 A
- Speed -= 5;
: ~& i( q1 H3 E - moto(0,Speed);
! ^! }# j! Q" q8 _& ^* V6 x+ z - do* `) ^) p2 C$ u9 [ @6 }* K
- {
2 q( k3 [) g1 [ - Err = getErr();) B4 q* y2 q- l) E$ z; A$ c' f
- }while((D1+D6) == 0);1 }, I: z- F# N+ t1 k
- }
+ t9 V9 @! E8 @7 y3 e7 ` - else
' T" s4 x) o4 L - {
+ M1 G1 ?4 A/ e+ B& c* e - Speed -= 5;
2 H. D+ `7 k3 [$ W( z' A% C - moto(Speed,0);
8 `& J3 g' x$ O, @6 [& I9 e; ~* E - do
* V' ]8 d y4 |' z7 i2 r, [$ y - {: o+ T+ A/ J5 `/ K
- Err = getErr();+ N( s* r. G8 D8 h4 I
- }while((D1+D6) == 0);+ L* v8 l8 c1 u4 s
- }
1 X& W" t* R( ? ^ z0 V' r - }
8 m n8 E( h7 {5 v0 d" g - else% o" A. ~. C! A% C. ~% y
- {
! G8 w/ F) g6 U5 H - if((Speed < 160) & (Err < 2)) Speed+=1; 7 L0 T& ~3 P% g1 V4 F$ l# G, x
- if((Speed > 100) & (Err > 2)) Speed-=2; $ B5 g0 s; K. s% F1 i
- integral = integral + Err;4 I% i/ ~% @/ E' g" k1 L
- derivative = Err - previous_error;6 @9 t7 E1 A0 s0 I3 G! }3 H! ]
- output = Kp*Err + Ki*integral + Kd*derivative;* a T# I2 x$ j
- moto(int(Speed-output),int(Speed+output));0 h& N6 J( u2 M0 ^$ |. e
- previous_error = Err;
: @9 U& X+ n% Y8 L - }* \5 G+ z0 M- `7 e/ ~ \
- }while(!((lightsensor_2.read()) < (10)));4 k9 g7 g0 z/ {: Z( B1 N8 g
- moto(0,0);6 Z9 i, H3 {& U5 i
- delay(500);! i% I! f0 ~# s0 r% u
- buzzer.tone(262, 500);0 X' }. N( S" f$ {: J
- }
! J" r' s9 L' a* ]4 V+ p) `
6 N& E, U) k M m; j, i- int getErr()
$ ~0 D2 \4 h2 N7 ?9 r. J) L8 ? - {
& R% Z4 {4 g+ W5 R& Q9 C+ ? - sData = linefollower.getValue();
: q% c" {8 `& \4 `8 `5 E! c - D1 = ~(sData>>0)&1;
- W1 _1 C3 y% E9 F! b+ t0 A - D2 = ~(sData>>1)&1;
/ N) ^% D2 S+ _0 M; G# a - D3 = ~(sData>>2)&1;; ]/ I3 d4 I1 O+ D$ E
- D4 = ~(sData>>3)&1;& G3 \7 K( L4 x& \; L3 T
- D5 = ~(sData>>4)&1;
5 G5 U/ E* q% |) r - D6 = ~(sData>>5)&1;
; f5 B+ e# p( Q7 A& U* O1 s - int downD = D1+D2+D3+D4+D5+D6;
! F8 \- U- c- ?" y, _ - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
" u% m' _- R* H! J - if(downD == 0)
( ^) h2 L! i' q5 K - {
" E( J( ?! q/ J+ t. t6 ^1 g - return 99;
2 Y8 A- D7 r% W" |& F8 B2 q" c - }
/ @1 _. S9 F$ x$ r6 V9 o - else8 `1 r) f* w5 x5 l, r
- {/ |% K- {. S* b3 y2 z5 ]
- return upD/downD;& b Y* Y1 v. a2 N
- }
) H$ {2 X/ F/ j - }
/ F7 s2 u/ G3 c% L2 S4 f: M7 v1 t - : H% u8 l1 N& M6 ]8 Z& }) @: [
- void moto(int Speed_L,int Speed_R)
9 Q6 u" Z+ I5 b+ u: { - {
9 }, f0 [: o: L! _0 U - Encoder_2.setMotorPwm(Speed_L);
6 o6 p+ z5 i. [ - Encoder_1.setMotorPwm(-Speed_R);- F9 i: n1 |" l. @
- }
複製代碼 ; F& p- _3 Y$ H' @; B
MeLineFollowerArray.cpp( g4 T6 b7 p, u6 l* l. J1 C
- #include "MeLineFollowerArray.h"/ {6 k# ~, c) c" t, s
- $ Q" e4 ?/ D% U$ A- M
- #ifdef ME_PORT_DEFINED d# Y# c, k3 ~
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
. R! Z) D# G2 o X1 i, n - {$ I' U* S4 U/ i# e3 q
- ; @+ |. L; y- V3 C
- }# P+ g9 C2 `' R, \6 P9 o
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)/ G7 [' D) P( g" I! W3 V2 X
- {
& B7 N& y! ?- d/ v - _DataPin = mePort[port].s2;
9 x7 j+ _9 k% S% g: S9 l/ A9 D( ` - pinMode(_DataPin, OUTPUT);
' O* c- s/ S6 f! B8 \) t+ v1 }. N5 Q - digitalWrite(_DataPin, HIGH);3 @; A- c3 _9 O
- }
2 L: ]4 v m# ~ S - #else // ME_PORT_DEFINED
* ^( z5 l4 Y2 d4 n/ k - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)/ \' T" U* e$ P2 `+ I
- {; P- R% \" f2 X+ I6 p# D% D
- _DataPin = pin;0 p& V9 }9 l& m$ L5 a
- pinMode(_DataPin, OUTPUT); . \7 _% x% ?2 I {
- digitalWrite(_DataPin, HIGH);& p9 H1 ~& ?8 l9 i( ?
- }/ W* ~0 y5 W1 t0 O3 H. D
- #endif // ME_PORT_DEFINED3 M. k5 O2 g# |6 P4 j6 D
: k, e- z- P( U. P; q( }
% c1 S O% n+ C* }7 O7 Q- |1 h( B- void MeLineFollowerArray::setpin(uint8_t pin)- {; X# R Y! A: x8 d
- {
; J5 @' W+ B6 O" g% Z% l! o - _DataPin = pin;# _, ?1 `/ A- g: l% G
- pinMode(_DataPin, OUTPUT);
0 x" l" m4 B% f" q - digitalWrite(_DataPin, HIGH);
) L/ g/ @0 d2 ?
2 W7 q% j; ~0 N- #ifdef ME_PORT_DEFINED
* n3 @0 ^* ^4 ] - s2 = pin;
5 X* i6 `3 P4 P" a' Q - #endif
7 D+ e3 n2 h; ]' X" r, q" N - }% W0 U5 j- E* c) g
- * u$ l: L( q& w5 ?, U- f0 U
- uint8_t MeLineFollowerArray::getValue()6 I: [- k( q+ M. ?5 h
- {
2 U1 F* u; `+ k - uint32_t LOW_level_read_time;$ E1 G! @5 c- G+ b
- uint32_t HIGH_level_read_time;$ h) x g$ ]. t) Z/ s& d6 p/ X7 u$ L
- uint32_t time_out_flag;8 r0 J. }9 y6 F
- uint8_t Sensor_Data[3];, Q# S+ J) z' _. H+ a( x
- static uint8_t old_data = 0xff;
7 d$ i- @; A* ~, |8 f, ? - % r3 U$ G5 ?. b7 L! U6 G" e8 n
- pinMode(_DataPin, OUTPUT);
! F9 b# t, x- j - digitalWrite(_DataPin, LOW);- f+ J9 k/ {$ K4 ~9 x/ O
- delayMicroseconds(980);
, Y' y; M1 K8 I: C2 y - digitalWrite(_DataPin, HIGH);3 f9 P; d( o1 ~8 {9 m
T0 ?: a6 L4 c: o f4 j! m: X- pinMode(_DataPin, INPUT_PULLUP);' {' n$ V u U
- delayMicroseconds(10);1 |4 D6 v( q6 E% l, _. D" G8 F
% {# J# a$ U$ {# q- time_out_flag = millis();
3 ~- F6 A4 R, n# A$ l+ j8 A - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );3 ]. G5 H1 Z: H! |' e
+ h1 g% |" X8 d8 t- LOW_level_read_time = micros();) s+ }9 N$ L6 S" T A
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out9 h, L" R/ p+ H2 l
- {# t7 B: B) T9 ]- r8 U& ?
- return 0xff;
9 D6 j& Q9 c/ Q L. ` - }, W4 M3 Z: {4 H2 K+ M+ V
* N$ _/ B( w* S# e4 v- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );4 N4 U! @+ ~/ w# L# M0 O
- 8 |) X7 n% }/ _
- HIGH_level_read_time = micros();
# j% C- O2 _4 ~ - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
* j9 G4 P$ f7 k b2 H+ T
8 b; [. w9 A0 W& E- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out9 p+ ^% t. m# ^$ _
- {
7 o8 ^ n9 ~; B3 R$ e, F/ V$ L7 ~ - return 0xff;
& H! e$ _0 b; B; u* Y9 p0 S - }
* N3 L7 Z2 G$ H - 9 m2 e: n0 X0 |* u7 Q! F8 o& n0 e
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))7 b1 ]- k9 O7 ]6 [
- {
4 v* `8 r3 R* @8 J7 f4 W5 d - return 0xff;
3 u8 z4 Y. z) K' b" C% G6 ~+ G* ^ - }
* W" `/ |& q# a5 K
- r9 {. `1 r1 v2 q8 C) l; T$ j- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );6 r* ]2 I2 w+ S7 l# {! e) X7 k8 }
- LOW_level_read_time = micros();
% Y" Q6 D7 ?9 J - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
3 v9 M- d* A- l7 w- y) w) e - : m, s% J; ?9 }% d+ m
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
; p+ [6 h- f1 Z: Z- \1 ^6 k! s - {5 A4 u) F+ t2 |; ~4 j8 H$ r# o
- return 0xff;
" M$ q" i" q X4 G' { - }8 V3 p( M) T9 L* f$ e; E3 }5 N. n
9 J2 o& X* C1 f7 ~" O( w- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
6 N* `! [3 o# s" C; R. U - {
) F) \8 E' {' i r - return 0xff;/ T8 U B; H/ o z# \, C3 t8 G6 x
- }
: C; r5 {- P; Y# l/ s+ q2 ?4 Z7 F
3 v( g% l; w2 [/ d5 M. n4 D- for(uint8_t k=0; k<3; k++)7 P0 @/ E' c/ A( ~+ m+ z
- {) Y$ \6 {9 W+ p* F, ~' }1 y9 u$ v! X
- Sensor_Data[k] = 0x00;% ` `: {) v/ A2 p8 f& C* n8 R$ [7 k9 i
- 3 L' T2 b7 V" {
- for(uint8_t i=0;i<8;i++)+ D" k% v. V) [
- {- X0 t- _2 K( @) E/ N$ T/ P$ K0 Y; }
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level- {0 ~3 ]2 L( X1 k
- HIGH_level_read_time = micros();
7 T4 q% B, R* B! D! O: c) @ - LOW_level_read_time = micros() - LOW_level_read_time;
- h* U7 m9 \3 K( N+ A5 N$ u G
1 c6 c# b2 {6 `8 o$ |4 L9 |- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
$ j! N2 ]4 z! }# ^ - {1 L {* |5 y, q8 ?0 E/ M. F: |( t8 ~- }; M
- return 0xff;
/ V# Y2 h6 @) F Q - }
' }( I- p( l* {0 y8 s3 U, T% D- q
$ D8 s; r# K/ w- U i$ F2 X! c- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ ]7 G0 p& V: j" Z; t( Q
- LOW_level_read_time = micros();
( I1 n' I0 N: V, u0 G6 Y& Y - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level+ g1 C( Y- y' R* x& ]
- ! ]1 U% C! y) C# v$ m* ?
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 19 a1 y1 n% N x8 X& f' F( I
- {
4 d& q, r/ ~/ z( o, q) U - Sensor_Data[k] |= (0x80 >> i);
5 `7 ^2 n( C- { - }6 G. ~7 i2 s% n+ i- t5 ^
- else if(HIGH_level_read_time >= 100)
% U4 q, d& f$ {; Y- X R! D - {. t3 E" d% p* R& X$ D% S1 J0 U
- return 0xff;
$ L/ ]4 {- ~% S$ n3 { o( o4 \ - }0 _% n( a6 e0 U
- " o2 ~- X( b; ^/ M$ V
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
' ]3 l! X" c0 y- F2 E# x5 ^. U2 O - {
3 B! \+ _ M1 p - return 0xff;; ^. W4 E: [) Q s
- }
% ^, g% B5 s1 J5 W, ~0 n! u! m9 m - }
- _1 h* ^$ P. x2 I+ k6 Y - }
; }& H9 Y0 X" o5 [3 h" v& U - ( M5 o8 \ s X. s
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level9 m [; [7 m/ U# U% P% k& x7 D
- HIGH_level_read_time = micros();2 l; B7 K$ t5 }
- LOW_level_read_time = micros() - LOW_level_read_time;
$ f' `# _7 D" w5 e! X - ) \0 x6 s0 m) g* E+ Y9 ]! [' h1 x e
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
, B, }4 n' n0 u! R - {
( J" ?' E) H$ v. ~ - return 0xff;
& f# h6 a& V' q# A% W - }$ L: I# w4 D8 L
- 6 M O, c* x, |$ E5 l8 W
- pinMode(_DataPin, OUTPUT);
9 p- ^: u3 d! I! M5 v) T. ~1 n' ? - digitalWrite(_DataPin, HIGH);$ H0 T0 O1 s' s8 c x( @* h
- 9 v- [) D8 m$ ?. X
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))( }2 y, t3 x8 J# X8 Y
- {3 X# I* B2 Z, K' v. w: @: ]1 k8 Z
- old_data = Sensor_Data[0];! x5 x3 }+ Y* O& c- M% q1 u4 @
- return Sensor_Data[0];
, l, @9 B- r7 K8 ?$ c; ?9 h& p - }2 d: K1 o# g) `6 g/ ?& i
- else) Y+ E2 Z6 B4 G
- {+ `8 g. b' u- r; ]
- return old_data;
6 `* f+ }# m' Y" q - }
6 N$ g, B ^! o, r3 \ - }! ^9 Y7 y: i# x% A( x
複製代碼
/ `' M! }: X/ ~# ]- NMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
) h. a! G0 m. r' Q+ i
& @3 Z0 ~! w) b8 z. p+ o |
|