|
9 Z: ?- {) S3 Y
- #include <Arduino.h>
* X) Z: A E* i9 q4 O - #include <MeAuriga.h>/ P" [# `$ ]$ @. ]- P4 m% u3 C
- #include "MeLineFollowerArray.h"0 A, x7 I! S1 x
0 U4 i0 o! `6 _! Q8 t8 s4 g9 y- MeEncoderOnBoard Encoder_1(SLOT1);# g' z* b; ?0 i' p: b
- MeEncoderOnBoard Encoder_2(SLOT2);% Y% P$ Q. r: Q L# r8 g/ f
- MeLightSensor lightsensor_1(12); k2 e9 x8 @3 \2 v
- MeLightSensor lightsensor_2(11);
- j# l/ t* K4 M+ F" E - MeBuzzer buzzer;. \6 I1 b/ i A; i" P
- MeLineFollowerArray linefollower(PORT_6);
* h; L9 x Y0 @9 d r
1 E! w# F3 }) h- O& R4 c: d: ~- #define Error1 13 [. F8 }2 K/ R& u$ o9 Y$ L& S
- #define Error2 22 n! M/ P. u) r. [4 }
- #define Error3 3# l4 C8 U7 |' v+ _
8 s, P9 _. c" w/ g w* s- #define Kp 15( Z' \ l0 D3 L' A7 H. z
- #define Ki 0.15- [. c- o: { z# Y
- #define Kd 0.03! {6 b1 ~* o( ^" O: x0 B8 h1 x
- , [& H, R a8 y
- uint8_t sData;: U5 X) X+ M2 r) q/ G" v3 m
- uint8_t D1;
& k( M' V; \ w3 u3 _ - uint8_t D2;
2 Z5 `; ?0 [, z$ c( l" g - uint8_t D3;
% i1 k6 J2 j4 ~/ Z' i1 P! T - uint8_t D4;( n; O( L3 z0 Q0 V3 \7 M
- uint8_t D5;$ e' m4 x, [9 n1 a5 P
- uint8_t D6;) i$ e8 M- g( s( [1 J
- : E7 j9 y& g' G; M
- float previous_error = 0;5 Q. O& B! ]6 m% }8 ~/ P- h% F
- float integral = 0;
* b7 C$ e% y8 g, [ - float derivative = 0;5 m# ? k. a) ^+ j5 b- t9 z
- int Speed = 160;
( M; f" v- Z, v! A, E - float output;) W% v2 }: g" B7 O1 g0 Z
- . f- k1 A- Z# [0 ]" I
- byte Left;* C/ i) o; `, P7 u9 ~6 i2 f: D
- " z2 w, H: x. V6 o! ~3 X$ R+ H
- void setup()
I+ Z3 V( W# X - {
# g4 u' |2 K1 u% E - //Set PWM 8KHz
2 _& p& \2 @5 O* _ - TCCR1A = _BV(WGM10);9 |5 n2 y2 F2 w6 A
- TCCR1B = _BV(CS11) | _BV(WGM12); T0 o( K' Y* Y! t9 n/ r j, P
- TCCR2A = _BV(WGM21) | _BV(WGM20);
i4 E* `2 t8 u. u4 Y* i7 |: q - TCCR2B = _BV(CS21);# U0 i! A4 Q$ L7 n/ m) N
- Serial.begin(9600);
' L0 j! ?: d# s% T9 f0 _$ _& t - buzzer.setpin(45);
; e+ o8 K( Q' `2 |: X2 p3 Q [. M v; N - }
; `9 `! E6 @4 c, b8 u - $ h0 q4 K8 N" U9 D" b" Y2 i1 |3 A
- void loop()
" d9 U: y, F( Z- b - {
# w* z; `8 a# w+ Y/ e& ?. z4 W - while(!((lightsensor_1.read()) < (10)));3 d E! Y; p( ], o. T/ y. d0 J7 F! m
- buzzer.tone(1047, 500);' M$ m" c$ T2 r8 k2 f
- delay(500);7 [1 n4 _$ B ]1 p
- do8 `2 |2 H" T B3 d
- {
; Y8 G6 q. p% @5 T; t; [ - int Err = getErr();
1 f% ~2 G& u8 y. u a' ^1 ^ - if(D1 == 1)! W/ X7 B G0 k9 {/ i
- {3 y, p U2 B2 E5 B A2 Q% t
- Left = 1;
# F) a5 E$ H2 S" @" h - }9 h( U7 Y& Q, u9 L M+ u
- if(D6 == 1)
% a$ E9 W" t2 _& a - {
+ h: W3 b4 M$ x - Left = 0; w+ C q' y8 N" H2 \
- }7 h0 R' `0 x" r. m- B7 p# R
- if(Err == 99)( k& j# C0 p3 D/ D/ I x
- {2 b! l$ g' w/ @/ _( z5 @
- if(Left == 1)0 h/ E' D, O7 w1 D
- {
% e( _" Q4 U) J - Speed -= 5;4 L7 B3 g1 c9 U
- moto(0,Speed);
/ V; G* P/ I, f9 I - do9 D1 C X T0 A
- {
2 A& Z; H* B" i, e2 p - Err = getErr();
! y3 H8 `2 g4 H4 r6 T ?# d; k - }while((D1+D6) == 0);
, e# q2 `- k5 \ - }1 a/ q' ~& H: U( B; s5 p/ \
- else
+ g, i; g& G* \6 U2 j, q - {
9 Y c6 v& ]2 \! f2 \7 O! U - Speed -= 5;
; ~+ r0 N. C( E/ Z* _2 i. {2 `1 e; a4 w - moto(Speed,0);
7 f9 j( F3 a* ?# C. z/ q* O6 Q% Y2 a% | - do
5 G7 }/ z* \4 m! l# U ?! | - {0 a( H. r0 |: G e+ J' d
- Err = getErr();+ L3 I& s- Y8 j u% Q
- }while((D1+D6) == 0);
* |; l" y" y2 }& [" O" O - }
p$ `; P( Q" t' I' U- `8 b - }
% t. Z6 _: K- v1 O' t' O - else
- G$ X2 o6 p$ D7 }$ m0 J - {( \1 m- ~- E: [
- if((Speed < 160) & (Err < 2)) Speed+=1; 4 p$ v- O; X/ q2 R
- if((Speed > 100) & (Err > 2)) Speed-=2; 9 i; o6 ?" ~1 q1 B2 S7 ]; z
- integral = integral + Err;: A. i: O3 l3 C- [- j9 L
- derivative = Err - previous_error;/ p$ ~, `: N3 }$ {. ~5 n; f5 C& D
- output = Kp*Err + Ki*integral + Kd*derivative;9 x& a- ]* x$ h
- moto(int(Speed-output),int(Speed+output));
; y1 E, n* U- u% U4 [ - previous_error = Err;
1 W. ?* @# b# @& T& c* k# M - }
' I1 q8 `; }- H% q" C - }while(!((lightsensor_2.read()) < (10)));
+ t' S/ E- P' D8 F7 A2 n. r - moto(0,0);
: p, F! a1 c" x( F/ b- c - delay(500);
. K( p9 E) @& L' E; t - buzzer.tone(262, 500);0 S% R! q$ A& F
- }
" l7 o& b. Z' E+ R* S0 {, B6 F - # }, f" K4 \$ E
- int getErr()
/ v1 L0 `. S1 y: Y( t- p - {
! y7 j K* q4 Y! q2 A* q - sData = linefollower.getValue();
( P& g% D9 Q3 v - D1 = ~(sData>>0)&1;
7 y- B. H0 l% \# x3 u5 R4 a - D2 = ~(sData>>1)&1;
: @ K+ T& ~2 I! | - D3 = ~(sData>>2)&1;
9 T o* W3 h, |, |7 B0 N$ b - D4 = ~(sData>>3)&1;
6 K- j: T- e, ~ - D5 = ~(sData>>4)&1;; A c* ~- I i- {0 z9 v7 x( \
- D6 = ~(sData>>5)&1;
" h$ v4 Q* O K" j2 C& U& v) X* s - int downD = D1+D2+D3+D4+D5+D6;
H& N7 @7 ? w3 `& z - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
( s/ p/ S' n% D - if(downD == 0)
5 z4 t8 z* q6 a1 u/ f m - {
+ R. I+ ]6 M% O( ]3 o# G - return 99;
, @8 F# e8 s" b - }
" ` o* Y3 u6 U! K7 H - else9 k# ~! U4 v8 o1 e5 L; @2 P
- {' H% \3 M- `0 K' Z( r" q; d0 Q
- return upD/downD;* h1 z) T7 v1 j, ]
- }
. _ D* C& g3 ~7 F - }
! w, P$ n$ m4 Z/ r" Q3 O/ D - # j0 d3 ?7 i" {
- void moto(int Speed_L,int Speed_R) E: j0 X( a, t0 J
- {
1 |; y* I2 _ r' a2 e! L - Encoder_2.setMotorPwm(Speed_L);
: J. u. N: ~* U+ F - Encoder_1.setMotorPwm(-Speed_R);1 k6 E! l# k3 g/ G9 w
- }
複製代碼 $ z# a( w# p$ v7 {
MeLineFollowerArray.cpp
4 U4 X& q! t: r* k. W- #include "MeLineFollowerArray.h"2 Q! f8 ] D: L2 E: K: l$ r
W7 D& V0 e, P' L- #ifdef ME_PORT_DEFINED
, h. B( J% m( W - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
; e: J' }% x/ A; p0 T# h0 I. E - {" K' T0 n9 I' {( _
- " A6 A2 z+ H% G7 ^6 H
- }
" p) `: P) l! b - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
* R- ?+ K# W' ?, t: k - {
& O" A- C" a% e+ e6 I - _DataPin = mePort[port].s2;3 l# n6 i3 k$ `8 }, e: E! b
- pinMode(_DataPin, OUTPUT);
* g9 h0 E' b6 w9 G% F) b - digitalWrite(_DataPin, HIGH);; N6 \9 g* u9 s
- }
1 _, \5 E) `, l% i$ N, I - #else // ME_PORT_DEFINED
$ @( O$ c3 X" R - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
" n4 Z! ]" @$ m. E - {, |) l; P2 T. }; x& Y$ r0 b2 u/ X
- _DataPin = pin;
8 |5 s' T, C5 {, }3 j - pinMode(_DataPin, OUTPUT);
# H5 d' ]/ J8 _2 u0 {; H - digitalWrite(_DataPin, HIGH);
; W1 W8 ?% F5 W; L) l7 r( i8 s - }" d1 w% s) |% I9 ?! {6 Y, P0 ~
- #endif // ME_PORT_DEFINED, Z5 O# x) ^$ m# @6 ]- \
- : b* l; s/ M7 }! J+ j5 g* u
& H8 T5 ^/ ~1 r. q' K$ D6 {- void MeLineFollowerArray::setpin(uint8_t pin)5 X; A5 g5 o. O+ L
- {2 \) z4 ~; h9 v# N: D% ?. G
- _DataPin = pin;; J' B8 }. N) g% s5 s. V
- pinMode(_DataPin, OUTPUT); 6 n" [$ H/ s+ O* a+ y. ]4 A" t
- digitalWrite(_DataPin, HIGH);$ x7 p x! ?# n0 R- ]6 s Q
" I& P5 J# }% W' T) h& {, T. g: F- #ifdef ME_PORT_DEFINED+ j, k; I+ n2 p e8 n7 \
- s2 = pin;: D; Q! P9 F+ ?! E, R+ R- d
- #endif% M! O) ], n" C3 k% ^% J+ ^6 q# d
- }3 `# }2 N% T' z$ }3 i
- / ~: l) D- T4 ~% y+ j: B
- uint8_t MeLineFollowerArray::getValue()' `( E5 L; p' ^) K' E: I: A4 X
- {
# B- F" A/ r. s6 {/ ^1 Q - uint32_t LOW_level_read_time;' Z Z* y3 b+ _) _! M
- uint32_t HIGH_level_read_time;
5 Z' M" Z/ U+ [9 k$ m$ }/ U h - uint32_t time_out_flag;
( H1 b. y# t R$ F% f* A7 d b' G - uint8_t Sensor_Data[3];8 A6 B0 `* O( B
- static uint8_t old_data = 0xff;. W5 a- I/ z6 O5 i6 p8 @& F p
- ! j3 W3 K# ^2 S4 i$ V0 T
- pinMode(_DataPin, OUTPUT);
. M, }5 |" v/ d5 k% Y) B$ q' [5 h% E - digitalWrite(_DataPin, LOW);
7 n- Z9 N- _: `, [( W8 b - delayMicroseconds(980);
1 |- V1 \. Y# t8 h - digitalWrite(_DataPin, HIGH);
' s! {% Z8 c# z A, r1 o - # I2 Q, d7 a$ h' U
- pinMode(_DataPin, INPUT_PULLUP);
$ N. [! h, E8 l2 T: J - delayMicroseconds(10);; I: X0 X& u- c$ g
- M! [& v/ U. a5 L, _
- time_out_flag = millis();& {- i! ^9 b; D! K
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) `; d* w. k4 A1 t s8 p
- $ P/ F) k' T3 m
- LOW_level_read_time = micros();1 N1 i E X b1 M" F' m
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out& B' H3 {/ ~2 O$ l7 p- U5 V
- {
* x6 `5 i0 m( z5 [% m1 V - return 0xff;
5 }5 j3 D5 n3 { \/ \3 k1 m - }
3 a5 _1 }' K& d G# U! V - 0 Z, O6 f* T7 r- }8 A3 _% S
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 r' E% f3 x/ u% C2 Q& {
; Y: u- _+ @0 `9 N% y( e8 o. n- HIGH_level_read_time = micros();
2 I- _/ u E6 ?. ~5 _' i! C - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level5 ~1 c# N E9 G! {2 o3 G
) a+ f5 D. @" h( ~, j! R- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 H& \7 Q- s; q' ~4 q( s - {8 k4 W8 o6 ]& f1 b9 L. C( ?
- return 0xff;
8 ]5 l- K) d6 x - }$ L' t4 e* N; `( p3 Z
- % [) `: q/ e: x |& N- m
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
6 T3 |. t' @7 M( K ^ - {
1 k/ e1 T0 \; y |$ ] - return 0xff;
2 Y" C/ L3 c! ?, O - }
m3 z9 Z& J+ h, s$ B - # r" B% d, u. o, K( e
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) M6 R: b" y1 w) R6 h
- LOW_level_read_time = micros();$ o4 ]1 I6 E% A. v/ a
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level2 l, c; @. P, B; M* ]
6 k/ b, h( S+ U# z) O& a( h( g- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out) m8 W) t$ F# G) x. B+ |/ B) l( R$ A2 Y
- {
/ f* [7 l/ U. w& q3 W5 C9 N/ t - return 0xff;& }# h7 v. J! l* V, _+ f* m
- }
9 o: ?$ J2 u. F0 r - . F! F, g7 ^3 D |, M/ [
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
: G2 ~4 b/ v4 q8 S7 N - {5 E# R ?* r5 s% ~% U2 {
- return 0xff;
8 g+ q- G, Q( c+ v* {: D: u - }
' b5 F/ N, F! b( l. z
4 e- w" a1 W. i* S; ^- for(uint8_t k=0; k<3; k++)
9 ]6 _2 U, [( J- U4 t: W+ d - {% C0 [" I; p7 x! @
- Sensor_Data[k] = 0x00;3 d+ z. g5 u2 K) O9 B7 A
- ' L0 T6 U1 }5 b6 L
- for(uint8_t i=0;i<8;i++)
: @; W2 I+ ^1 Q' q3 Y9 b - {3 m2 n# q: i6 j5 N- g, G- p
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level# l7 ]3 G$ |4 b$ v2 e( z& X
- HIGH_level_read_time = micros();" W4 z- \ H0 J* b e3 d% T% Q( y
- LOW_level_read_time = micros() - LOW_level_read_time;
3 j1 h0 m* R$ T6 V7 G& O! G - ! X3 T4 w8 }# g3 s
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )+ e) C) H" c6 v( f
- {" v: F2 p! ~- X( j6 K+ l
- return 0xff;( a9 S. W: [6 ?; F N
- }$ K5 X- a/ O& p1 u8 d$ r
5 k! |, R9 K# m3 J- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
" j* ]. e1 ]( q& ` - LOW_level_read_time = micros();
! j7 y* z. @: G4 {9 A7 z* {' P - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level9 J8 v3 _, f7 K$ o6 E
- % m$ R0 E" b; [6 R) |( B" v
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1; f1 L$ n( h- W4 O! ~, U
- {! V# l. b( {( X
- Sensor_Data[k] |= (0x80 >> i);
; d9 e3 [' c* h1 F - }7 h1 \2 X3 d/ P) O" O) I- n1 j
- else if(HIGH_level_read_time >= 100)
6 A5 I- d' K5 }9 k/ G8 P8 x5 \ - {
3 Y! y" o, d( `) w+ [ - return 0xff;
5 N9 `5 c& j3 {4 g7 c' b5 F - }
1 |+ _# p A3 }# B* {* v1 y
7 X. P2 S& ^7 _+ t8 G' V- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)7 A6 L& L/ I6 @6 ], V
- {/ U8 h: s0 `: C6 k9 G
- return 0xff;
2 _* k$ w/ C7 }- T1 G3 ?9 e - }
% Z5 v# D& v0 D' c3 M& H/ z% q - }7 \! b2 t: `# j- L) f- a, X7 K
- }) H M/ x- }7 G% X6 l$ y% M
- ; o% Z: K7 z' d4 f8 n7 a, W c' M
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
2 m. h: A: c; N" F7 ~ - HIGH_level_read_time = micros();: p0 g2 }% o4 e, k+ ]1 W; z
- LOW_level_read_time = micros() - LOW_level_read_time;
, [" l8 T7 {7 O& ]
0 w; j( c4 f3 e+ z, i2 |6 d0 C- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
/ E" K7 c" e# l$ ^3 u H - {
( C/ G0 t7 B, z |1 g - return 0xff;
. j4 Z1 }4 }! ]% |: e - }
! z- h, X5 X6 |8 D& z" p0 v - - e, v& _1 R# D5 T) y$ [
- pinMode(_DataPin, OUTPUT);
. ~6 t p& F8 n' e6 ?* M& v3 g - digitalWrite(_DataPin, HIGH);, T% a V9 p7 j0 D: M
1 j5 C, x1 Z3 Y2 i$ g- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
* s7 g7 g K( z6 k - {
5 R0 c- F- @( X7 [. ]# J3 `! _" Z& _* S - old_data = Sensor_Data[0];
! u9 v6 D% j8 H9 D2 ? - return Sensor_Data[0];
2 h: l' A+ {+ x7 U$ x8 j3 E8 _ - }. u( P' x) j4 f) y% @0 k% a: ~
- else4 N5 M. K4 n4 L" Z
- {) i4 E4 K% `7 K# A) N7 p8 O
- return old_data;
5 z& _* v, c4 d$ p; e# _) O9 h - }
& {; G' @0 l& s' ^: E+ k - }
0 N% h6 C/ g R4 j/ A+ `' p
複製代碼
. u2 T5 K) t: |0 o* g0 _: j3 }1 ?( sMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
) r( L) h* `; {. P
! A8 b" l% A. A" k& V8 b |
|