|
; k+ [* w6 ?/ J; ^8 }4 v$ w8 R
- #include <Arduino.h>
' d$ Q: c0 c; `' z' N5 ?5 ?7 Q - #include <MeAuriga.h>
; ~; ]* |. O! o3 p, s; o - #include "MeLineFollowerArray.h"9 C. v- Q8 E) |2 l8 q# Z) U
6 T9 M. l$ p$ D. p3 q, `- MeEncoderOnBoard Encoder_1(SLOT1);
, k- a& s* w0 ]8 X - MeEncoderOnBoard Encoder_2(SLOT2);
3 d& l( O W( c- s7 E! I3 t } - MeLightSensor lightsensor_1(12);
; x! j0 H8 h" g4 ~: `) w - MeLightSensor lightsensor_2(11);
3 r$ d& ] B" W- @/ y& \ - MeBuzzer buzzer;0 A& O/ C) |. d& ^, F! E% X
- MeLineFollowerArray linefollower(PORT_6);
. i: A' P, f* C8 N L4 R8 i+ E - 2 U$ g9 F. }4 ^3 n, ?, _
- #define Error1 1: ~8 L# {0 G( n; r
- #define Error2 2
0 p( B6 a7 {& o9 V1 `, C R - #define Error3 3
5 K# W. ~3 Z7 \ - 7 i) n- z3 l4 }+ X' A
- #define Kp 15
3 C1 n: b' z4 u- R2 ?* \9 T - #define Ki 0.15
/ ]" b- y; d' t; v3 R6 H. \ - #define Kd 0.03
, S; G% g) l. m9 j+ Y( ~- D+ H; d# j
S9 F/ E6 x7 Y# _( T- uint8_t sData;
% z$ ~3 |) X9 \) g - uint8_t D1;5 i7 w# ~$ {+ H ?& _3 }
- uint8_t D2;
( ]* K% m1 h$ G/ W$ K# O* a8 t. O - uint8_t D3;+ n) W( G* u1 M& ?9 z+ A
- uint8_t D4;
0 d4 q$ \& L# J! ^% ] - uint8_t D5;3 W4 q# ?7 L; Q/ j& r) A _
- uint8_t D6;
# ~' _6 Y" I6 {1 e5 S8 K1 U' N
7 K8 o2 ^( F" s; q9 Q- float previous_error = 0;
7 @9 c/ x+ q+ s7 Z% x - float integral = 0;- w/ Z' T! x; G1 R
- float derivative = 0;9 U( t0 P0 }3 }8 _7 u0 K/ _* ~
- int Speed = 160;% Q& T' L- C, t: c
- float output;/ [1 D: u! r% @: U7 \# Z
& B$ r+ W2 h2 Y( a; M- F- byte Left;
Q x' P$ ^$ D+ T% e+ o5 `9 r
! O* ]4 |" X% L4 q/ N% ?- void setup() 9 h& U; q2 y3 _ ?7 J3 {% [. J
- {1 d( ^1 f& F3 f3 M3 D- n
- //Set PWM 8KHz
6 }1 `/ A, M+ ~2 r& D+ D - TCCR1A = _BV(WGM10);% V' u0 r* e8 P- |" z; f1 U. k
- TCCR1B = _BV(CS11) | _BV(WGM12);
; y: V3 ?0 r9 n* Q - TCCR2A = _BV(WGM21) | _BV(WGM20);# u" O r6 X. {# `& K- i
- TCCR2B = _BV(CS21);. K0 ~4 T" T7 t
- Serial.begin(9600);9 V) R) M1 D' P0 _1 c
- buzzer.setpin(45);
3 E% c- J6 Y8 g) Z K! p - }
- `/ ~: x0 V0 Y
% `" [* X7 Y4 ~4 j4 g. R3 C- void loop()6 D3 U0 G) L0 j7 K
- {
/ f" X9 }' U6 u. | - while(!((lightsensor_1.read()) < (10)));2 Q+ ?- |# [; E+ B' [
- buzzer.tone(1047, 500);. o4 h: r2 F t8 V
- delay(500);
! n0 F# x# B9 Q" H% D - do
+ j' m4 }' D3 ]9 c& h" S( X - {# U: @/ x+ e p9 ~; U, V7 H
- int Err = getErr();
( A1 c* q2 m- S - if(D1 == 1)
( C' n9 ]9 t5 M1 y! A' _ - {- z: I/ m& @* p1 b- U' s; D9 u
- Left = 1;
6 o+ ~" Z% `+ |. D4 ]! @1 ~$ m - }/ I/ G& w" [% \" B5 x0 o
- if(D6 == 1)1 @. n& Z! h" o" p
- {: n- s( u: [7 O5 J; \
- Left = 0;: ~( n6 b& I# d2 u; n! ]
- }# V/ B, Y4 p& O7 o5 c R0 k& ~0 t8 \
- if(Err == 99)
# H F k3 q. |& w$ z7 u% T5 u9 { - {
6 i. V7 [6 P7 j$ V1 g4 N2 \: x - if(Left == 1)" ?6 L' M; z6 \. x3 a. T6 D" z
- {
! s; G: l3 w( [ u2 D [! d Y - Speed -= 5;
) x9 e6 v% p4 ]" _8 f9 Z9 T# E. T# Q - moto(0,Speed);
7 M' L, c* _" H% q2 b' r& f1 w - do
8 B, I) M# Y* @% h- ~+ G - {
8 ^! H& c0 ~$ {9 p0 [" X( K1 v - Err = getErr();
l+ N( [( A: W% H/ w& h - }while((D1+D6) == 0);& a* S! V8 e2 B; O6 a
- }
1 D4 Y3 o3 T2 s `% b - else- S+ k4 i5 w4 N2 u4 U
- {
+ R4 [$ |0 a8 M9 @2 H) N - Speed -= 5;7 ]0 z: a1 P+ e# l
- moto(Speed,0);
% [- r7 @' Q5 ]! T7 X) O* y# A - do
# _! \) V6 ]* W3 Z. J1 n/ @ - {- c5 K& A2 y( y, ]; ]3 w
- Err = getErr();2 `% W' h+ `- x+ s% H
- }while((D1+D6) == 0);
% y' A6 ^2 \8 ?3 x. ~6 p' Z - }( f. Y8 Z8 I$ o& K+ e3 v9 F" Q" R
- }6 a: Z1 F- Y7 [: g8 z) `' F
- else
6 v7 u( V8 ?) J! ]( I - {
8 L3 I# P; S5 U% R0 t) y - if((Speed < 160) & (Err < 2)) Speed+=1; - M/ b$ _' Z( S2 v) [3 z
- if((Speed > 100) & (Err > 2)) Speed-=2;
3 z. Q7 a- o; b3 ?" h$ ~& B - integral = integral + Err;' l" C* ~+ h3 k' ^' ^
- derivative = Err - previous_error;
: k$ z/ O, q+ b' g3 [ z& k5 m - output = Kp*Err + Ki*integral + Kd*derivative;5 M) \* M6 |) \ Y2 ] s. R
- moto(int(Speed-output),int(Speed+output));
$ j( y7 m4 p0 `0 o% v; E0 ^; R - previous_error = Err;* e6 J/ V/ Y; c1 u( G
- }2 I# t4 w3 @; ^; Q4 t
- }while(!((lightsensor_2.read()) < (10)));
5 Q# D" y) W' i+ c! b - moto(0,0);
8 q* s; O8 e# r - delay(500);
% B9 h4 u+ E/ b/ W8 X! Q8 O1 I* r - buzzer.tone(262, 500);
9 _3 d' P4 ?- L1 z2 h7 r - }
! e6 V: L! d8 y2 _( J( G* y - 9 ^+ F* H9 H9 W! k
- int getErr()$ V( u2 ^, e5 p5 ?4 o" Y7 H
- { ) t% T1 D* E2 w! n# U$ A$ A. q0 k
- sData = linefollower.getValue();
* \6 u' z' W9 e* [ - D1 = ~(sData>>0)&1;
+ `6 m7 _9 Q3 j* o7 ]: Q - D2 = ~(sData>>1)&1;6 P9 J+ b1 P w. A( h2 S( e
- D3 = ~(sData>>2)&1;
& u! x, D) |5 r2 ]2 W - D4 = ~(sData>>3)&1;
( C1 @' q; i2 q2 H7 A* N - D5 = ~(sData>>4)&1;
9 n# ~9 ~: n9 Y+ O! |' y, ]4 Y - D6 = ~(sData>>5)&1;
! H5 G. q: K' j8 v8 ~; i- |) h4 B - int downD = D1+D2+D3+D4+D5+D6;& |; I4 T" `# U. J9 u/ f
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);) l8 _/ N. c7 B0 W' ?6 J
- if(downD == 0)
, H+ ?/ L5 Q! S1 ~& j' r - {
5 f! C" `8 T3 Q6 w4 N: g - return 99;# o4 o5 c% G2 v
- }: ?% r2 ]% _6 k3 Y; _
- else
6 ~1 ]& H0 ~( {/ b; N0 ?# x - {
, p6 U% \/ K( X. D% N, d) h9 P - return upD/downD;& Q4 h9 m, m4 A, W6 e6 u% S
- }: ^2 M/ Y4 i: ]- Z6 U
- }4 G+ B9 v# G6 o) ^/ t j
- ) Z+ Q6 @+ v. @
- void moto(int Speed_L,int Speed_R)! o$ U( ^+ [. r9 r* g1 }6 n, [) R
- {
8 h# {, Z# M# g7 o) e/ n; ] - Encoder_2.setMotorPwm(Speed_L);6 t# f1 v2 L- u2 k- W9 _
- Encoder_1.setMotorPwm(-Speed_R);
7 o2 l. p9 g6 {6 x! {! {$ `' _# k - }
複製代碼
% A# _- B" d! {+ G2 j/ r& |! QMeLineFollowerArray.cpp
8 X$ c' v, K3 f* u9 d. F. `- #include "MeLineFollowerArray.h"
: R5 P5 F0 n+ U( P- {" b - # ?' \& C* O( [, G6 ^
- #ifdef ME_PORT_DEFINED8 E4 `9 _, Q% J
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
( d# O; P: Z+ l - {; w+ K7 r( m) D) h: J6 t
- , D# f% m) t( V, B; E- o
- }$ w! o" J- M0 Z2 K3 P6 p6 y" s: J
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)+ s7 d' }' J( D' q! J7 h
- {9 C2 v8 Z/ o e$ ?& }8 i4 J* W
- _DataPin = mePort[port].s2;2 s2 D( m+ f: }7 k5 o4 X9 X, a
- pinMode(_DataPin, OUTPUT);
$ [+ J( z0 |: H/ n/ M - digitalWrite(_DataPin, HIGH);/ r$ m" n$ f$ ?+ `7 M. \
- }
- e3 z3 i1 h d# _8 \' J" T - #else // ME_PORT_DEFINED* f/ D h4 z. d7 q$ K) y2 W
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)6 S* N z* y* A7 Y9 I
- {
3 m) ?$ r. o* J$ i; D9 E - _DataPin = pin;
( Y# }& l% W$ a; G9 A) Y - pinMode(_DataPin, OUTPUT); , t- `2 k/ U( W9 e2 r$ V+ a
- digitalWrite(_DataPin, HIGH);
" j4 M% D# s$ g1 A% Q) V1 O - }
* N3 k6 E w( m8 u0 a- U - #endif // ME_PORT_DEFINED
" u! G' q6 u, Q
1 X9 V; z g8 @$ U" K( N H0 P5 V
, J& ?0 K% z U) a. H2 W3 m2 c' i- void MeLineFollowerArray::setpin(uint8_t pin). {* O6 u! ~, x5 Q
- {4 E: Q$ e; S: b8 a! {6 d9 m( R
- _DataPin = pin;3 H$ G5 v0 i; r3 N- @3 T7 Q2 `
- pinMode(_DataPin, OUTPUT); ) [, X8 N" `* b: {+ a
- digitalWrite(_DataPin, HIGH);: q+ _% _( `# t
: F- m4 N+ L4 `1 y- #ifdef ME_PORT_DEFINED/ @4 @ B' d4 Q1 }) f U9 `
- s2 = pin;
3 j# @, t. N" B8 C9 e( l7 X - #endif# k2 c0 N( O9 ^9 ]: B+ @' u
- }
6 |8 ^+ n4 w* t, }0 ~1 L) ~
) r; J& s5 b" h0 m- uint8_t MeLineFollowerArray::getValue()
& [1 D) ?/ e' n5 A( y+ c, h8 J - {; `1 O; r B# u0 d
- uint32_t LOW_level_read_time;
& V7 D6 t/ f* C: a( ? - uint32_t HIGH_level_read_time; r- S% H+ Q4 l; o- P1 X. r, h+ R
- uint32_t time_out_flag;
4 C g" p: h; i! ~9 g - uint8_t Sensor_Data[3];4 \3 Z, t- X: p- x
- static uint8_t old_data = 0xff;
4 o& i4 i# m3 ^
1 |0 @5 s. i+ b* @0 D- pinMode(_DataPin, OUTPUT); _$ o6 h9 Z8 F- H
- digitalWrite(_DataPin, LOW);, U& j- k m! S
- delayMicroseconds(980);
! ] a* ~& ^4 ^ - digitalWrite(_DataPin, HIGH);
: t# K8 U6 a& L- Z7 y. W - ( \0 I0 `" D; t' I
- pinMode(_DataPin, INPUT_PULLUP);% R7 O6 g# G0 l8 w$ Z
- delayMicroseconds(10);+ a) g, u% M7 C L! _& A! l( k
- 7 Q: ?+ c5 F u5 w
- time_out_flag = millis();
Y* I9 [. U9 v! a - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 V, F0 X' ~7 d
- |* Q4 {4 |9 j5 e u) |) S
- LOW_level_read_time = micros();- _8 K& P" n1 u5 l
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ \: P& g9 E( F
- {' C! X( N6 d2 P; G1 H/ c" M- N
- return 0xff;: C! o/ {) k, m0 r5 }
- }" i) |/ z7 j* P& Z9 S" C1 r( F
! w$ w! H$ M# M6 D5 H8 Q$ v- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 F9 n4 b" W6 I$ `5 H. h3 _ - ]5 O7 m, e3 J( `- F) k8 A$ a1 F
- HIGH_level_read_time = micros();$ B) u" [+ {$ u5 p( D7 r7 s+ I
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
1 i% c- q1 r9 _% N0 V0 T
1 ?7 L& p) N# Y- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* `' d7 R5 D; X% P& @! A
- {- |! z6 i/ c/ X/ I4 J! g: [9 q
- return 0xff;1 D4 {8 F+ |; H; B6 c& B
- }( {0 V2 l+ [& L9 E; U$ @
- # `8 K4 O1 r- \ h5 I( ^ Y
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
9 {1 n; C( o' |' @8 S - {
# u4 J+ A' }) }5 p7 d- d _ - return 0xff;. \, b4 h/ P; A% j% U9 h% _
- }
$ G; @& A) Y9 S% h. q4 n9 |4 s7 z
" @+ @( t, p9 l- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
, O8 @7 d4 |/ K3 | - LOW_level_read_time = micros();$ x6 L1 U' n& e
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level7 l8 L5 y* ]3 x. K+ v
- W5 L+ |9 i8 s5 ]; W- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out4 ~! J: P/ R& Y% S
- {; h$ N, l: V2 }) @; a3 D4 k
- return 0xff;
" h: \3 Y1 K) N" n/ M' m - }9 p a9 R; b5 P* h, r/ ]9 f
2 o* y' e! j2 W W- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
7 Q# R* i# `+ F+ Y3 v5 W - {+ U+ S3 R* z$ x- j& |
- return 0xff;* T8 _ t3 u1 t% x, Y _
- }
' w D! ]/ |" h* ~ - : C" Z' Q3 d6 t4 V V/ j, f" \' y' B
- for(uint8_t k=0; k<3; k++): c4 P5 Q5 ?* w5 g# Z, i
- {
$ |2 |/ C$ A& N+ b" b" ~ - Sensor_Data[k] = 0x00;
1 D4 O F8 T" h+ m
' ]6 c: f5 q, _9 {4 Q4 w, U0 }! M- for(uint8_t i=0;i<8;i++)
6 q# S9 U- [, F/ o K, H9 @8 v - {
3 i- e. N& b$ H3 R) l( ^ - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
1 p; j$ ^3 k6 M$ U6 H5 D* M6 ^1 v2 e - HIGH_level_read_time = micros();
! F* k9 m/ c7 s, f6 U; A - LOW_level_read_time = micros() - LOW_level_read_time;
- b0 r. a+ s$ n Z+ \" N' F$ D2 p
+ ^. B/ W, j8 j, D5 K- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )& R; B5 R! e( y/ V* y i7 [: F3 t
- {. X2 n [7 Y. c. _% Z2 D
- return 0xff;
( O" R1 J( @8 |: D - } p. |4 J1 [; e
3 @2 h2 g" \ w! x, n3 F2 h/ t) U- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );; P9 i8 r8 U: M
- LOW_level_read_time = micros();7 k: S) s5 Q# d. \. j6 m! L. k$ F
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level8 F3 @8 e- S0 e# y0 r' d5 Y
- # C/ M. G( q! {9 u
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
) i6 @* `; w0 Q. K5 c1 X$ @ - {# u3 n) n9 C3 y7 ^( g# ^" |
- Sensor_Data[k] |= (0x80 >> i);3 }3 h) V0 d) R& a
- }
: S/ H1 k4 e' }$ |# Y - else if(HIGH_level_read_time >= 100)
, ?2 Q/ U: J" s+ s, W4 Q4 u# B' l - {1 g, Y! G/ B5 r1 C# A
- return 0xff;
" D( a: a1 M: f; ^ - }! N2 y9 ~4 H# i) S- ^0 e" z% Y
- . j3 A6 c( R7 h! w# ^4 c5 L3 |
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
/ S1 t1 v- `! t, K1 d - {
5 Q+ s6 o9 |# ?$ o6 [5 I6 r - return 0xff;9 j- x& E2 B3 G4 e. _6 `* }$ }
- }- V- D$ s9 C, D$ q4 b
- }
1 V0 x4 X4 b7 m( Q- M5 q - }2 C6 ^& P! Q5 X5 Y. I4 d
- o5 i+ K# j' N& Q% R- L# ], w8 z7 V- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
. @4 T+ J1 v3 S - HIGH_level_read_time = micros();
$ R2 r* F& ^' l/ } - LOW_level_read_time = micros() - LOW_level_read_time;2 h6 R- x5 _8 F8 v
- + D( `: t3 U" f
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
3 A$ u" i4 I2 o1 o! b - {: D, S p5 s3 {
- return 0xff;
) X" o Y% a& A - }
* t! Q2 N* \/ p( D; Z& ^* ^
4 H, w( J, R2 P6 c' o' |) f- pinMode(_DataPin, OUTPUT);/ t: b5 X1 o0 ?/ }. y
- digitalWrite(_DataPin, HIGH);9 N, V+ `2 @8 X: O: L9 l
- 0 l D* P, a- q% ]' F G: J
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))) Y8 |+ A* j$ X4 r) R; i" s; W: g
- {, x1 n1 m+ f, Z; S- v' C! @/ u
- old_data = Sensor_Data[0];# Q3 J/ s* ~2 C* M2 b
- return Sensor_Data[0];& O( z8 W% T" I) A6 L$ Z7 ]% Z
- }6 `! j" {& a1 t0 @. k' E9 h3 B
- else e! A7 k4 [1 z: s& h
- {
$ W9 A W0 V" B8 p B J; Z - return old_data;- z5 i1 w4 C- `& E$ C) z0 c
- }
# n( M. z# T- ]0 a - }
, k2 d6 ~4 {% y9 \0 M6 ^- N! a" I
複製代碼 % \7 O% u% z5 b) x
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
|
|