|
|
& U0 p( C) f) L( w0 V- #include <Arduino.h>
( D: J' x3 ?, u$ H: r% _+ [: v - #include <MeAuriga.h>
+ f: }, K# I8 h- M6 | - #include "MeLineFollowerArray.h"
: T" W7 Z& l' Q! S7 N - . Z" H5 [* M) X* M2 \2 A
- MeEncoderOnBoard Encoder_1(SLOT1);
/ v1 h$ G6 r% H" L; m2 \! e - MeEncoderOnBoard Encoder_2(SLOT2);. |2 s8 H( R7 k: J! G. ^7 A- d: t
- MeLightSensor lightsensor_1(12);# w3 o9 ^9 l. k3 K0 o5 ? ~
- MeLightSensor lightsensor_2(11);
4 C& D& c( g5 J( K - MeBuzzer buzzer;4 n# t: ^$ K1 M+ ], M' S3 p; ?0 N
- MeLineFollowerArray linefollower(PORT_6);3 v% T! G. b2 d
- ' T1 G1 i: }- q+ \ a" V. O% z
- #define Error1 1: k6 m& [9 Q6 p. B% k5 O
- #define Error2 2. L5 F4 J: V4 v0 T1 t! q) M
- #define Error3 3# V) w: S# l3 m* D; x
- ( s: Y# F# H' c) e
- #define Kp 15/ K, ~1 I5 x7 X. n" x& U
- #define Ki 0.15
: R; I3 S. `) N6 q" O1 \% n - #define Kd 0.03) i# N& P( q4 \7 `# O
- ' Q, i. i d" j# a: b, l+ J; ]1 d. ^
- uint8_t sData;: a# G- e/ f: Y8 f
- uint8_t D1;0 O! y, l! r& O/ x& i8 L
- uint8_t D2;
! L* r; h& D6 D - uint8_t D3;
) A2 H5 N0 i- O8 A$ z - uint8_t D4;; @% o3 e; X. U# A- l! L: V/ w, x; A
- uint8_t D5;
/ U1 |( w6 B$ f& D' T' n5 ^ - uint8_t D6;
( L. \$ W- f: B3 [+ ? - ) M- ?) X2 S: u) }, g" k" `
- float previous_error = 0;
8 l, B6 N; R6 X7 e; [1 g - float integral = 0;% h" `6 V# ~2 x6 ?" F9 n" k5 W
- float derivative = 0;
A% L2 f, O, H+ x# c8 K! \1 c4 h2 } d - int Speed = 160;
8 l, }, u( O+ {8 z: P! i2 ]1 @) J - float output;
* }# q# h" K- u0 W( T( B6 |3 f8 E
$ D2 L. B3 i9 W- byte Left;% ^# l! D( g5 D* ~8 a
- ! p4 x( G1 m4 J7 N3 B& y6 z% a- t
- void setup()
. }& m" C& L5 x# @+ W' [ - {! c/ W+ `6 Y0 n" K, H& T! u
- //Set PWM 8KHz# w; Y) U. F6 d1 a# g6 ~7 g4 F7 Y
- TCCR1A = _BV(WGM10);2 b, l% ~1 F% i; O% T2 ]
- TCCR1B = _BV(CS11) | _BV(WGM12);# Q2 s& u; I- i3 d+ y
- TCCR2A = _BV(WGM21) | _BV(WGM20);+ K0 [6 c; W$ k
- TCCR2B = _BV(CS21);
+ h8 J% X+ c! n9 M0 Z2 U - Serial.begin(9600);- s1 z/ E* W* J6 Y D) E
- buzzer.setpin(45);
# s6 E2 I$ L' e) f) m h - }
# u! F1 R, h; f! y4 V" m
3 D9 r1 c0 N" ]9 ^: X: x- void loop()
5 a. @$ t! p* | - {" u- |) I- b* n& V' f! I: `- |( M# X
- while(!((lightsensor_1.read()) < (10)));+ T* V7 M. P7 I. U! T# p) D
- buzzer.tone(1047, 500);* ?5 d' T3 Y W H
- delay(500);5 c9 w" E* [: |
- do. i* d4 x7 V4 ?7 n- O4 u) w2 S0 F6 o* Q
- {5 \8 w. w% j) D! i* O- q
- int Err = getErr();0 `8 d8 ?5 a. a) K4 u
- if(D1 == 1)4 ~: b8 j1 N6 S9 s& m8 y1 P& q: u3 D
- {1 J+ L* R3 q( M& i' _* f) ]' _
- Left = 1;
# Q9 |/ f0 U# }' L% V' C: s" J" v - }: z7 _) ]2 K" z# y
- if(D6 == 1)
# c* u3 `1 j8 z4 D( M. U - {- y) C: V x3 Y
- Left = 0;0 E! J/ g, ?& T# f/ e7 _1 K' \# r
- }
- ?2 X4 Y6 C4 w. l+ s - if(Err == 99)
' ~$ k& o- h% d0 _7 w9 b8 m - {
8 F. l Y7 f0 p - if(Left == 1)
6 i8 P9 Y* I5 d - {$ O& w- I ~- E' b+ a4 C
- Speed -= 5;
. v7 J/ v$ X9 B- b; M! a1 J) d/ l - moto(0,Speed);
8 w' {) Y1 F4 u - do4 c) J2 M6 z8 e7 H- }! Y
- {1 ~2 o- ?, [% e, n# p' g
- Err = getErr();& ~1 y& G8 c0 X4 u6 l6 s% |
- }while((D1+D6) == 0);
" b) S3 ]2 H! `7 f4 e/ I. t - }
1 j( M* t5 C" L9 ~9 z1 A4 d* h8 g! R0 G - else3 h# C+ W l4 j& t2 m* x) s
- {
- a+ Y; O/ }7 G7 l5 m. p; R - Speed -= 5;/ E$ ]) |7 {! l& h$ R+ @
- moto(Speed,0);
f6 |- |2 g% i' N - do
" G( q3 b$ Z1 e - {5 ?* u/ x& B, z R
- Err = getErr();$ {7 m' E2 U; Q& L# {) P
- }while((D1+D6) == 0);4 I7 l" h1 R7 V p
- }
* L- `, g. j5 {) o6 N9 R - }
! p5 s9 N* m# E$ [8 Y) {) Q; v - else; H+ h" ]& Z- | j% |4 c# Y( K# D
- {8 K. B2 g9 B1 V- ?
- if((Speed < 160) & (Err < 2)) Speed+=1;
* ]; a% d) t% [3 L% d+ z: h: I( g - if((Speed > 100) & (Err > 2)) Speed-=2; . E x0 @# a4 ~$ w/ ?- I. S' K
- integral = integral + Err;4 `4 ]9 B4 z" |1 l# l; H! F0 |2 u
- derivative = Err - previous_error;
6 X6 }# c, X. l8 k& c# t - output = Kp*Err + Ki*integral + Kd*derivative;1 W& W& j2 ^6 U
- moto(int(Speed-output),int(Speed+output));. Y( `5 M+ v0 L1 t# I
- previous_error = Err;
! c: n$ S6 K' z3 Z1 P/ B - }
: w% B7 Y o$ F - }while(!((lightsensor_2.read()) < (10)));( _! p+ H# L3 p* v0 O
- moto(0,0);
8 |( O7 x' s5 ^3 N9 R - delay(500);) I" m4 }9 L* e/ y5 _
- buzzer.tone(262, 500);6 r$ ]* B! E) P
- }- L; T; V5 m2 s# ?
: I/ A! ~& c' T9 V0 g; A2 }7 o- int getErr()
% |2 s8 D9 ^5 G+ E. B1 c) v. q - {
- R% r/ `, f7 u N# T, ?8 ]( @ - sData = linefollower.getValue();
4 f7 P. a& J6 A8 V6 z4 L. N0 ^ - D1 = ~(sData>>0)&1;" P2 T8 J7 |8 H' b& `6 [+ l7 p
- D2 = ~(sData>>1)&1; x5 L/ f! ^+ c T: s4 b
- D3 = ~(sData>>2)&1;, Z0 D$ m2 P/ Q5 i! v" t. ~
- D4 = ~(sData>>3)&1;# m' y2 p' F+ a: Y6 @* d
- D5 = ~(sData>>4)&1;
2 |1 z6 l5 J6 T, X - D6 = ~(sData>>5)&1;
$ K0 k+ Z+ z% V - int downD = D1+D2+D3+D4+D5+D6;
/ N+ G8 X- m8 S- B5 P6 q - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);4 H2 C* Y6 r( W2 n3 `& @
- if(downD == 0)9 Q1 o/ f' [& _, L# N/ J
- {9 p" b: G' ^ p }! c* c
- return 99;
# e/ z6 `- _8 [2 [+ g9 @4 t - } X E2 Q, J4 ?( ~7 w& ^
- else" i$ T7 ] e6 S7 h* i7 m" } K
- {
; j! ]1 ?' P4 Z+ n - return upD/downD;) I7 |: ~! V. n* R& v
- }! p1 w k* ^% J# p% ]0 X
- }# v$ @; T$ H. b2 ~5 ?, r) a+ X, h: d" F* b
- 0 P6 N0 X; w: U4 ^9 i
- void moto(int Speed_L,int Speed_R)
' `& }- _. i' F5 [ - {0 L9 B9 `! G( \! E5 q: A3 `
- Encoder_2.setMotorPwm(Speed_L);" S2 f* k: d/ k l- p
- Encoder_1.setMotorPwm(-Speed_R);
) f" |5 d! g7 W9 D4 @, P5 ` - }
複製代碼 ) _# Y' O" S# O/ k
MeLineFollowerArray.cpp# H/ v; x. }- p
- #include "MeLineFollowerArray.h"/ ^- ]# T2 Q1 \+ M
- 8 r1 i8 B4 B. G, P' s% x
- #ifdef ME_PORT_DEFINED: J) {$ u4 h4 {- f, \- L8 e
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
/ d8 j# ? }# Q2 a* j - {2 H; @, A# K) e1 `
- / s7 s1 s7 ~* ]) K
- }
. V- d; f# ^" n0 [ - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
2 @4 o" Z4 n8 [ n% J& @1 O5 h - {
" C" Y. X$ v$ G" S0 i+ r: u) m - _DataPin = mePort[port].s2;1 k' [& q r" D& _4 h
- pinMode(_DataPin, OUTPUT);
2 p7 L+ m9 ?+ D3 z - digitalWrite(_DataPin, HIGH);
+ I7 G* A% B& ~6 ~9 i - }
w2 j8 N1 A1 m' s" b9 B9 A( f - #else // ME_PORT_DEFINED/ V) m+ @" P ~- l$ R
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
% ^0 n; O6 g2 h/ T - {
. E( w3 }: |( y - _DataPin = pin;
: s' n/ m& m* `& q - pinMode(_DataPin, OUTPUT);
# H# j, o0 }9 j4 j( i) y, e - digitalWrite(_DataPin, HIGH);2 C% |9 B* D& ]
- }5 u) F0 v8 B H/ |, e
- #endif // ME_PORT_DEFINED
, Y3 r4 A' ^6 y' p: A U# F6 b
) b6 `0 ?0 Q$ ~+ J$ N- " u+ Z r! C+ C
- void MeLineFollowerArray::setpin(uint8_t pin)
# J' n- _3 {2 @7 V4 {0 O - {' R. n$ ^ l7 @) }: x/ R
- _DataPin = pin;
A' Y4 I4 H) g$ I e, a - pinMode(_DataPin, OUTPUT); 0 p; i& a3 \ ~. A7 E
- digitalWrite(_DataPin, HIGH);& A* _+ ]" _6 S3 {# s N9 y
7 b) l, B# |5 m3 ]8 C- #ifdef ME_PORT_DEFINED5 I! O/ d. b/ J8 x W, H
- s2 = pin;
2 o$ e- j' Z* t3 ~ - #endif6 T j8 N# U1 v4 @' L+ V
- }
4 ^3 p7 M! x$ B6 E
, u% L& `% D% x- uint8_t MeLineFollowerArray::getValue()
( h1 y# | X c) z - {) ^5 ^1 Z5 j& v& d/ c7 E5 ?
- uint32_t LOW_level_read_time;
# P3 d- G( k$ ]2 P6 {' U. E# l - uint32_t HIGH_level_read_time;
, E( y6 q' c1 k( u- _ - uint32_t time_out_flag;
+ t2 e/ [7 @# Z0 i& ~# r4 @7 h - uint8_t Sensor_Data[3];8 C+ M$ x/ u m5 [
- static uint8_t old_data = 0xff;. \2 ^# C* N9 h4 p
- 3 D( b; R2 o% h9 n% d
- pinMode(_DataPin, OUTPUT);
6 n8 b7 ?7 T( a. \' a, @0 J! i V5 Z - digitalWrite(_DataPin, LOW);# v) E2 I4 E4 F) k' {
- delayMicroseconds(980);) H* _, q3 [8 U
- digitalWrite(_DataPin, HIGH);
& d4 b* v1 U4 X+ @ - 0 i! h/ N, D1 R" g6 R3 B- D5 o7 Q
- pinMode(_DataPin, INPUT_PULLUP);
4 B: s2 j& | d8 ?) X$ ? - delayMicroseconds(10);+ a7 I5 l7 {3 M9 J9 l
- ) Y+ E: [. t G- a% j9 M( [9 s
- time_out_flag = millis();
8 }. _+ r% \$ R A - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 m4 A7 Q; h4 C M - # r4 v4 T2 g# @
- LOW_level_read_time = micros();& A S# C) U7 @3 C: Z c, {
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 A5 l; M9 d5 A: I7 G
- {
2 i5 }1 _ G* X7 R5 [ - return 0xff;
8 Q9 t& `. _, z7 h0 m( z - }) B. r: g3 `/ R6 R* c5 d
' v' z% f* p! A- t; P/ y: `! a. r) P- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. c7 U( L. v8 E/ T3 _3 t" _5 B4 `0 \
- D9 `0 x) w* s8 ~* C3 J- HIGH_level_read_time = micros();0 S9 p, b, y( Q& ~* j
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
% p/ J; c: u. h% |; K - 6 G K1 L, `, y/ d: `
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
, W3 F$ v7 N5 r' [# W - {
" V4 f, d C ~5 Y - return 0xff;
5 `/ b* x% Y# J6 n - }
/ X% f" U0 O3 A8 ?( i; T9 K
7 h, r& g& m9 Z# R/ x' |: S- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
+ M4 d# X- v$ _" [* @- Q0 l - {+ z3 b& K* f2 n" z' V4 q4 Q7 Y8 }/ n
- return 0xff;
: f7 ]3 {" W4 k$ G - }9 t( q0 l( }2 _: d+ D4 J5 c+ G9 D
- 0 F. O7 a( @) M0 L1 M' V
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); S5 V5 L" I1 m! [2 m8 g
- LOW_level_read_time = micros();
- P& j3 J% C4 X! n" q - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
0 i% x7 R) U# G4 B- `* l5 t2 k
) x+ _) u# @/ u8 z. X" O- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
8 | P' N: h. O ^3 r' P - {9 F4 y% z5 G( v8 N
- return 0xff;
+ y" p" [6 n+ x - }7 x9 l2 g/ o' R: D/ X
- . }0 J" E" ?. g T( s% ]" D
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
2 Z$ Z* J2 d# }" N( s, ^8 X# z - {
( C/ u# B4 h- L- f - return 0xff;
" j$ p+ e0 {% L- |* l8 h - }
; C# }! o2 w- B7 V6 A/ {* M# Y
7 p/ i. A2 o6 @" P- H- for(uint8_t k=0; k<3; k++)
+ F: O; \* m3 K0 Z2 Q" S - {( b3 t# j0 P% M! M" A4 x# N& d" G5 O
- Sensor_Data[k] = 0x00;5 \9 Z, a0 U6 u3 e O
" U! A! z7 i$ h0 C; ^1 U# @- for(uint8_t i=0;i<8;i++)/ p3 n, G$ A: u6 M4 Z O% w" o$ z
- {
7 {( c# T1 n7 O' C1 o - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
7 q% |. }4 D7 ^% c, C/ b - HIGH_level_read_time = micros();
* k9 O0 C3 H g S& E - LOW_level_read_time = micros() - LOW_level_read_time;
4 m+ U3 ] ^" ]& H- }
* z% y- u6 H* x, o j8 v$ s- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
' G* K& v# d' Q" i2 ~/ ] - {
2 q1 {# }) P f% p - return 0xff;
& I1 j4 P2 D* n0 c# |8 {5 K4 [ - }1 Z) t& p& V" m/ m2 o( L% \ A3 w
2 E# F q9 q, Y- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
' |& c" L O! `2 \6 I) G - LOW_level_read_time = micros();
% q& k, u* u6 V6 Z( Y - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level- I' t7 J5 T* T& e' Q* ~, j8 G
- 8 G: q9 p2 b4 T6 N6 o* g! G b7 b5 H
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1( l! J: y4 \2 _% S
- {) B, Q3 `( Y& z' q/ a
- Sensor_Data[k] |= (0x80 >> i);
# z1 |9 S( o" f+ N" k a - }
7 L; Z3 h% u" y+ n - else if(HIGH_level_read_time >= 100)
* r$ b9 P' q+ E7 O - {: x, j0 }/ T# X
- return 0xff;
4 h; X/ ^4 {6 s- S( E/ \ - }
t. f0 X) s* @% P* w
" I) A4 J) O' h0 W/ l0 c- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
2 r5 c/ Z% m. U. ~: ~2 F - {
: u& r% I/ m/ P* u2 G' t5 W - return 0xff;
- g' I6 O9 D; T& x' } - }
, T2 U9 j* Y, E8 F6 B5 D - }
6 N7 @" j3 [- f% X0 S - }
" K+ ^: m6 H" G/ F
, o) c& k/ Y1 C! S8 {- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
# D0 T* \/ }/ N! Y d$ w* O* I - HIGH_level_read_time = micros();5 l+ G! v/ S+ K: [2 ^2 T$ a
- LOW_level_read_time = micros() - LOW_level_read_time;
& ?& N4 m4 ~+ L2 ` - ! Z# \8 }. R" J' ~- n6 u w
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
9 A+ ?6 e0 {( K0 ]( S% t% C - { X3 ^3 ]! v8 R# y& ?! a
- return 0xff;
8 V. k! R* _2 S - }
7 u0 r# S) w ?5 u. v' m) m. [) N' {
( h5 u) A2 X/ i' V4 B- pinMode(_DataPin, OUTPUT);
a, V: F! y1 F6 S( @( A6 |' Q1 c - digitalWrite(_DataPin, HIGH);
1 V' y8 v8 R3 s5 I9 M
4 X W7 L. ]' h. `$ K- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
2 X5 B4 \2 j( S! w" R - {
6 K2 H! U# L# O( M- o5 g" Z3 g8 Y - old_data = Sensor_Data[0];
+ A6 F6 w- ^- p" x! _- ]- T - return Sensor_Data[0];
3 f; p' x# o0 K! z' ?- l( X4 t - }/ |5 d P/ V$ s
- else& H. s/ D0 b# p1 e0 Z
- {7 ?& s. w5 h3 S2 n7 r) f! h
- return old_data;# x" j$ `* b; O$ K0 s3 p; W
- }4 J0 [2 S% |' t+ s1 K
- }
! c& O7 Z, |1 e* N) o9 G% f3 e" C
複製代碼
. C t" |4 X+ k7 @MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
; {7 _9 e5 a/ O; q: ]6 R. t* S+ J
" e7 P9 O0 G. G& \* B1 ^( g
|
|