|
|
) k! G& n- a: F
- #include <Arduino.h>
) f' `1 \: ^9 }+ \) _# R - #include <MeAuriga.h>! W3 u* N; ~, M/ @* t
- #include "MeLineFollowerArray.h"8 ^4 n; H* o7 ]; W2 Q
& _: K& ^# a( W0 r8 S; ^- MeEncoderOnBoard Encoder_1(SLOT1); N2 J4 [( a, J: C. q( ]) J1 V; e( ?7 b
- MeEncoderOnBoard Encoder_2(SLOT2);
. s! ^& r- ^1 G6 O/ [! J - MeLightSensor lightsensor_1(12);. x1 b% U) d$ A
- MeLightSensor lightsensor_2(11);
1 ?1 f7 i: e& X7 R: |4 Z1 }! t- h' ? - MeBuzzer buzzer;
- h2 [! S1 b- z. y0 Z - MeLineFollowerArray linefollower(PORT_6);+ W5 D3 n1 C# ?! {- [- | m" a
- & }+ N! }' |! u1 a$ b( G* `
- #define Error1 1; S2 I+ H3 ?! B+ H `. [
- #define Error2 2) F( D8 ]# ~/ d) N4 H Y, y: l! z
- #define Error3 3% W7 u* C! O4 t
/ U3 s- z4 C D( a/ h- #define Kp 15" F2 A3 m3 j9 S/ M
- #define Ki 0.152 p& r5 X2 s/ p2 y0 _# _2 p
- #define Kd 0.03
% S+ q# r# i2 Q [ - & J' K/ P- w9 }6 Q4 K
- uint8_t sData;/ S5 n7 T2 a$ ^8 h9 t
- uint8_t D1;6 U( j$ |! X$ m5 f/ i
- uint8_t D2;5 N( L# L3 r a! p
- uint8_t D3;0 Y$ H0 Y7 o/ |' R3 q; K \
- uint8_t D4;
; Z7 z: l; ]; C/ H8 K+ S4 d - uint8_t D5;
- Y. T1 ~6 C. b) u/ A; Y3 ?0 x - uint8_t D6;
" k/ ^1 U5 y# ~ O - : Z" {7 y- R+ H$ z7 W2 y6 h9 t
- float previous_error = 0;0 k! x }; H% K- d% b
- float integral = 0;
5 N. i1 G) A2 W2 q& k - float derivative = 0;
" |# F: N0 A" b& W - int Speed = 160;
2 M% a5 ~/ `' i( ? - float output;
$ ^) l. H' L; Y - 6 o! ~7 n n- Q7 ~- p
- byte Left;
% l8 J' P# U5 b3 k; F T: B
4 w4 }- D! y9 q5 P, |8 k& X- void setup()
. ^) A6 i2 ?, U! ` - {# }$ N1 k; U, ^3 M
- //Set PWM 8KHz
- i7 Z8 ?# J+ u' G+ G3 z) ~' R - TCCR1A = _BV(WGM10);
7 F5 E" T% q2 b6 u - TCCR1B = _BV(CS11) | _BV(WGM12);9 B9 R+ \5 a8 o+ g
- TCCR2A = _BV(WGM21) | _BV(WGM20);
9 m" g, H8 Z/ g+ g! s - TCCR2B = _BV(CS21);
9 C# b3 K# P- ]! O: A& `' l - Serial.begin(9600);* a4 m9 e5 V+ I4 X+ L4 F
- buzzer.setpin(45);* E k9 }/ b! I" ^
- }
! d- m _- h- |! r a$ N" S
) H1 J( ? j8 h$ f% z5 t3 q' S- void loop()
+ S) K8 \$ H) o0 m- f# }2 z$ f: i; h - {9 g9 T }% t8 x: l) T& ?; ]
- while(!((lightsensor_1.read()) < (10)));
9 [. ?7 p: [* e3 X& }0 [9 ?! k - buzzer.tone(1047, 500);5 o0 c+ q; p) v2 w, F) z
- delay(500);
# ?$ e) h* N. ]8 {6 D7 n% z - do
( X- r, j, b: V; @/ C2 x/ Q2 N - {- Y* _, T5 [, T6 W* y7 h: \
- int Err = getErr();. `: F8 I) M& t5 z* D
- if(D1 == 1)3 a9 X' p# {, g8 N$ }7 V, Z, Y& u, T
- {
0 B. d+ K, u6 u; a0 e - Left = 1;
, U. r# k; P7 D - }7 R, w2 S( x m4 p& A1 e
- if(D6 == 1)9 s3 q) {1 j, o6 c, z
- {
$ w! j- S5 T# y - Left = 0;3 [% |' u$ z7 [' U/ ~3 o0 W2 M9 Q
- }
L9 h* C/ w5 { - if(Err == 99)8 a# p# M7 m/ |$ ^3 A# A. Q# q
- {9 ^5 {1 V4 w, h# e" h! ~+ f, T8 i
- if(Left == 1)
2 `0 X5 e( o& b+ _ - {
6 y, S6 W& M" k7 @ - Speed -= 5;9 f# o2 [! h i6 l, V7 D
- moto(0,Speed);
9 y, {$ w. @/ I - do
7 k/ n, o7 P# F6 [/ X) B - {- j7 f$ K' z) Y1 V* z$ [& Z
- Err = getErr();. M- W9 m# L' o" N* F) Z
- }while((D1+D6) == 0);
* C1 o' [6 g, A, ^$ {% g; M - }) U% N1 E5 o" o, I' j. |/ i$ ?
- else" J: b0 {; ~1 E/ {; q9 Z& ?- ^
- {0 u1 C' A2 a, g# ^( L) _
- Speed -= 5;
. c9 B! { w# A- e - moto(Speed,0); 6 l" s7 o; S! e1 r F: d+ m7 U L
- do
4 d* q6 l6 e+ b' Q - {
/ `. D$ E/ a0 @0 ^% j# P - Err = getErr();
1 z$ c# Q( @ S) x/ T+ G5 c - }while((D1+D6) == 0);6 @2 T% K: _4 d
- }
. q# i5 l+ z/ q - }9 s% h0 m$ p8 S! g* j. g$ m
- else& d/ N% C5 |3 w" s9 F$ W
- {( Y! g, W; L' _+ j J. h
- if((Speed < 160) & (Err < 2)) Speed+=1; t$ t: ~* b8 P/ I9 C( T
- if((Speed > 100) & (Err > 2)) Speed-=2;
5 s+ L0 o% O @+ u+ q( s - integral = integral + Err;
) l. ~. l" ]/ R! z% M6 U c z: Z( E - derivative = Err - previous_error;0 w% K3 }7 w$ t) {0 _6 V
- output = Kp*Err + Ki*integral + Kd*derivative;1 |, I0 {, j9 u8 ]0 Z& G- m
- moto(int(Speed-output),int(Speed+output));( N4 h8 u1 ]9 e9 G, _
- previous_error = Err;
- p, g: h( P2 a; K. q8 Z2 d* e - }
& D! c/ [$ y9 y, @" h% h - }while(!((lightsensor_2.read()) < (10)));# c; t" O1 C" M
- moto(0,0);
2 _% F: H& h* z1 Z - delay(500);
* Q6 G8 F/ q) Y5 k& L - buzzer.tone(262, 500);' v) T3 K" R, _: d) `6 m, F
- }
1 v' Y2 n% h* }* E% }( _
% j* J5 S. @; z- g# |4 C5 d- int getErr()* s. H2 S' e8 r& s+ L. X8 v( ]& h9 B# \5 d
- { - {$ k8 C% q F
- sData = linefollower.getValue();/ h9 y5 Z; Z9 }$ y9 \
- D1 = ~(sData>>0)&1;
/ ^- h" b: G8 s3 O$ a( c5 X - D2 = ~(sData>>1)&1;2 X0 ]% V# ]) M) I& t) j2 Z
- D3 = ~(sData>>2)&1;; ~1 u; X' n, B4 `" c$ c5 B
- D4 = ~(sData>>3)&1;7 F1 A, J$ T* Q2 `+ d
- D5 = ~(sData>>4)&1;
# x& L* K" R. C$ V7 |+ h9 l; N - D6 = ~(sData>>5)&1;
: f9 n9 M9 P0 V/ `6 P- x - int downD = D1+D2+D3+D4+D5+D6;/ m, s( W! m0 n1 D
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
B* V2 w5 \6 F) ^2 `, p7 J$ `5 u - if(downD == 0)" M; {7 p9 s4 u, o
- {
% B4 Z/ }( f" g# i7 E, t* ^' { - return 99;& p: x/ S( N2 z( Y$ P8 d
- }
) H4 q" z% i8 _- z, M' w. { - else, q; Q. e0 W7 a8 `; w
- {
5 T \5 D- l( y' p. y6 R - return upD/downD;7 V8 F1 a6 i3 U6 T. u
- }" ?4 x5 i- y/ a- R2 E% @
- }
5 _- a F* r/ v& Z; t - ; X; D6 s( v$ r: O6 r# P
- void moto(int Speed_L,int Speed_R)
3 W# T+ b' ?. O6 w - {2 K$ ?( z! i# J6 z# k
- Encoder_2.setMotorPwm(Speed_L);% T3 D4 f9 i7 z. k5 u, h# C
- Encoder_1.setMotorPwm(-Speed_R);) F/ z" v) T9 E0 p# w
- }
複製代碼 , C! y2 _9 f9 X
MeLineFollowerArray.cpp# T! K* S% e& P# Q: u
- #include "MeLineFollowerArray.h"
9 b' M+ \( \; N# f$ W& X& Z# R
) P5 Y) p1 O {1 {- #ifdef ME_PORT_DEFINED$ k0 k$ h7 }- G* e! I, C
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
" u: l; M$ k9 N. y' _/ g+ \ - {, z( N- g5 S/ p k# Q+ W
9 M, F/ i! b5 s8 m& k- }
; I* w- V' V& f/ p - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
8 ?; U& h: R6 O; ]/ z6 p0 u - {$ f7 b3 x: }) B; z+ [* I
- _DataPin = mePort[port].s2;
8 q# K/ e7 a7 Z# g - pinMode(_DataPin, OUTPUT); / Z1 S3 y# D) T3 u& ?& e, a
- digitalWrite(_DataPin, HIGH);
9 ~4 O# D+ U! E( K# L - }
9 }, z+ R! J( s& p& s - #else // ME_PORT_DEFINED
4 _7 g) C$ @+ y- `. O) g$ y - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
7 _9 f5 b7 X( c' i - {1 {& S2 E! S' d* k0 `4 o
- _DataPin = pin;* W' K, [7 I6 ]. j: o2 p
- pinMode(_DataPin, OUTPUT); $ ~' N0 B5 M# \% P u
- digitalWrite(_DataPin, HIGH);
; P* K2 z% A" l, h" p+ r - }7 ?: n) E( h, Z+ P4 ]5 u3 p
- #endif // ME_PORT_DEFINED- O# V/ N/ n* X, m6 j
& m, R& J# |, D* F/ A) O: R- 0 T0 s, u1 y9 c1 n# N6 b1 d# A Y
- void MeLineFollowerArray::setpin(uint8_t pin)
7 T5 a X @: n, t& w - {) y# Y# U' S* l& a* v9 I
- _DataPin = pin;
& v# q1 F, h ?1 v - pinMode(_DataPin, OUTPUT); . V' N% t O: Q1 g& F, l f7 g
- digitalWrite(_DataPin, HIGH);/ m+ V" r0 z# _* k* F! u. H
- % o! d- p" f, s; |
- #ifdef ME_PORT_DEFINED3 B( r) J( F6 c1 R- `3 H
- s2 = pin;; a9 u& z, h! a
- #endif
4 W( R' s0 I5 J$ {( v+ M - }) J, F4 ~# o K+ y; [# M
! p: x/ S" ~- n+ K4 \, t4 f+ m+ i- uint8_t MeLineFollowerArray::getValue()
. _; t3 F2 j, I0 D - {; T+ _& J" q% `+ q" i7 m
- uint32_t LOW_level_read_time;9 T$ l9 @! T/ r
- uint32_t HIGH_level_read_time;- @& D! B; D# U
- uint32_t time_out_flag;3 p5 O( S+ a3 j2 F& j0 v
- uint8_t Sensor_Data[3];
8 t; i9 C. I" ~. B2 A A - static uint8_t old_data = 0xff;1 ?/ e- k' [2 O" _2 q! N
. @( O% z+ j2 G4 p* {( s2 P- pinMode(_DataPin, OUTPUT);
9 y& m, O9 m$ w2 p. s' T - digitalWrite(_DataPin, LOW);
# v0 H( Q U0 u9 {6 W( t - delayMicroseconds(980);
7 ^8 _' f4 A" u( W# e, U - digitalWrite(_DataPin, HIGH);6 _1 K2 Z/ } q
- 4 R5 Q9 P1 m* g( {, a' s8 L/ S
- pinMode(_DataPin, INPUT_PULLUP);- q2 G! Y3 @& r+ i9 S0 b
- delayMicroseconds(10);
H' w5 s8 n/ |6 D$ Z* ?8 q/ _' j - , A1 z9 [, l+ l; H9 q' n
- time_out_flag = millis();' V9 A) x; v6 f
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: R+ b0 k' F- N% G
3 n! l5 U6 @: b, ]' F) t( l- LOW_level_read_time = micros();
+ \+ K# K8 O6 M0 J9 } - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
% _$ @6 ]; q* ^ - {4 p/ \% f1 h8 m5 ?4 [
- return 0xff;4 }) R# s$ K% j, R v
- }
" q% K1 T4 i/ n: Q$ B( U- C
! H. Q3 q1 m9 [- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 }: r& B8 X8 j. x" p4 _& ^ - , V5 _( _5 X) ]/ b) K+ M6 `
- HIGH_level_read_time = micros();
3 D. r3 ~9 R: k7 V! c2 V' d - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level, B+ T4 B$ Z* F% q4 ^) W8 e
- ( s! [. ?4 W6 N3 g& O+ Y T9 I$ C7 f1 _
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out# U/ D U6 `" {* m
- {
( p9 V- G' y6 C. b, |2 W/ r1 d% P - return 0xff;
' f; _# ]9 y5 _ A n+ | q) X w - }
# O8 `9 C$ f8 @ - ( g' @4 R$ v! ]( c
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
9 \2 O7 P1 w, t- n6 A - {
. h {- k& q* H8 ]+ G - return 0xff;
6 ?% ?9 `9 O0 t& z6 N - }1 A. P! T- S4 u3 m7 s2 {3 N. q$ S+ J, y5 k
- 9 ~. C R8 Q. M b! n) P1 [6 X
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );& I8 F% [) X" }) R6 `
- LOW_level_read_time = micros();; `2 T, s6 S$ Y: r7 G
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
% E& _$ l9 s8 [7 G: _+ o5 ]
0 R7 E# @0 @: @! R% g3 ]- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 f/ s7 b9 V0 j+ K! F2 t - {* c7 `( S8 m9 W. S, ?: J% e& R* y
- return 0xff;2 w. w# G' S, c$ X3 D, `/ ]1 }
- }
% q- V( n; W& h$ U6 i5 N, L2 a
2 P- c# ~0 n- ^9 e1 O, p- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
- m% ^1 q" E" P- c8 }) D - {
! y4 ^/ }( B# H; S- h# _8 u - return 0xff;" T6 \# l) K j; b r8 P
- }+ i9 Y3 P1 P2 X2 i* J+ K6 k c2 R
- " N, N: k8 F# N* f# c2 f' c: v+ g
- for(uint8_t k=0; k<3; k++)
& I: o3 D; b0 u) b- j0 C - {$ o5 K0 {7 s4 k* m' Y' R( X
- Sensor_Data[k] = 0x00;
' m0 \& z' O6 O! W- _& U9 y# B! G
, c' }% O' w* N5 m2 j- for(uint8_t i=0;i<8;i++)
. R! S) }8 W) P9 O- |5 G4 n - {
1 I, `# K% E1 U! B - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
+ H, J: x, ^+ `4 ^' `4 ?: Z$ @; N4 T, P - HIGH_level_read_time = micros();
0 G/ v5 W# l* Y* B - LOW_level_read_time = micros() - LOW_level_read_time;( @9 g$ }0 t( g1 K
- " C2 t7 f. g! n$ G! f
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
1 W8 q! C1 T2 ~ - {* I5 V: L3 G8 o- Z! U( e
- return 0xff;. N- g7 o2 u6 W% ?
- }
% u" C/ s) z8 ]2 c - + V- G8 s, f" j8 e
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
& S3 J; I J/ k) g* i4 c - LOW_level_read_time = micros();
: Q' \5 Q9 g: E0 w. o - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
! m' h( i3 D* V1 J
7 b1 R t; f! D, @- U$ @- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
: z" F9 ?' a( s2 l - {
0 S5 P; @6 n2 N9 P - Sensor_Data[k] |= (0x80 >> i);+ \1 M4 i- {( C' c9 i1 T
- }7 N7 x$ o# `, }3 z) o! @& b
- else if(HIGH_level_read_time >= 100)+ |7 y* | I2 ?, S( L. |
- {
* P) y# W- V. l3 _2 F# j - return 0xff;% K7 ~! w/ i1 s* L* v/ u
- }
/ y4 _+ E+ u0 B' }( k2 q) x - + ^' }. J; Q+ a0 x7 z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)6 h* M; K2 N! V E5 O: s8 d
- {/ o' v& f" _2 K @
- return 0xff;' D- |# n( c5 t: G) l
- }
' G: S! x$ _6 l1 ~1 r& D4 N! W - }
: Z) V# t' b* K/ ~' ~0 L$ Q7 A - }
) z! V! l& H$ p/ r* `
0 N5 h- d" M A. G- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level$ Y3 ]+ @3 A$ W5 F5 a
- HIGH_level_read_time = micros();4 m( Q5 ^# f* Q5 ~( g) }2 X# h
- LOW_level_read_time = micros() - LOW_level_read_time;; `. A7 N) r; v4 h
- % t8 q! S1 [0 @% _' q) Y* H
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )4 `+ [. Z# Y9 }0 b
- {& v9 q+ a% y( W& i2 o9 B! ]
- return 0xff;
! L, ~7 p/ y8 w R$ [1 n5 r - }
' {/ G# f: J* Y9 P" l) ?! L( H5 |0 f - , N9 y9 P1 n* f, a7 {6 V
- pinMode(_DataPin, OUTPUT);
8 ^3 F& K, j3 R) Z - digitalWrite(_DataPin, HIGH);
?. ^* l& Z1 ^- M1 Q0 I6 l& w E
1 J) J( R; N& S/ P' p0 _5 x- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))+ z* w* A* V; P! w1 B
- {4 F7 R% O# H9 |( g% R' e
- old_data = Sensor_Data[0];
( S; G4 I# a0 H1 J- \2 C% B - return Sensor_Data[0];
: c) ^5 `! e3 K4 W2 J% I2 P9 Q y7 u, G - }
! e! f& d( X9 {8 U - else
, U9 a9 G+ ^# n5 P1 \( f: m- d - {
1 s# ?' o. { n2 ?7 d, g) h9 }$ q - return old_data;
2 C( d4 ?6 F1 @/ o3 ~% n$ g - }
/ W& C5 s: f1 E - }
: a. h2 k4 t! ^( X5 b5 u
複製代碼
M" B/ {. T0 ]3 `3 } [MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
3 G# z6 E( ~5 x7 ?* [; W$ C+ o, L( V
* c% x: m( \( n0 D6 M |
|