|
|
6 D* N& r; y6 o3 t- #include <Arduino.h>$ L W* z. j$ i
- #include <MeAuriga.h>% E; K7 Y8 r* J, U1 Y/ R/ _' ^
- #include "MeLineFollowerArray.h"' x/ x1 e# C4 n2 z0 n: v. W
# o% t% j; q5 T& r- MeEncoderOnBoard Encoder_1(SLOT1);, U# C7 G* Y9 Q1 a' T6 B2 q
- MeEncoderOnBoard Encoder_2(SLOT2);
& U+ d! l% y g9 h; \ - MeLightSensor lightsensor_1(12);6 D; |! `6 t4 K$ j9 r$ `
- MeLightSensor lightsensor_2(11);
) M5 [( n/ U- s1 ]( {+ u* [+ G8 v - MeBuzzer buzzer;, Q2 t( p% g$ F* G) h a
- MeLineFollowerArray linefollower(PORT_6);2 m# a3 d8 P3 Y$ A3 I I
- 2 t, z4 O4 [. ]7 Q e! I
- #define Error1 11 o- k- z% \1 R: ?' @ T/ l4 p
- #define Error2 2
$ l) e! m q* P4 U5 I7 b - #define Error3 3
) M9 e0 N% x$ | - & K: Z, h9 f! e/ c. ^" V
- #define Kp 15
# V( q2 I+ \# E0 D& ]9 J. I5 R V - #define Ki 0.15
* ` @1 Z: O$ r, t0 Q$ _ s& ~ - #define Kd 0.03
7 u. r' s, e0 H8 u6 q2 r+ L) J( l - 7 u# U- }) l) H
- uint8_t sData;
0 M; i7 Y) W7 ~3 _* E - uint8_t D1;
5 v: V) \9 q8 N2 S( D6 j" X5 j1 \# m - uint8_t D2;! K, F% y- _: R4 r+ w
- uint8_t D3;; C2 d! ?+ Z! ?: }- m( V9 y
- uint8_t D4;* Q5 W( S% B, F1 t7 I+ H8 O* L
- uint8_t D5;$ ]: h" c8 f; Q2 d& S/ \
- uint8_t D6;
' J, O6 ~2 ~0 E" }( Y
; z: j5 P. t! C9 w- float previous_error = 0;5 Z/ ~ G: a/ J4 a8 j( X: L- `& `2 w
- float integral = 0;& F- N& d7 L3 a) M# g6 _* f* {
- float derivative = 0;. r1 e8 @/ [- l3 v
- int Speed = 160;( k) |( a r/ P* t
- float output;1 E9 g; a. l; F8 t! L' r
- 5 J& o' _ ]6 c
- byte Left;) F2 G1 m5 J$ K9 z/ K0 }0 h3 s
- 4 {. J( Z' Y( `# U
- void setup() - [# B9 E0 O2 x0 e1 |8 f2 v
- {) u5 t$ n. _) b% R% q
- //Set PWM 8KHz; H% x3 S- C" |+ x, \
- TCCR1A = _BV(WGM10);. @9 s2 q2 J( r) F. O
- TCCR1B = _BV(CS11) | _BV(WGM12);
* J R; @7 N4 U0 F: w+ X' y, V - TCCR2A = _BV(WGM21) | _BV(WGM20);
3 }. s6 {1 G" ^% b. l8 l - TCCR2B = _BV(CS21);2 S6 b, F3 U) T% I7 d0 I& {; s/ |5 u
- Serial.begin(9600);0 I n7 o7 L1 {/ e& F( b5 H
- buzzer.setpin(45);
q% _2 h# Q# j - }, {, N6 z- y* j
! {1 k: M5 j, ^8 b1 O' y- void loop()! J) F; J5 Y1 n N0 e. k/ G
- {
8 _! J; l5 P( M - while(!((lightsensor_1.read()) < (10)));& M/ o8 _3 {; A( v
- buzzer.tone(1047, 500);
( Y$ Q( _ R4 M7 `" B' N2 m* | - delay(500);
! `* I) b. s; _7 s - do7 U; H# u3 w# j$ V' j9 G
- {) B5 K5 w) H% d
- int Err = getErr();7 M3 A5 j0 G& O* X3 \
- if(D1 == 1)7 [! i: `- j3 J& S z
- {3 z+ e2 F1 _( n% X5 N; c
- Left = 1;: w; x1 Z; a1 L ]! r
- }
5 x- G9 z! F0 {6 b5 ? - if(D6 == 1) `+ e5 M. B; d" H4 }9 J7 J/ A0 [
- {
2 Y# [4 T# }5 o+ t- t9 a - Left = 0;# K g3 a5 m6 I( r/ Y, n
- }, Z! I' m) {% T8 t3 A
- if(Err == 99); F. J9 f( Y4 x
- {
4 Z! B. Y$ M5 N8 K6 z# K0 e - if(Left == 1)
9 q/ ^% `4 m* @ - {/ B3 W" ^; l7 M4 b: Q
- Speed -= 5;
I0 @* W" e3 c- N8 l4 R - moto(0,Speed);! R ^; k3 w N# }
- do
! L" H6 K3 @8 D& Y+ G& t" V - {! g1 \- a, G) a- R
- Err = getErr();
8 ~: ?4 |* \/ j% g3 ? - }while((D1+D6) == 0);
9 H0 B( q1 J: G9 g, I6 O6 z - }( N( F( ?$ G' l0 \+ c2 e
- else
: r3 f( E4 z( T6 _/ } - {9 p8 h! u# }. r% T9 n, o( c$ H
- Speed -= 5;" v" T6 \# B6 @; ]9 z% g
- moto(Speed,0); 7 g/ V& A% Z% H' _' X6 J7 L* N
- do! H3 T* u& L5 C! Z+ I. X( W+ U( Y
- {
3 q4 T% m) M6 p9 j# { - Err = getErr();; X6 z7 j8 O9 x# W+ y& P
- }while((D1+D6) == 0);2 O! U6 U- r* _# P: V
- }
8 g4 m8 m( E* T+ J4 j/ u - }$ s' @& c; [8 F [7 y* D
- else% D7 w. z% ~! n( N) i2 c6 S
- {4 t+ T3 _4 y: I" d
- if((Speed < 160) & (Err < 2)) Speed+=1;
5 a# y) p4 y' Z$ a, B3 ~ ?, O - if((Speed > 100) & (Err > 2)) Speed-=2;
1 h: T9 l, }+ Z/ V - integral = integral + Err;; e, a1 S" h2 f) N& O
- derivative = Err - previous_error;
; G+ ?* p+ B! B0 x1 K2 e - output = Kp*Err + Ki*integral + Kd*derivative;! `% M- B0 w4 X5 Z# k
- moto(int(Speed-output),int(Speed+output));
! S& C( |- _. W4 W% S' P* G# n8 m - previous_error = Err;0 J( L. A& \, ^. i: D) ]9 v) [
- }4 A9 p. u$ W; r# h; B0 O
- }while(!((lightsensor_2.read()) < (10)));
1 ^7 A9 r9 ?) U. Z( R) L - moto(0,0);4 G: t7 {6 a! Q& b# E" Z
- delay(500);
9 |5 R# `; Q* w - buzzer.tone(262, 500);
0 M9 ^9 w, q$ x& ?2 W3 v, p; j7 x - }
3 x @1 S8 Y$ A2 Y
" a$ e0 d. z; a6 k9 c- int getErr()3 l4 x1 l2 L5 j! F }& J0 x- m
- { % j4 W* U9 b' S
- sData = linefollower.getValue();0 \! V2 g7 K! i+ u
- D1 = ~(sData>>0)&1;9 b1 K! ] \9 J s/ L
- D2 = ~(sData>>1)&1;. E! j9 }4 ?: c1 w, n
- D3 = ~(sData>>2)&1;$ H2 Q0 C n* U4 G0 n- D! R
- D4 = ~(sData>>3)&1;
8 c. H% n/ S; Y( k% ?8 N - D5 = ~(sData>>4)&1;
, T5 P% R3 \# A3 R* K6 R - D6 = ~(sData>>5)&1;% K9 R' a- C2 p0 \4 Y9 P( R
- int downD = D1+D2+D3+D4+D5+D6;
' O+ ^) x: I: y" e8 E/ O* l - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);2 F0 z0 {; t6 t6 D0 {
- if(downD == 0)
; e; } i9 }$ Q8 }/ L v4 V/ u - {
6 n6 l/ f. E3 b' N8 i - return 99;
9 e$ g9 ~9 L, \) r - }
; r) C H8 z* m; m& I: P7 {7 v* y - else
2 P! D& J# h4 w0 K# v) |9 k ?. [ - {+ o) M* k+ E6 }2 t: w8 z( \ [& `
- return upD/downD;
1 c2 P' }0 J4 L9 E% @2 \4 ]" M5 X - }
5 b" o Y; G* G7 [ - }
, t2 X" O2 A4 L- x2 F) w* J! a
9 C. D$ J, P w" c4 I d7 ^- void moto(int Speed_L,int Speed_R)1 w' m' h1 {, V7 y2 I2 Z# j5 H2 R1 M
- {* j+ {) F* z3 d" ?: L) _/ P2 ^
- Encoder_2.setMotorPwm(Speed_L);9 u, E4 V! {7 U' u; |9 _8 G
- Encoder_1.setMotorPwm(-Speed_R);( \( x c8 g: S V$ u
- }
複製代碼
1 R8 |8 {! Q* ]5 q( S6 CMeLineFollowerArray.cpp( u# l3 G: \, H$ i, o u! u
- #include "MeLineFollowerArray.h". \( L/ B. ?4 N7 R# { u0 `
- ' P! B/ U+ {7 n# G" U0 L
- #ifdef ME_PORT_DEFINED2 P( K" Y& n( |6 ?% S& G8 @( j
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)' Z& T6 Z' A' n+ Y/ P
- {! {# a }8 j0 G0 ?5 ?3 R
; L3 e0 Q" @/ ?. S% F- }
2 c- [. N' i& v6 H5 V1 @% ` - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
, L/ d) z( `9 s$ @, v9 w. n - {) I# T, }' d2 W$ k6 H5 Q
- _DataPin = mePort[port].s2;
1 o& q% i8 q: z/ a/ o( I+ F - pinMode(_DataPin, OUTPUT);
" F$ [" Q5 z% v0 G: y. E - digitalWrite(_DataPin, HIGH);9 L8 }+ M8 z5 V) @. T
- }
1 j" E2 P2 b4 H3 h) ]. k) ` - #else // ME_PORT_DEFINED! Z4 [# Q- @2 V. g. S' c/ Z% F! r
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
% v2 E% |3 R7 o$ @7 Q - {: T9 P U: }- |( O: t: V4 U
- _DataPin = pin;) O/ L' P4 H, B- k
- pinMode(_DataPin, OUTPUT); 7 ~# T5 v: e z
- digitalWrite(_DataPin, HIGH);6 M1 C2 E+ ^( k7 i2 |! q/ m
- }
: Z; m: }" w' l$ v' n - #endif // ME_PORT_DEFINED6 }- _/ [2 V6 U6 x
7 Y& h8 K" W' f& D1 [* E& c5 f- , G/ u9 s8 N+ c I8 y
- void MeLineFollowerArray::setpin(uint8_t pin)1 ?+ X8 p5 f! V0 h2 O8 u- J5 ]
- {5 `8 M* v) \0 }2 h- s1 \" ]
- _DataPin = pin;; W* K" ?. ^) X. I) o+ P- U
- pinMode(_DataPin, OUTPUT);
; L1 {, Z6 L a - digitalWrite(_DataPin, HIGH);
& l y7 j1 d x; Q1 L
# z3 F9 \0 S' d% P" {4 o! }- #ifdef ME_PORT_DEFINED8 f. V- Q% b+ @: A7 ]
- s2 = pin;
9 o4 J W+ ?. ~ - #endif
) S0 J1 P8 m5 j; F- _, M" l8 ?" s! P - }3 |2 ~' e8 E# z A6 k, `) M
- $ p1 E4 A0 D# b4 K5 e
- uint8_t MeLineFollowerArray::getValue()3 P$ d8 d7 S) e* n* I
- {
6 w: m! y! V0 _ Y0 K7 k2 \ - uint32_t LOW_level_read_time;
6 a+ d! S" q2 ?, U - uint32_t HIGH_level_read_time;
4 M$ M5 h* J1 ?- C/ z+ W - uint32_t time_out_flag;
' P# H0 ]" G' Y2 q - uint8_t Sensor_Data[3];
$ z$ s5 N3 f4 f& D4 Z3 m: U7 w, b1 h - static uint8_t old_data = 0xff;& z O7 a4 e; W! q* o
# e% y8 l8 H* j4 Y* k* m I: Q- pinMode(_DataPin, OUTPUT);
+ R2 d9 H; H6 o. a3 d, G - digitalWrite(_DataPin, LOW);
+ G- S% B$ G1 Z" @% T) N; C# q - delayMicroseconds(980);
# X% k" Y$ B0 i4 t" a: n: @ - digitalWrite(_DataPin, HIGH);
$ W- {; o- m$ C
3 h8 F8 R* A6 t* m- pinMode(_DataPin, INPUT_PULLUP);- e' g4 l* W6 p8 E F; g
- delayMicroseconds(10);
7 r! p; t) _! Z; P4 X' R& `5 |6 U
" L& D2 W+ O: \. _- time_out_flag = millis();; @3 L( a* c {. ?$ H9 x
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. o+ K$ {4 p# A2 d( ]# N
2 {) J; I* b: e$ t- LOW_level_read_time = micros();
8 w+ }( O' i# [! d, T - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
. l B4 P9 V( B y. Q - {3 _: M1 g8 B* X/ e
- return 0xff;, F3 @3 d' y Y- E0 b4 E) T' c
- }
# J; w% o: {, k- s/ ] - 8 Y5 M, _5 n- D
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 G5 ~$ Y2 A4 ` D
- 5 z5 J* L6 J& P( c) ?: y5 [
- HIGH_level_read_time = micros();
' q* g5 ]2 ?0 E/ ]4 j/ ?' Z, Y - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
1 w- M R8 d1 |8 A - " F+ B/ i7 [; y: U/ s
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
r9 x" G: L$ @( W3 r" C% A0 F - {, x+ F( L: v) H! K1 h. A1 y
- return 0xff;/ T9 g+ N" m! d' a! \: k5 z9 u8 ^4 ~
- }
" P# x% i6 h& k+ h/ a4 ]) D& {
! x5 q/ B3 B4 i r/ N- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)): U& f' o1 X2 _. j6 b
- {3 E0 B( e! ]) ]2 \! ~9 y) i
- return 0xff;
+ P: Y- [- T0 I% g - }
* D. m7 d4 P5 C a+ v" g" w - 1 Z5 [9 A7 X9 Z, v
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); U% A s9 i' y
- LOW_level_read_time = micros();
+ f- j( I3 ]/ G% r# E( S* ~ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level! e1 v: Q- q3 L3 m* m
- $ n3 T( f6 ~# B$ C1 |' w9 j
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out4 e h0 l' U* A2 g
- {
) w5 ~' k, t0 P2 ^8 u - return 0xff;
* O0 v/ W! N0 U- x - }
o- |3 R% r4 B$ g - 0 P5 w8 O! S, V) P! Y( N2 k
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
" P b5 o; S+ J [ - {
& x5 ] {* v0 g$ Y - return 0xff;
N2 a$ Y6 I& Y# }: T - }
8 m0 Y8 C1 T4 u+ r- Z% `
" H9 u# C/ N, ]( l& k: Z) Q- for(uint8_t k=0; k<3; k++)+ N6 J0 J c/ L4 `
- {
, h2 ]8 c# L& v, m1 x - Sensor_Data[k] = 0x00;5 Z3 G) H I. G ^5 k( _' [
$ w+ D/ h) H" A9 h# u- b8 N. d$ `- for(uint8_t i=0;i<8;i++)% U5 g O. H/ d/ O; k% x# I
- {
2 h* p. G7 T# K# M - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level" e6 l1 ?6 T; W, \3 K( M7 p
- HIGH_level_read_time = micros();7 U" d; o' v- l/ o
- LOW_level_read_time = micros() - LOW_level_read_time;
. k8 o3 s% j- C! g - # ~) R2 T2 G& s; t6 h/ |" [
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
* B ?! M9 N f& J: S - {& X+ p! B7 d3 n8 T
- return 0xff;
& L: u W2 ^# ] - }
/ q4 R6 O2 {: H2 s3 L& q' Z* ` - ( F% H6 ^' | U8 B/ I; r
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );5 ?+ H; m" S, o4 e; \
- LOW_level_read_time = micros();
8 u: {7 A, e( Z1 V - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
( @7 J9 A& J) b - 7 F J, @( u e2 H
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
0 R9 x* @$ y. \/ ` - {
/ M6 _) v/ ~. J b( y. x - Sensor_Data[k] |= (0x80 >> i);. j( v' w6 t4 D& L: z2 `
- }
4 `* c! d1 ]2 s, }) o - else if(HIGH_level_read_time >= 100)8 r# v$ T1 S1 C" g: U5 E
- {6 A+ F f& J4 {7 k* l
- return 0xff;8 K7 r+ i$ I1 P% B% W/ U1 Q) b
- }
% X& y l1 p" J+ p- U: v - - G/ d& V7 ?& r. j
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
- c; t' o; i: T9 J9 ~! L% C: D7 n1 ?% } - {
9 ^/ G& `: V8 ^$ }/ `# U* V5 v - return 0xff;
, m' H% ^* V3 }0 }8 b. @: q. E. ?4 e) `$ c - }
: E- \4 E8 v: l+ z4 r }4 a0 m - }" a; O/ v! `, h( }1 a
- }
1 q1 R9 B9 E" n
( b) o5 b; C! B% @% W! e) s! N- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
) R+ P4 L4 r- M+ }! v - HIGH_level_read_time = micros();
/ J# H; t \& L5 t( p - LOW_level_read_time = micros() - LOW_level_read_time;4 e2 n: Y6 b" e. M5 W7 D
8 y- y; e& T/ |! j- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
0 V7 m* c' W; Y% L! S$ d - {
8 M: o. B1 r1 b* q# d* S - return 0xff;- u/ Z$ \8 Q$ H
- }
: Z& |0 h, X, [9 K5 R - # S) m' O" |+ o
- pinMode(_DataPin, OUTPUT);- a) G% I6 O! V8 m; l
- digitalWrite(_DataPin, HIGH);, S/ x; K7 a& t
0 O7 D7 e" I: q3 Z7 V- J- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
9 E Z0 v) S, E9 C# f9 P+ b - {5 g/ p/ N" }# ?# m3 U% c J0 Z
- old_data = Sensor_Data[0];! J! R3 b9 b- \! S( ?! P: A9 D
- return Sensor_Data[0];
, j& T" i+ e3 S6 O; t9 M' V - }
1 Y+ ^ ^+ ~: o# E- h! ? - else
h. s' P* u- a - {: _6 @' T b1 u! M U
- return old_data;5 X: F: p# [8 Y
- }( P* Y. }& b7 ^, ?% X4 p
- }
8 x) r: L) y+ G5 A5 u
複製代碼 0 g" g g* P0 ~& T: ~3 i: k" R- K5 H
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
1 ]1 i9 z( v# _7 j( H
) |& ]5 v3 K/ V/ I9 _7 {
|
|