|
|
2 j/ [2 D0 I2 A6 |, a2 X- #include <Arduino.h>8 x1 o. I& A# y, s0 t
- #include <MeAuriga.h>
( ~' p" y: e3 _$ i7 V3 Q# D - #include "MeLineFollowerArray.h"
. f o" y9 S5 d/ T' a8 G
. w: X; c$ h# b* H& @- MeEncoderOnBoard Encoder_1(SLOT1); v3 j& K7 n' i2 i9 k( P! x3 i
- MeEncoderOnBoard Encoder_2(SLOT2);: U0 m/ H# \. e' M
- MeLightSensor lightsensor_1(12);2 [0 R1 `; p3 y% z3 b. N
- MeLightSensor lightsensor_2(11);
8 G3 S4 b. ]0 v' f* q% M - MeBuzzer buzzer;
- q& O' Z3 F! m+ d- `- ~: N3 G - MeLineFollowerArray linefollower(PORT_6);, g9 o+ r. U$ A9 K. w9 j, V( O% J
, R' A6 n* k- \" S% L- #define Error1 1+ ]6 E) C5 a& g2 c: B& U9 g- _5 c
- #define Error2 2
3 J9 J% y6 f6 a1 O - #define Error3 3
$ j f) D5 n* m7 t - 7 A" Y5 i- a% S3 `, I9 M4 `
- #define Kp 15. E+ K0 `0 B8 Y2 Y
- #define Ki 0.15. d0 {( D3 d" l) K- v8 ]+ C% \
- #define Kd 0.038 q! f% c9 N& Q" W5 ?8 G6 k
- 1 ^" B2 m q1 F
- uint8_t sData;
0 ?7 o0 X5 ^6 I# }2 y* S - uint8_t D1;
9 U M2 E3 K2 g( X4 g - uint8_t D2;
( N0 |% Y0 v' X3 A1 K7 Q5 B: } - uint8_t D3;4 W: d4 o0 G3 J: E, v8 g
- uint8_t D4;7 i' X% w4 Q% |* u1 S* x
- uint8_t D5;
' a: O1 X" s' }2 R ~( | - uint8_t D6;
2 x7 F5 D. m2 ?( q. U& ^% d1 ~4 R7 U
& |: W+ x7 d$ ^2 ^- float previous_error = 0;. V; x v0 L2 R& w1 f+ ]; ]# ?
- float integral = 0;
% X" z6 z3 r# t7 P* Y& I' @/ i5 R) w - float derivative = 0;) I, i6 E, I. F
- int Speed = 160;( M+ M5 n5 a! n
- float output;/ d" P+ ?1 }7 ]/ {" J
% L! |7 T# B! X- byte Left;0 G3 o, b2 Z- v$ ]! X+ j/ u
2 @2 v( X n, N& b6 k- void setup() + n8 g, W$ R& x2 s. a& y; r
- {' {, [. M6 q* B: Q% b$ c1 z2 W3 Q
- //Set PWM 8KHz' \/ ~+ R; |3 U. z6 w: E
- TCCR1A = _BV(WGM10);$ H4 r( b; o0 X' `$ B7 S
- TCCR1B = _BV(CS11) | _BV(WGM12);
{) l2 S- X$ D9 [! M - TCCR2A = _BV(WGM21) | _BV(WGM20);( [, f% ]1 M4 j7 v! c: s2 c
- TCCR2B = _BV(CS21);) U1 G) C8 C& W' S, I" u
- Serial.begin(9600);
3 [' p% Z5 k( y% z6 l W - buzzer.setpin(45);$ t, W, s" B* F8 K& i" t8 k% _
- }! Y* E6 M9 D8 k" B/ l `. y+ f; [
- % S" E- b6 v+ m& |. R& Q! i8 L+ z
- void loop()( O+ ~7 y. b8 i% \0 r
- {
+ [: D( M2 Z1 A! U - while(!((lightsensor_1.read()) < (10)));/ b) q) |+ ^3 f! W# L7 ?& w
- buzzer.tone(1047, 500);+ g% @1 m6 m @7 |! Z. p
- delay(500);
+ C Y7 `# p; s$ [ - do" L' @8 ] A; x1 O
- {
5 J6 U4 m. o, B4 s1 k1 s - int Err = getErr();
# k8 m4 n" m8 T* G - if(D1 == 1)
m; r% A- @4 Y2 `8 Q V9 b - {3 { f+ {7 ^% h& ^
- Left = 1;1 q% k! d; s- ?
- }7 ~1 s6 M( z2 S' X: Q2 Z- G1 J
- if(D6 == 1)
3 }: Y$ C5 e8 E# i - {
: p, H5 a% J5 A' p9 U+ U - Left = 0;* `) Y, B- s6 d" Q' C
- }
6 a3 A2 {4 Z0 Z7 |0 l) J - if(Err == 99)/ b! j' x- M$ Q* s r
- {
, N, r. w: b8 g N& L6 e9 v - if(Left == 1); ^+ ?6 M& g' s# k8 h( q
- {+ y" f+ ^2 a `& W1 m
- Speed -= 5;
- R' A0 L' y, n3 d - moto(0,Speed);" d- G! ?3 y2 W. z! f: H2 _ p
- do6 d8 O" o1 Y( \( H
- {
; K1 z1 b$ C3 r% q* ^$ b) f4 } - Err = getErr();
" Y+ f7 |; f9 U2 @( z! V - }while((D1+D6) == 0);1 q' e0 K1 h+ v5 a4 D7 K' O
- }. ? h Z/ O7 a/ H# _( s
- else9 w- R2 k8 _3 q3 M% @6 W
- {9 {) Q( l; E( s7 {9 o
- Speed -= 5;1 F2 R, u3 ^- a$ ]6 @. `
- moto(Speed,0);
- z4 Q: f# J! m2 B7 C - do9 w2 a4 P# d4 {3 B: q% Q9 g
- {
1 D, L5 u, k. Q) k! s - Err = getErr();9 a7 J3 l* b% j8 X% M& h/ d" B
- }while((D1+D6) == 0);& x0 D* Z+ e k P3 N0 S9 q" F9 L
- }
) w/ U2 M- g9 C- ~, M; @. \# v - }
- L$ p- u G$ [" X6 V _ - else
: r8 c! H5 i8 w; e% d6 Z) f - {
8 W, z+ c& Y" G' p - if((Speed < 160) & (Err < 2)) Speed+=1;
( R) _1 a8 E& S* S+ \+ _ - if((Speed > 100) & (Err > 2)) Speed-=2;
/ i+ M1 e S% x) J9 B' [% i - integral = integral + Err;- N8 ^ H1 f, I$ X
- derivative = Err - previous_error;, t) a' z- J5 C2 e: v: Q% P
- output = Kp*Err + Ki*integral + Kd*derivative;
0 V9 E! Q* Q( a' f - moto(int(Speed-output),int(Speed+output));' {4 D) {, Q1 R: x1 ^: g
- previous_error = Err;- {; H# V( k- U G i
- }
; o1 {* B& X1 T% [4 C. ~' `2 I - }while(!((lightsensor_2.read()) < (10)));8 X3 k* C. V" h" D8 Z
- moto(0,0);
& X1 b7 l: u4 m& W" t - delay(500);
# \8 f- a+ y- W5 x! O- E - buzzer.tone(262, 500);
8 `9 I- {+ l% g# ?) q' t6 P - }
* f& J* y7 L1 S+ I& H" V$ ?
/ [+ l* J7 x/ v; X) A- y# ?1 o% {2 U- int getErr(). i0 g- }% |2 R3 E7 Z) O+ O! w# z
- { 8 }3 |/ r% u- J0 G" E
- sData = linefollower.getValue();
. t$ b( Z( q% X( g3 l8 t! q3 O - D1 = ~(sData>>0)&1;1 t& L+ M8 k5 |- i, M2 i1 U% n0 z
- D2 = ~(sData>>1)&1;) x* R( D+ p" N' l
- D3 = ~(sData>>2)&1;
- z- ?( J. j5 E6 n: i3 n - D4 = ~(sData>>3)&1;
i9 F0 i' S- J& ?4 i - D5 = ~(sData>>4)&1;* h* I1 e' I* x! N
- D6 = ~(sData>>5)&1;2 q9 q J' C5 y. Z5 q
- int downD = D1+D2+D3+D4+D5+D6;
1 C( \+ _5 z5 k* E - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);5 z( M. y- k# }/ G# o
- if(downD == 0)% l9 X: u7 d+ t v) D
- {
) b, v$ m9 C2 R - return 99;$ ?. Y0 b; |! Z. } @$ a
- }0 [6 Q1 ?" a+ r
- else
; l7 q/ x& X" m3 z7 j - {
" R2 c/ h$ A+ W% L! j5 m+ s - return upD/downD;
, [5 B6 q2 V' {* s6 I8 Q - }' [- g- H+ Q/ k6 ~& i3 B
- }) X# u: i$ L! ^: O+ l( p
4 F- t8 S% R( }- void moto(int Speed_L,int Speed_R)
3 F5 q/ b% B7 w1 T - {4 c% U- r- u& D) O/ P- l! z
- Encoder_2.setMotorPwm(Speed_L);1 X2 N6 K% g' D2 N4 n: I0 O( u8 m
- Encoder_1.setMotorPwm(-Speed_R);6 J- j1 t- j4 o
- }
複製代碼
. Y+ ]) }5 H0 ]* k) VMeLineFollowerArray.cpp/ K! `7 \, e+ P
- #include "MeLineFollowerArray.h"8 C8 [4 k5 f2 m& ?$ A
- 0 ?* T, W/ ?+ l( R) Q( O$ S
- #ifdef ME_PORT_DEFINED$ Z0 k/ H% }* Z+ m. @3 \* C" E
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)9 q; s) \& g, T6 u( Q
- {
9 M E; g2 _' |# k
5 |5 }0 a' D' T: H% {- }
' \: [* e. X% s - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
, C9 ?4 C7 J' F9 Q+ k - {
1 f) R% _8 G$ i# y1 t - _DataPin = mePort[port].s2;
- ?4 @2 R% _$ g - pinMode(_DataPin, OUTPUT);
- o* v+ \/ o2 L4 J2 s/ u - digitalWrite(_DataPin, HIGH);& M8 f0 i" n, o/ y% v4 S$ K: c; z
- }
/ k) n1 J0 K3 p9 w+ \ - #else // ME_PORT_DEFINED" p E n H; F3 Q; l, `
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin); r8 i5 ^0 U( Z0 w' T* g
- {
% m& |8 L4 z" s9 S; s8 f' r+ x - _DataPin = pin;2 \# A6 u g8 v& ^
- pinMode(_DataPin, OUTPUT); 1 R& Q% G! F- q( K* h
- digitalWrite(_DataPin, HIGH);! z- p+ [& Y* X+ o9 F5 ]6 X: T
- }
" f* [+ v' q) d+ G1 m, w) V1 O - #endif // ME_PORT_DEFINED
9 w5 j- S: Z: l5 U
# j* i% @3 b* y- e* J6 b, b
( K# d9 s" I X d- void MeLineFollowerArray::setpin(uint8_t pin)
" c% m; u% A1 M( s( W8 ` - {
( t% K3 y0 J/ `) `" j - _DataPin = pin;
$ y! w6 k c" U0 l$ m, Q ~ - pinMode(_DataPin, OUTPUT);
) W) N0 g q( v3 Y - digitalWrite(_DataPin, HIGH);
+ E4 V1 v( `9 D. o; k1 p - * v( ^7 ]! o+ C$ q& Z0 _, |
- #ifdef ME_PORT_DEFINED
6 ~( ^# E: a2 e. ]: b* F9 Q: a - s2 = pin;
I. A6 N% W& v1 U! p+ x# _ K U) E - #endif& L) a* g; `$ h
- }
/ K- u* f0 W. F - 9 d" T% g+ U$ r8 [ R/ ~
- uint8_t MeLineFollowerArray::getValue(); \8 J( r1 n( Y2 H( x% H
- {8 x, [8 b7 I8 J$ s
- uint32_t LOW_level_read_time;
. E% s0 T4 q& b% L3 I3 M - uint32_t HIGH_level_read_time; g$ l2 n9 j* Z" N
- uint32_t time_out_flag;' S" ~) k: G6 n/ P! r
- uint8_t Sensor_Data[3];
7 R. u: ^. a: { U$ U2 ^6 A& m - static uint8_t old_data = 0xff;! `/ h# ]6 }3 p& E. l& `
2 e8 o: v1 g3 l* e% D* c- pinMode(_DataPin, OUTPUT);
0 s8 G. D2 J+ U, l3 M2 t - digitalWrite(_DataPin, LOW);3 Z) u: j) j( {7 Y$ Z
- delayMicroseconds(980);' T, x& ~+ k! |7 I w' N; `
- digitalWrite(_DataPin, HIGH);& {" Z/ }( n' `- h* T+ k9 H) X
' H: C4 o, O& e5 a" z- pinMode(_DataPin, INPUT_PULLUP);
+ i; [# {* A' [7 Y( B - delayMicroseconds(10);
# F4 ?' X; h# W9 Y' L
6 w' D. ?$ }' {- time_out_flag = millis();6 B/ M# [% `9 x; M/ Y
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 {4 ~% y3 P1 L |$ d n
, w& w- h/ h! y$ b$ ]- LOW_level_read_time = micros();" q- h$ U9 F: W$ T6 u& p
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
5 F8 y: s% [" ?, a a - {6 O4 ^3 O- Q0 J, M4 N
- return 0xff;
5 Z C1 x2 I+ S( F% z - }& q# O \1 b0 a
- ; O1 W! s r1 G% ^
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); y3 w5 c- h7 O5 h; _
- 9 D" ?/ {8 `% ~: Y# |
- HIGH_level_read_time = micros();
! y! |% h* `; Z- i) G& V - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level0 l0 Q9 }3 Y& e. d9 ?5 |$ y
- & t1 Y- f8 b) Z" P
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
" [9 t% i0 F+ G' y - {( k1 X8 g$ l) B
- return 0xff;
1 m( A3 w8 H8 P - }
3 @; j" `( h: S. p& D; u - 0 w8 o: `/ X( w3 i; I& ^# R3 ?
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
9 N5 x3 N+ `2 Q3 [ - {
9 ?2 A5 s ?) s* Q - return 0xff;
# A; f8 r6 i. i9 }$ y4 N8 Z, ~- N - }
# \0 V0 U& m2 f( k - / [/ }& m* @6 z1 }6 M
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
8 Q$ Y5 f/ {" W' t9 D5 R - LOW_level_read_time = micros();
+ }4 r4 I. ?: C$ B# T0 V/ J% U - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
# G& E& E$ A. A! I1 t
1 W4 J& i6 O9 i3 J$ [( P- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 z3 _3 h$ J3 @- O) G E' r' O - { U; H" p* d. S% Q" }( {
- return 0xff;
( J9 g6 A2 g+ a" p8 O - }0 o9 _8 Z' k: Q/ H `2 ?8 C
- 4 g# j* L% a, k2 j
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
# _2 e3 @, F# o5 R - {
) Y2 ?, n2 o0 V5 j# l - return 0xff;
. C5 S! `* D4 H5 u0 Y; I - }
+ |2 R3 A M! D
$ z1 n8 D* x' A+ m! H2 h* W- for(uint8_t k=0; k<3; k++)
2 ~) q' f0 G! M8 ] - {( S5 q) ?+ s& }6 g. V! m4 Y1 V
- Sensor_Data[k] = 0x00;! S1 S4 N- w. L! V& V
- + @/ N7 x! s1 [6 e8 y( A o4 e
- for(uint8_t i=0;i<8;i++) D2 U, M1 Y# I& z8 V8 [4 j- ^
- {& [8 q$ e4 A* l6 F' P
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level" @+ T1 e( x) s
- HIGH_level_read_time = micros();# ]; q6 U. P+ B( l+ R
- LOW_level_read_time = micros() - LOW_level_read_time;6 s% k0 p4 x: s1 {; z4 e7 C* Y% e
" Y$ A8 F# F) ?- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ) D$ a# U7 o9 \# c- E! z+ e* P
- {8 n: {1 ~; j5 L0 ]/ {
- return 0xff; R4 m' U3 U0 \& O# }
- }
! d9 D( M* b) N# n9 M1 {8 ^$ d. P$ Y# \
$ S6 c5 x3 i3 I* ]2 a7 k$ e- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ ]7 P4 W% \; t - LOW_level_read_time = micros();
: C8 b; w9 B* n" u9 q) o; G7 R - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level# d% h5 G* U' X( J
]$ F7 V1 Y7 l# z( s- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 19 V" q1 e* N) w; l( ]
- {
7 z, R2 _, I9 p: m6 Z - Sensor_Data[k] |= (0x80 >> i);
0 D' W. a$ O y, W - }
0 e; i$ ?* J# B3 w4 c - else if(HIGH_level_read_time >= 100)7 |+ Z1 M8 X t; J
- {
9 p+ t- K0 O% M: |& b$ c- [ - return 0xff;
# n; f4 _# ~- v9 J0 H. k2 B% B9 t! E - }9 j! k* e' y% f4 B
7 Z+ X2 o! V) K$ y# J2 M* V5 @9 F- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)! q; i& G1 o: \+ y& a$ j
- {9 C, C' b/ k, w
- return 0xff;
* Q/ |0 L% J! H2 N' A' A" Y6 Y - }
) `) G6 q5 _+ a6 P9 w - }
, Q, a- ?$ @1 s' `. G! T6 Y - }
% K$ B& s9 f/ l7 h3 I# u. ]2 H
( J+ R' \& M7 X$ V. d- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level" v* }6 W3 x$ m7 j5 q+ M- j
- HIGH_level_read_time = micros();
7 ?2 R: f$ |* X& q1 e& o - LOW_level_read_time = micros() - LOW_level_read_time;
8 z) ?7 K3 ^& E: b" |
' K7 ?; i$ ^ v2 m) R9 d- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )5 q9 Q$ D" e. _9 s5 g: R
- {
0 @) ] L$ T+ A% p - return 0xff;
: K/ z! _: l2 L/ N; Q+ O - }& g2 k) D" F/ \ n# H0 U
- ( t' p# l8 t+ P" L* m1 s3 L$ U
- pinMode(_DataPin, OUTPUT);
; x0 ~7 h8 u0 e - digitalWrite(_DataPin, HIGH);
5 a! O- u+ i/ l2 |# }( z
( X v) p; @, t }- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))1 n% Q4 u9 P, a) H' J6 l5 e
- {
+ Q3 j) V# m' P! O - old_data = Sensor_Data[0];
# S/ u, w. N, H$ p - return Sensor_Data[0];
& ~2 H7 d2 S4 c2 D+ _+ p, l; {' j - }+ @2 U1 f3 I6 `3 ]: [. z2 Q$ l
- else* I* l/ E; p9 j4 N
- {
6 i( U# V; `! x" w& a+ t - return old_data;
0 k R' T. o3 V$ D; C- L% O - }
0 ^/ C! [1 ~* g v- D! }5 R - }
! N* e; k5 N& Y2 j
複製代碼
7 H& E0 l# j1 k/ g% dMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
. P6 W( X8 ?, V' y
4 J$ {! X. \; R8 f% o8 \( b |
|