|
|
; y8 a; j, O8 \0 Y4 K# R: G+ B
- #include <Arduino.h>3 F+ S& b7 \9 \* t
- #include <MeAuriga.h>
x& h; h( k4 I% \4 Q - #include "MeLineFollowerArray.h"
5 J# c6 q) f1 o9 I2 i" X% @' c
$ J# G$ J: H4 c: r' v" _- MeEncoderOnBoard Encoder_1(SLOT1);
, P$ H4 J9 b: u, W. h. @ - MeEncoderOnBoard Encoder_2(SLOT2);1 `9 x3 I% G! t j0 V, W- j
- MeLightSensor lightsensor_1(12);/ J. \9 [9 ^& G5 x4 q* X
- MeLightSensor lightsensor_2(11);5 V6 ^# M% M1 c- t+ J! C
- MeBuzzer buzzer;7 a1 F- R# E+ ~: }
- MeLineFollowerArray linefollower(PORT_6);
- T0 B z; K( o2 S A3 [
0 m' @! I& {. n+ W5 v- #define Error1 1
6 D# P' z( ?; }- d - #define Error2 2# f! i: u* U! z! V
- #define Error3 3: Y# ?) e/ ^* h2 q. N
4 s( g! H$ f& D" _, f5 w- #define Kp 15
( p- U9 _) h) f+ m5 W; f - #define Ki 0.159 k: I- i! I, g( }
- #define Kd 0.03
: v- \( {! ?* h1 @2 z/ v+ d: ]3 j
3 M& k O9 c; j) _- uint8_t sData;
% S f8 q4 t2 ?* f% Z, S$ x - uint8_t D1;
B9 L- ~6 G4 q' x3 c. p - uint8_t D2;
4 {$ Z! g) v% I# u - uint8_t D3;
! {; b6 u# z$ q$ i8 |# r- [: Z - uint8_t D4;$ U6 l, a3 H8 M
- uint8_t D5;, G0 k+ c7 x" ]; ~8 z2 v- G
- uint8_t D6;! q8 ]: a- P5 R: T. h& E/ e$ {! W
- ) E( q* j8 ^ k6 z2 g
- float previous_error = 0;
$ ~$ y, Z. [! s# i2 N - float integral = 0;& f) U/ V- c, e6 D i/ f
- float derivative = 0;
! `5 _! d8 n+ Q, X - int Speed = 160;3 ^6 e( T. |3 k& j7 D
- float output;5 B/ T8 V7 J; a0 b' J: U
- : T( C$ f7 e$ T; o) [6 G
- byte Left;
0 `9 b5 e& X2 o4 f3 N - # g* [& j9 x& F$ o
- void setup()
% F1 l. F+ s, ]3 M - {2 L% e$ q* y& @/ D& n: [/ Q0 Y
- //Set PWM 8KHz- ^2 J* P4 _5 b+ ]5 U& a; C
- TCCR1A = _BV(WGM10);* e% D4 m: J. x
- TCCR1B = _BV(CS11) | _BV(WGM12);
# y/ t# Z2 N' n. n3 n# L - TCCR2A = _BV(WGM21) | _BV(WGM20);
2 X$ Q+ d. t6 d6 ?& M; { - TCCR2B = _BV(CS21);
7 d/ s0 v" U9 O, W; q - Serial.begin(9600);* c2 b& |6 Q; ~3 w+ e
- buzzer.setpin(45);
. h7 M0 i9 R. I+ M1 h0 w" B - }
; c' ?% V/ _' V$ j; m1 Q/ ~ - 2 C/ h+ ^* |% o; {5 i- K
- void loop()
$ u y* U+ C$ A% C. g - {
- k$ r7 H* {# q - while(!((lightsensor_1.read()) < (10)));
# _6 i' f5 r% r7 g# \9 W$ ^ - buzzer.tone(1047, 500);7 C, L& ]% |6 V1 v, |9 h9 P. g: U0 H
- delay(500);, p4 {6 P. X8 a" q
- do
2 l3 m0 E) o$ t3 X. M; Q1 y, E - {
0 O% E( h7 P, |. | - int Err = getErr();
4 a+ `$ u* `+ q$ F6 U o2 m - if(D1 == 1)
6 D! g; @8 t8 T; [6 r - {! d/ Q4 b! q% ~% ~
- Left = 1;
% a+ g' U# P$ A- Q2 |" r/ U - }( W% y7 X+ E8 }: y7 Y; I
- if(D6 == 1)- A$ u$ `0 B- Y5 l z6 o; t v
- {. z+ ^$ O( D: a, |
- Left = 0;8 d% Z8 S. l1 i
- }
* h. k v: a0 G9 f( f" L0 @ - if(Err == 99)
7 N% j5 Q8 X& x' d# x, P# P7 |: X - {
1 t! h1 q+ ~4 p; |* _( o! b; C - if(Left == 1), B; T5 V9 H1 \; @/ C' \6 Z
- {
5 ^) ]6 @5 Q+ |- ^, x- @$ v - Speed -= 5;- G: F, I& O; M2 n1 f, [+ K
- moto(0,Speed);
, V3 o7 Z0 C/ a t0 I# Q - do
, w% V9 j0 q% w3 q - {
& n6 @* V9 j" y. f; D5 | - Err = getErr();0 W. q* S: z4 ]4 q/ b3 P- r
- }while((D1+D6) == 0);
: g/ W* G/ p' W+ N3 \" j! n - }" M/ g" c! C2 M$ z& Z
- else$ c8 ?# q _3 o1 {
- {6 J6 L+ D8 ]1 ]" W# o" q7 S6 H/ x) L
- Speed -= 5;4 a4 K% y9 M$ [, w
- moto(Speed,0); 2 m0 z% U6 m( {* t1 m
- do
% s! j5 b" e* j - {
# B) S: t: r' ] - Err = getErr();
; L# ~# U6 A% @9 L - }while((D1+D6) == 0);
4 r8 k* P+ h$ M! J c2 X - }
4 d! j5 w/ i1 V - }) @2 J+ \2 `. B* n
- else
2 K( E' z5 X4 B* J: J8 r( ? - {) |4 S0 j( ~! _( S- \
- if((Speed < 160) & (Err < 2)) Speed+=1;
K' k; L# j0 }2 V& C3 l. L: z. s# }" t - if((Speed > 100) & (Err > 2)) Speed-=2; , |6 r' g" J# K5 s8 w [+ W
- integral = integral + Err;
/ u* j. m' B8 F& u' k. R8 c. s - derivative = Err - previous_error;2 I0 t l! ~) Q, w% D2 H
- output = Kp*Err + Ki*integral + Kd*derivative;, s6 m! c n& Q( W5 l
- moto(int(Speed-output),int(Speed+output));7 b9 c; W5 b7 m) q2 i
- previous_error = Err;9 H& Q+ Z/ r' S. ]6 T
- }! `: C/ U' d$ V; Y) a
- }while(!((lightsensor_2.read()) < (10)));8 E5 b1 m& Z+ Q$ E$ Y; f+ p% {3 ?
- moto(0,0);8 {- `8 k! K5 c' S& q: e9 A
- delay(500);
! c9 v( F$ s( w7 z1 o( o - buzzer.tone(262, 500);
a% G3 V5 J5 G8 u7 t' e2 U! j- W - }. I( p, P3 t, k, z
) K6 n' H8 f# w- int getErr()
X+ Y: p% W9 k( D9 b7 G) M, W - {
0 }' s0 U( H ^3 N - sData = linefollower.getValue();
7 s: a5 Q# d/ C2 y# ?3 M. M - D1 = ~(sData>>0)&1;: m9 b$ j5 B$ E) X! n) f
- D2 = ~(sData>>1)&1;
$ O7 h% L5 s. O2 I; Y - D3 = ~(sData>>2)&1;
8 A1 C6 K5 F Q; K, c - D4 = ~(sData>>3)&1;
2 H z& J" h9 f6 L* p o - D5 = ~(sData>>4)&1;
, F0 ?3 N0 }( W - D6 = ~(sData>>5)&1;( _- o* a% j5 T$ F s
- int downD = D1+D2+D3+D4+D5+D6;* m4 y* K1 x: G6 o2 M$ v
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);. Y2 Q3 _/ s: c4 ^( [) y
- if(downD == 0)
# Y( y) N, r! K2 g; D4 F - {
' Z- m7 z$ U7 [/ X7 _2 k - return 99;
7 I, {; R3 B8 o2 \( e8 P2 x3 T - }/ P( v5 _/ u: I: _' P( b
- else. r( q& s# k w5 E, W$ O# b
- {
% a5 R, Q: C# C3 s9 [ - return upD/downD;7 W( r6 `3 m, B+ _' N3 `
- }
P2 l5 C5 M. K* ?; E - }) U$ g0 A. {& `5 c/ ?4 R" y
, |$ l I+ i- r2 L- void moto(int Speed_L,int Speed_R)6 [6 D0 l, o2 B$ p5 O3 k6 k1 \
- {
% N+ A( r/ `& |2 o: t- ^4 G - Encoder_2.setMotorPwm(Speed_L);
6 J/ }# T& }* I) ?, q - Encoder_1.setMotorPwm(-Speed_R);
% K2 t p- ?3 v+ o! o - }
複製代碼 1 ~3 {8 K5 b, T
MeLineFollowerArray.cpp
# F" g% y% C6 s% C# |- #include "MeLineFollowerArray.h"/ ]) g7 e8 h1 F0 M# n) K: c
6 R$ I6 q" o8 v- #ifdef ME_PORT_DEFINED' Y1 ?. l. G- y3 ?
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0), ?+ h0 q/ S# [( f: d
- {; Q2 L6 u2 z( d$ ? P
0 l/ Y& U" E, H9 X( ]3 R) R0 z: c. f+ N- }9 P; ^0 x' ^4 v; \
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
3 x1 t4 B: q) H, E - {2 x/ z! [! s! V7 f( d
- _DataPin = mePort[port].s2;
/ O8 k9 n T& z! O - pinMode(_DataPin, OUTPUT);
h8 y4 b0 o2 k7 P& N' ] - digitalWrite(_DataPin, HIGH);
7 e9 j, a8 Q6 i: R7 y8 C - }
6 Y6 t$ r7 |- |3 _ - #else // ME_PORT_DEFINED4 U: ?+ I6 F4 f7 \% O3 m8 A: I
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
2 `+ V6 V2 [6 C( Z) G0 L - {
2 V0 q# }2 _0 S, U B4 y& W; ^/ \ - _DataPin = pin;" R9 E& l6 o+ Q0 O% F# ` P- j' ]1 Q
- pinMode(_DataPin, OUTPUT); ! Q' n" c) F0 M' I
- digitalWrite(_DataPin, HIGH);, H; a2 q* j! J1 w8 c- j% O
- }3 K( B! d( M* t' a9 q# C/ t
- #endif // ME_PORT_DEFINED
) f8 {: y( f2 z/ r' o: ] - & R: n, U& p' k: d3 N5 s% B$ I
( k* G8 g+ h1 o, ~/ A- void MeLineFollowerArray::setpin(uint8_t pin)4 Z6 F6 L. A" `: t* P9 l7 N$ \- g
- {5 a! f5 @! H4 W' `# t- ]
- _DataPin = pin;0 Q7 M: z! k: Q$ _
- pinMode(_DataPin, OUTPUT);
& I/ P5 C1 ]; s0 R+ q - digitalWrite(_DataPin, HIGH);
* J; V% k3 B3 I+ d
" ^" z& p5 v; n. T: E) ~( k- #ifdef ME_PORT_DEFINED4 T: z, }) T1 m+ f9 J
- s2 = pin;
( ]! p9 ?3 E+ T+ | - #endif
0 R) M9 \& w z" I2 t" g2 O - }
; {, d, k& e. ?4 k% U E - ' A3 `& t* n4 u+ K' P6 \
- uint8_t MeLineFollowerArray::getValue()
( b- A" ^; O2 w2 O - {
: z# h) J0 r6 ~2 F - uint32_t LOW_level_read_time;
2 h; Q" Z! n! u+ _& t' e! r - uint32_t HIGH_level_read_time;. S- O* G# ?' V/ H9 |+ d1 A
- uint32_t time_out_flag;7 ?, G! J! K8 T% l* x& }
- uint8_t Sensor_Data[3];4 x \' O' h0 H% R
- static uint8_t old_data = 0xff;
( q, c9 L9 q" v( `% |
0 |, W* L+ }3 H% J$ F- pinMode(_DataPin, OUTPUT);
- z# T7 Z$ y1 S- C. C7 e - digitalWrite(_DataPin, LOW);
7 S, X3 r; w1 W* O d" \ - delayMicroseconds(980);
; H4 l, K2 q( k8 L# ?/ q - digitalWrite(_DataPin, HIGH);9 z4 N5 \) k: g
, q$ D( {" ~' v0 |" u0 F0 b5 i- pinMode(_DataPin, INPUT_PULLUP);
1 ]. _0 b/ S9 y$ L3 e5 V' ] - delayMicroseconds(10);0 `: p$ v! x1 m3 v0 x C, [
1 P% r7 f" c% X7 F# e- R- time_out_flag = millis();
. ]7 j! _$ k: ~0 D' c - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. [) }& p s) n3 O - $ z; z. n- a" ]" z* C$ M
- LOW_level_read_time = micros();
1 d( ~4 y! k1 _9 `0 N9 Y - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
' r0 B- u1 O1 [: V) ~/ o - {! y3 @7 K8 u8 W2 Y) T% f
- return 0xff;
9 P) S$ R* @0 v0 t) \ - }; x- b1 r9 `* C- i+ t7 y
- * N- {9 _3 u# N5 K9 ]8 t" `
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 x" r( X- u/ @
$ _- M* f) ~% q2 V" C- HIGH_level_read_time = micros();
/ t; @5 v0 g/ G& n3 c/ y* v& z; } - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level* \/ `9 d( c1 S# [4 i6 ]
7 l5 m, r9 Y: Z6 x% `3 ]! N- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out# z* o" L+ a" m7 Y
- {0 A" A( c- {. v/ |$ y
- return 0xff;
* `2 D& ^+ V: @ |8 H - }: \8 T: M% E; j
( U2 U, n4 y1 J" K3 t3 W- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
$ V* S p, m5 U7 [! B - {
( ~. E% d: a' [! v4 s% m/ j' c - return 0xff;/ P3 p/ U: p/ b- q
- }
y. m/ F% ^, Y& x6 i0 n5 F9 F/ K6 d
2 _$ t" G. l" X) t/ a- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );; E& c3 I& I9 I( e
- LOW_level_read_time = micros();) n S6 b+ W) }/ Y- `. o7 u6 A
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
7 ?% U: c5 x. @( e5 s- l - # m% E; E- s$ `4 [6 ?2 `
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out5 y+ m- a8 R0 W# Y
- {
1 o3 R! x* d. i - return 0xff;! c& s" h# Y7 k
- }
8 L9 c& [; l! [( y
! ], n" {1 d1 s- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))1 u: k" U9 h9 W5 ]9 q% r5 Q
- {
/ T/ J+ {7 r Q$ L - return 0xff;' Z9 O; A# u# W" h: L4 {2 L
- }
! ^7 S: L# n, j7 L- O
9 |3 l8 W$ b6 O* q- for(uint8_t k=0; k<3; k++)4 u2 y& z. Y. F, Z H+ P& f2 H# b
- {" i) x0 ^2 O- ?! f6 K
- Sensor_Data[k] = 0x00;/ y) X3 I) M% b* D s9 r
- A4 o, z! f$ }8 e8 [% ?
- for(uint8_t i=0;i<8;i++)4 | }6 v" O( r& b. f" B* L H+ f
- {( [& k& R: y' ^& }0 u$ F2 @
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level4 i6 X* E) X6 N
- HIGH_level_read_time = micros();
e2 i" z1 n! k - LOW_level_read_time = micros() - LOW_level_read_time;
3 @8 t$ b3 n' i$ S5 K8 E! v
& A& f ?! @) c( f2 t% }- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ); _# n& }0 [$ y) o7 n9 C
- {" m: |0 `- d: O, X( r& n7 O3 r
- return 0xff;
0 }! d D" v. w4 _' x/ z1 o - }) T/ s* S( T" `0 p
- ' Z; b2 y) v# J/ ?6 t2 l/ r; j' d& e
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( M" ~0 N+ d! H
- LOW_level_read_time = micros();
' Y; O T1 H6 }( \& `% O, Y - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level% w* w4 Y0 X( j# T
. P3 W. O3 I5 {3 |- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 12 s6 l t9 w! {+ u# a, j
- {
1 F! \! R1 ]( R6 t# Z. D2 t - Sensor_Data[k] |= (0x80 >> i);+ v7 e( i. q/ r! Z5 A# e
- }
/ @9 M w5 @" R' ` - else if(HIGH_level_read_time >= 100)& \' o. c+ I8 @
- {
! j9 p$ E) `, R - return 0xff;
! w& v% `/ S. ?" g0 ] - }* C+ s& o, ?/ I2 _7 ^
- + B1 ]- x" S6 r0 y) Q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)% `$ O( `9 O2 j& j) x
- {
8 a& r9 G8 x4 b: ^/ { - return 0xff;
/ @, ?5 I. z0 g# T' \$ } - }
# |7 L5 r( M2 s! `! Y( z) s - }
8 C5 y/ F- U. ~8 z8 ~! F' _ - }) P) s. Q4 Z* \
: k+ C5 E4 q) o1 S) U4 g: J- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
# ^ h4 q$ J5 ? - HIGH_level_read_time = micros();
) F5 J/ m$ c: X5 x6 T, ?/ z* h3 ^ - LOW_level_read_time = micros() - LOW_level_read_time; I5 B g1 @. C
- 0 V$ g3 W `% g3 t; Z3 ?+ _4 I1 ] O
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )' B, L% }, Z( s: _0 _' @
- {3 X4 C! m& w& ]4 ]3 T
- return 0xff;
* D- e( H3 K. J0 U& y) ]- c - }( n5 Q1 i) h, Q# |2 U% i1 @4 X
f7 |5 ~) h0 X# W. U4 O, ^1 I+ o: S4 k% t- pinMode(_DataPin, OUTPUT);6 K* Y4 G4 X% g* k* h
- digitalWrite(_DataPin, HIGH);8 D( [! h) Y! p" k0 g |8 j
- ]9 n4 @1 a# o* q. ]0 `: Y- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
3 c7 m' {4 ?/ M; T5 c, D7 Y - {+ W5 n+ Q0 E, K! @ ?7 N
- old_data = Sensor_Data[0];
$ z, j. _( `% A - return Sensor_Data[0];% S& B) U9 Y4 l, S1 E; @2 O! A2 S
- }
- A8 |4 c2 ^+ k6 N5 Q - else, m% i& g& W# o- F$ X2 b2 R
- {; H. Q; X& b; m2 P
- return old_data;$ X% U7 x) F7 G. B
- }
) R# u$ a6 u* a& U& n1 g& c - }5 I$ B8 |. D4 ]. m' g
複製代碼 ) S7 O/ W% ?1 p! N7 r
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
% \+ H0 P8 q" P
! V5 y4 B; i6 C9 _ u5 w
|
|