|
|
0 `7 I7 j7 t4 r3 \& f+ T- #include <Arduino.h>- {+ D% N4 c- X. G
- #include <MeAuriga.h>( \' Y% M3 F$ u; E
- #include "MeLineFollowerArray.h"1 y! [- C9 R! L7 K7 d6 R
- # K1 f1 v1 Q4 S
- MeEncoderOnBoard Encoder_1(SLOT1);
8 F& O$ j& R! [0 N* J1 [ - MeEncoderOnBoard Encoder_2(SLOT2);* S2 H9 F$ C! f/ ?' U
- MeLightSensor lightsensor_1(12);
9 Y( ^4 }- ~3 @2 n/ l - MeLightSensor lightsensor_2(11);! s) V4 c W, T
- MeBuzzer buzzer;* \" U" d" b7 ]
- MeLineFollowerArray linefollower(PORT_6);
' b/ S$ M* r' ~- N - 5 ]; o8 z5 _& `" w
- #define Error1 15 t1 P, g* ~# @& S% O
- #define Error2 2
7 u$ |2 I4 M- D - #define Error3 38 ]& B. I) X" X5 F' c
! P) ]# M' O' s+ c' c2 v% Z- #define Kp 15
1 }, ~; m7 Y# V q: w1 w$ ? - #define Ki 0.152 f0 c' t- r& i" D X# |3 n7 z( o
- #define Kd 0.038 A' T$ {: R$ b# f8 ?4 b- q
7 `2 Q' t1 i9 w( N X, a8 s- uint8_t sData;
& `; W6 g H. }$ u - uint8_t D1;; r& _) ]% ^0 P' r* F: @' c
- uint8_t D2;
' n* N2 V5 d9 U5 o - uint8_t D3;9 B0 U9 k' ?8 j" S/ k
- uint8_t D4;
4 |* O7 z, V3 D( m$ P7 J8 J- E; ?' F - uint8_t D5;* ^! O8 K' S) I$ T4 V
- uint8_t D6;
4 j, F; l$ {& U6 S8 I4 U" _" ~
* K- ?( f) s; n K% T9 d- float previous_error = 0;
2 b4 U1 ~ T3 u5 \1 E$ }* [ - float integral = 0;2 `, `* G+ ~2 Q- F3 g5 }0 Q+ N
- float derivative = 0;$ z- r* F& o& _) c2 Y. X
- int Speed = 160;
4 B5 f2 N5 s) v3 X - float output;
; v: ?9 Y) N, o6 G
9 _ r3 c! z4 ]% r* v6 ~& T- byte Left;1 i- f' g. C5 t* J2 `1 ?+ l6 O7 K
4 G$ }; y3 g0 U! C% J( W! j# E- void setup()
; \* x t# }1 @ - {
1 ~8 z+ f: d/ M: ~/ Y! p1 U - //Set PWM 8KHz
8 Q0 U. p) S5 ~ - TCCR1A = _BV(WGM10);8 c% q3 |: [" Q( C% `( d
- TCCR1B = _BV(CS11) | _BV(WGM12);3 S& }1 D2 [: w, W0 `4 t
- TCCR2A = _BV(WGM21) | _BV(WGM20);0 j j7 U& U9 e& i9 n
- TCCR2B = _BV(CS21);
( ]5 U$ t o5 j. M2 d$ m& y$ | - Serial.begin(9600);
8 i* G- p2 C8 p" F9 d$ q0 A' } - buzzer.setpin(45);
. W' X3 _: o8 |* [* | - }/ q5 @4 i+ ?6 T) c
. C" K: _% E+ v- void loop()' g. C( k8 X! ]
- {) Z9 ]% o; f6 h% d* R7 o' |
- while(!((lightsensor_1.read()) < (10)));/ }+ S3 l& H" A+ [' v/ x
- buzzer.tone(1047, 500);; T# _6 C% Q4 D3 e6 Z
- delay(500);' k$ N7 q7 r) W8 x0 N
- do
: |9 s/ j3 ]' A i# c# z - {
9 {) j n$ g- F' E9 r- W - int Err = getErr();& d6 J3 `* J: l" v2 ~' a. J
- if(D1 == 1)6 K. D1 M. t; W; m( y- m; Y
- {
% E, i- b: X! B5 ]6 _/ R( [9 A - Left = 1;2 i. _- V% G+ y$ k9 {/ u
- }
4 o6 Q/ @- D5 B/ H" V# ? - if(D6 == 1)0 T k0 Q+ s: g) G
- {
, `2 n8 l0 G; C1 M7 M* ~+ c - Left = 0;2 y& o& y3 o. d. F
- }
9 B# P/ e/ {& n* l! v2 J - if(Err == 99)
/ P; {* t9 k8 C - {
: A& Y0 N4 b; Z. N4 ^9 _- c* ^ - if(Left == 1)" I" b: H3 ?2 C, L8 P
- {
0 x( e0 @7 R6 \ - Speed -= 5;1 n* s" r1 S; [* h$ z
- moto(0,Speed);; {7 z- r( i8 v+ T3 Z
- do/ L0 q Q, I; C6 \$ ]! C. |
- {
6 b4 t+ F0 L# Q+ k: L4 B - Err = getErr();
+ Q; ^- ?+ f6 ~. P$ h) g2 o( V% P - }while((D1+D6) == 0);
! g" [0 v8 }) z2 e- Y" t' O- t# T2 E - }
) m7 J1 Y# h) o0 `3 U. a& ] - else3 e0 a5 U* V# ?* f3 C; w
- {) F3 d2 k! g! H0 E
- Speed -= 5;6 a: ^3 l; V( R5 A2 p [/ @
- moto(Speed,0);
) q. p4 k! Y; Y+ V; n. c0 I# v) o - do- K; I& w9 ]6 M a7 O
- {- e% n9 u5 ~: ]: N. o. m5 K u+ R
- Err = getErr();5 A: J! c% e' b |5 `! Q
- }while((D1+D6) == 0);. Z9 \" v. O/ d
- }
0 K% e5 N+ ]% ^' U' ]; {3 }. g - }
' g. ?/ C& Y F( H4 r! ^ - else6 @$ w7 D" {7 g2 G( x4 O
- {
3 d- H; W7 H1 O2 X v! }* Y$ K% t - if((Speed < 160) & (Err < 2)) Speed+=1; 9 u$ R3 \3 F g# t" \" y A
- if((Speed > 100) & (Err > 2)) Speed-=2;
6 R" W, Y+ [3 `1 E) ~ - integral = integral + Err;. B4 l9 w1 G% x. i; O7 L/ {* g
- derivative = Err - previous_error;; V& W4 j5 c2 D7 I6 I8 n% q
- output = Kp*Err + Ki*integral + Kd*derivative;
. t7 ?5 Z: z/ o6 x6 J - moto(int(Speed-output),int(Speed+output));+ r6 K5 k2 i: }9 G; W. H
- previous_error = Err;5 D! Z Z; ? U1 \& d, o2 G
- }
$ c# R. k- ~# W* g - }while(!((lightsensor_2.read()) < (10)));' R) j* `. A. L- v
- moto(0,0);
( M; l+ W X; ~ - delay(500);
3 C: i9 a& i) C( J5 s: b' I% d - buzzer.tone(262, 500);
* t$ F* P& a5 X# t - }* h# f% U ?0 J, C
- 1 t7 ~/ H. h# t1 u- o
- int getErr()' q" m5 q1 Q& \7 K
- {
: L, ^' i ]4 |: k - sData = linefollower.getValue();; w4 a5 D- M! w8 f- `& q, `
- D1 = ~(sData>>0)&1;
: j6 v5 A( M% r+ v2 S* V( Q - D2 = ~(sData>>1)&1;; X( V9 Y+ D {$ X% h9 g3 C
- D3 = ~(sData>>2)&1;8 I) R5 d6 P2 f1 g- r3 a7 E9 X9 `4 k3 R
- D4 = ~(sData>>3)&1;% I% [8 h1 R8 V% x5 P
- D5 = ~(sData>>4)&1;' i4 u i& `% L2 x
- D6 = ~(sData>>5)&1;' [) i6 o2 P3 _- M* B& G" g
- int downD = D1+D2+D3+D4+D5+D6;3 _6 G( N! d" t# S
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
) w5 x# g! i; M# w - if(downD == 0)% } g5 x9 ` U5 p
- {& d- D+ E6 q7 D! I; b* Y
- return 99;
8 r/ m) V2 _. F; s$ V, [$ I - }; i3 ^! R. B$ y* X3 _
- else; Q! k' }7 q& R6 d+ ?3 j
- {
. e( m* w3 y' C8 J - return upD/downD;0 V3 X0 M: b/ I9 ]! P8 D
- }
% S# e: [" [! \( T4 H - }
# c' O+ Q) {6 [ k) M6 Y2 C
3 [$ s2 j9 J B, q* a- void moto(int Speed_L,int Speed_R)
% U# k+ H6 g) v" O0 h - {
* h* n6 B7 Q2 {/ V* g, K- B - Encoder_2.setMotorPwm(Speed_L);
( J1 J$ F1 ?9 J/ Z" t6 V* m - Encoder_1.setMotorPwm(-Speed_R);7 |7 i; ]$ d6 |6 O0 f ~
- }
複製代碼
" U& l3 b: M$ o4 E+ `( CMeLineFollowerArray.cpp
) E" d! _( b9 s% S, m% j! W5 _2 E- #include "MeLineFollowerArray.h"3 [- Q9 [+ j. x* c' U/ I5 T. z
$ `1 J" x# g x( e- #ifdef ME_PORT_DEFINED! a5 L$ e" }# i8 V1 B- s
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)% i# v3 p5 f, q6 i! r4 P3 \$ A
- {
- R. t; z* h7 }# B - R5 H" y! U6 W3 O. M
- }
3 L, d: f4 ^4 C8 @ f( O' w - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)2 O3 @+ T- K$ L, R
- {
/ \& M. w/ |3 \ - _DataPin = mePort[port].s2;! E; f5 d( j1 S/ A; h3 O8 o
- pinMode(_DataPin, OUTPUT); 5 g' X! S7 i$ L/ q4 K
- digitalWrite(_DataPin, HIGH);4 e6 P: W, u% U: |8 ]
- }
" a6 y; l" A! I n - #else // ME_PORT_DEFINED
0 }. o* a2 h; M4 [ - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)4 R+ I# g; l7 ^. H" l
- {
$ F# s6 r" i- e8 k) K3 K! A. S, x - _DataPin = pin;
) ]/ p5 M2 m6 R - pinMode(_DataPin, OUTPUT);
& `- s& g! x' B5 _$ T! q - digitalWrite(_DataPin, HIGH);2 \$ A6 f- u- ^5 ?
- }. D e" ~" d9 L8 n( ?9 a
- #endif // ME_PORT_DEFINED
. L, T+ L) T9 o: R: W5 ~" L - + K6 J5 |1 h# [ ^( R a
- * j. B. |; V0 `+ Q
- void MeLineFollowerArray::setpin(uint8_t pin), K7 L2 s$ F! r: Y7 I
- {6 f0 r- `3 J5 `2 o8 q7 s9 l, \
- _DataPin = pin;+ N" O( G+ }6 V7 {7 e, B5 Y! ]
- pinMode(_DataPin, OUTPUT); 7 ?" m3 H5 T3 j, G3 M+ A
- digitalWrite(_DataPin, HIGH);
. O& A+ F4 F8 ] - ' S. ~7 I5 o9 V, c
- #ifdef ME_PORT_DEFINED' H# D9 l( `4 Q* E" F3 H# l8 O
- s2 = pin;
# o) A& Y( U+ E+ y - #endif h; n5 s5 X9 r/ T2 t
- }. y+ O! e+ {$ S2 U- v+ d
+ d# `6 K* X+ N8 `9 w O0 S! J# V- uint8_t MeLineFollowerArray::getValue() x$ R6 P# o, \9 D
- {
" T. ?& S; u" n: c - uint32_t LOW_level_read_time;
7 o) N8 e7 r P% ` - uint32_t HIGH_level_read_time;
; o, L/ f( y! C; U m" h - uint32_t time_out_flag;
2 ]4 a8 V4 \9 e6 T - uint8_t Sensor_Data[3];( R8 x* E$ I) n, u/ x; c
- static uint8_t old_data = 0xff;
, x& Q( U# a' i* c% H9 c. H% Z
- S) M4 [6 g, i& T9 n; q- [- pinMode(_DataPin, OUTPUT);$ [8 D1 A( t6 ]' t8 d
- digitalWrite(_DataPin, LOW);; [. k1 q0 z5 O% D, p4 W; L b
- delayMicroseconds(980);/ R7 K8 V% A7 S: B. L
- digitalWrite(_DataPin, HIGH);% L$ [* @4 t \$ h# P
Q7 d$ W* h! X- pinMode(_DataPin, INPUT_PULLUP); N; v8 B( c" F: }+ I3 @! O x
- delayMicroseconds(10);
" M; Z8 Z o1 i% ^/ Q* h
+ @1 w O L, {+ ^0 T% X P; u! e- time_out_flag = millis(); K9 i( m: d/ |/ H! U" n8 p( z
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 `2 G+ J f) F
$ C7 H- H) E: q# b4 @0 W6 X7 K0 e- LOW_level_read_time = micros();
& k: |: V) ]- H( } - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out# T! L5 g: Z0 i" U
- {; z4 Y: K: Y% b( |- h3 v
- return 0xff;$ K2 ?! {6 x1 [( u! N
- }" a, [9 I7 ]" q& [# N; T' }8 K' I
- ( G- Q- r0 g u U3 b8 E
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 W/ ~7 P! {, ~, x& a) {0 O8 Q- R1 \ - 7 {# [5 }7 e% e, s7 A ^
- HIGH_level_read_time = micros();
" R% }6 v2 l2 u. N - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
1 P% l7 ~1 W, B; Q - , E6 L; V9 z6 n
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 s' T Q, A1 [; F
- {- s$ X% C2 S' `: u5 a$ c
- return 0xff;
; g% c, L) A$ ^3 Y. { - }+ g" e! J+ z' @5 v
' j2 E$ ? ~6 m" ?5 r$ ?* D! @7 ]# x- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
+ T1 w q' Z' P) J% p, p - {' Q$ P$ b# |& }& I- Z9 z
- return 0xff;
+ C. s% X" A' G - }
+ L( a7 V6 W, Y2 h5 g$ ^) T" o - - m! t& h1 Y3 o0 C
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- V4 {2 g- {5 D* T+ F
- LOW_level_read_time = micros();
8 ^% L! C/ L1 r) s$ l) G0 g - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
" ]% a4 u. g6 g& e: T
' F ^+ I/ u1 y L% H- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 m* }& h5 S: B0 ? - {, d/ u8 l e8 M- t' j8 g+ ?
- return 0xff;. P* ?0 m2 n9 I: U4 V+ r3 \
- }
[9 t1 _9 Y/ z" s: U- B* F7 L - & u0 w V. Z* S0 F) f( q6 w
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))8 j* a6 f: S) S8 x: H
- {% k) } ?1 p# Q, L V j8 l7 b8 K! n
- return 0xff;% ^( ^: L0 D" q5 n
- }
" X) _5 L( J7 ^9 c3 H6 `
4 X' C' D& B1 X3 q5 K- for(uint8_t k=0; k<3; k++)
/ o- J+ j$ X& e9 \$ j, m8 D) Y - {
. F' S( B7 X8 j! d" t* g+ A - Sensor_Data[k] = 0x00;
8 ]% d5 x+ N- x5 Y9 R) i, S - h/ }& [1 |2 r
- for(uint8_t i=0;i<8;i++)
5 k' V9 o r2 {' {2 i - {2 M6 [0 Z1 e. Q
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
- F; U1 m1 ~" N0 F, Q+ H - HIGH_level_read_time = micros();
) i: ]' _( L) s, K" _4 u - LOW_level_read_time = micros() - LOW_level_read_time;
& w' W' W2 v; u3 n3 e+ t+ |
% s0 `9 Y Q9 A/ l- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )" D" I N' C9 ~! u0 B4 K
- {
, {) A" i/ C" {# W% [6 o - return 0xff;
6 q! K, g. X! h9 E$ ~: `( Y+ C - }, ~! |# e+ ^1 k/ {" n [1 x' p
- * x) E$ Z3 ]/ T \, i5 w
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 h% Z7 P7 y6 d- X; a5 q. E - LOW_level_read_time = micros();4 I) m$ l5 I0 D7 o, a2 A7 I8 w- h
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level) I1 D9 V5 S" |& `; x z4 F) K2 ?6 w
- ) l$ G) B3 x5 _! Q2 s% r. d* ]
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
4 H/ {' ^" e0 }' t8 i" J - {
% T4 V" S* X2 c0 S0 d - Sensor_Data[k] |= (0x80 >> i);1 z7 T% k( Q3 P' S& ]( N& q& V
- }
( x( x9 t' @" @7 T- W* r! T - else if(HIGH_level_read_time >= 100)+ @$ [7 o' i2 f: V
- {# ]% J: F( V9 p) X- G; K9 Q# [- m+ v
- return 0xff;
$ b$ C/ Q( e: @2 y' n - }0 M# v6 x) F/ r
- q$ \2 [8 g+ B I- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out): [ T' t* f3 w& F* O/ _
- {
( A% \4 y: N, D5 T1 X* K! N. H) _ - return 0xff;4 R3 ]" Y. O+ X$ c* B
- }) L" f1 D* v: E1 l
- }
8 f. R9 j* `! g% c7 P - }$ o7 A* O8 P* a& Y
4 c4 g5 o% s1 D6 ^1 ]- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
& c- |9 M. B3 L( L9 Q) | - HIGH_level_read_time = micros();
2 t2 u% U) `4 O' s, s, a8 }. A: K) B - LOW_level_read_time = micros() - LOW_level_read_time;
2 u8 t" t) D4 a$ Z, S4 U2 h5 G - ( U; F* l5 f7 d! V; n2 N" N
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ) e, z# T4 h4 p9 U8 e7 K8 C4 n
- {
3 c. G' Z2 _ ]# U$ A7 \# a" c6 h. q - return 0xff;2 d9 z7 b/ _5 ~3 ^7 f7 M% x2 B( R
- }1 ], ?! u2 v* H, J$ t+ i" `6 t) Y
' Z! w3 q$ L6 `0 @- pinMode(_DataPin, OUTPUT);4 O4 |7 Y8 V6 U/ w& i
- digitalWrite(_DataPin, HIGH);
8 G5 r; p2 g1 |) ^- R, E
% ^& j# O) x' A' ~: T7 P- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))/ a, i4 A1 L7 c' \4 N$ Z
- {
" _9 W4 R; o, I# }" }% q - old_data = Sensor_Data[0];1 e7 V4 z+ `/ l% U8 ^) n2 H
- return Sensor_Data[0];3 q* S1 W. E" ?, S
- }- \* [2 s# {# p! \7 x! Y2 F
- else
. w6 G& E% C! D. X% Q1 [' h7 D - {
" s1 C8 M3 }4 t& b$ ~8 q# g6 \ - return old_data;- m, G; l" w* {+ p
- }6 o: M) v2 C9 l b3 C
- }
, d- }4 s% l: C. m; R8 h" Q2 G5 y3 v
複製代碼
8 e9 g8 P4 T6 a. |8 hMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
9 K6 z5 ^1 A+ E m* d4 g/ ~; t0 s6 h
$ Y/ g/ I3 j( X; }# f* x, g |
|