|
|
- G: g6 C. f' ? u9 f# |4 a
- #include <Arduino.h>/ H; z6 y5 U/ O
- #include <MeAuriga.h>2 }- q5 `% ~4 e
- #include "MeLineFollowerArray.h"
1 e) a6 S+ [9 J7 I1 y% q
# V# Q+ D! d6 h7 }% g- MeEncoderOnBoard Encoder_1(SLOT1);
. l6 x. S5 S( l - MeEncoderOnBoard Encoder_2(SLOT2);
! E9 ~+ J: a& L/ S! O - MeLightSensor lightsensor_1(12);4 @0 S# t, A2 q1 s+ H) E6 |
- MeLightSensor lightsensor_2(11);
8 m# f d- A3 { - MeBuzzer buzzer;1 s) S$ Q% p8 R/ g# Y+ f! j/ H
- MeLineFollowerArray linefollower(PORT_6);
: V: F7 [: Q5 T0 _9 P# S
! q$ n! X) R& m5 m& C; ~- #define Error1 1! o- H& [, G$ y1 l" k/ U1 v
- #define Error2 2
: v$ [+ v/ l* _ - #define Error3 3
* d# F0 y/ E, t" w - " P N/ o5 P1 ]( g4 T- R) O
- #define Kp 15
, o: S; L e; o/ U" W6 \' w - #define Ki 0.15$ z" [% r+ w& v/ v( Q3 B- I
- #define Kd 0.038 d+ b+ f2 j( @" f
- % e( [" w) n% _, r5 ^9 K
- uint8_t sData;+ `. z4 f: K6 h7 j) T
- uint8_t D1;
0 ?4 Y. f' p9 T' ^: e4 } - uint8_t D2;4 U' x( z2 E J% V
- uint8_t D3;8 V6 @ L& q% @) q6 ^
- uint8_t D4;
% V% o: c$ G9 l- n2 G - uint8_t D5;
3 L' }$ L5 O; [ - uint8_t D6;
2 U0 t2 v- }/ T/ Z5 R$ }2 L9 o - & L1 ^9 p: Y1 L* o
- float previous_error = 0;$ k* T8 c9 w3 R0 w& i Y9 {+ ~
- float integral = 0;
5 u4 s g; A! A. y) T- Y - float derivative = 0;, ?7 U6 f, p5 n, _7 m/ j( D: I3 E
- int Speed = 160;! R& o( p/ `; K7 F3 o: e
- float output;
; O3 \7 u) `! V0 d" M - $ Q$ w/ w* @$ e) B/ H
- byte Left;1 M/ Q, ^+ i: b! n% P3 N
- ( Y2 s% E- Q3 M' f/ l$ e$ z! S) A3 R& N
- void setup() 7 Y7 r- V3 v% U! B- Y+ H/ k* |
- {3 e& m7 J( l3 M$ d- ]# U
- //Set PWM 8KHz) h+ U0 ~+ W6 F6 c# J
- TCCR1A = _BV(WGM10);8 |% y! p. ^4 |" f
- TCCR1B = _BV(CS11) | _BV(WGM12);
; Q S- u) e# y - TCCR2A = _BV(WGM21) | _BV(WGM20);/ e" {( s5 x" G/ J' w
- TCCR2B = _BV(CS21);( n$ e$ e+ n# k& Q* w
- Serial.begin(9600);
1 C E/ v P6 X3 S6 Q7 Y - buzzer.setpin(45); c/ j/ d; D. z: L. @3 u
- }) J7 x# J2 H9 N2 d! t, i
2 x( T' C% X& l- void loop()% f' S! b9 _3 b5 M
- {& y, w, W2 q e& ]& D/ q
- while(!((lightsensor_1.read()) < (10)));
) r Z8 z/ M7 f; F; V1 S - buzzer.tone(1047, 500);
1 L% E$ u7 m5 Y7 G ]2 h - delay(500);
G; ?8 [% e4 q$ E- D - do* J' c$ A5 e' g9 i
- {
( i$ B6 T+ ~4 c! L - int Err = getErr();
5 p& X- h9 Z3 n j# d - if(D1 == 1)$ M z" p! A# @9 w4 c; l; \) o
- {
' D) K$ q8 D- U% d - Left = 1;( P7 l% k0 S0 j& h
- }
4 \2 [6 @0 A( }8 p O - if(D6 == 1)
% N4 d! c1 T6 Z' R - {; U; j; Z* O3 D
- Left = 0;8 U# B2 v# h" b
- }# B% r$ s) j. P/ a" t- M' F1 i
- if(Err == 99)
2 d( b1 {* F% i# u' e6 A) E! q - {& N( T k: R9 w; ?
- if(Left == 1)
! i. U! n# Z3 ?- N, x - {( R: C6 x% T0 B; K% s
- Speed -= 5;+ }0 ~# J# V& M1 Z
- moto(0,Speed);, m" f/ @$ ^- u0 H- y0 h
- do* g% G2 S( C2 t1 t* f6 R
- {: L! v4 O0 {* r: I h" a. J
- Err = getErr();0 G) ]" Y2 F" j7 {% X x2 c9 m L6 o
- }while((D1+D6) == 0);
8 j( {( _8 E' p( A6 A1 U7 d - }8 T# l) C4 @! B5 _; E& l
- else
4 @/ O- L4 b# b1 p9 N - {* s3 D8 p* Z, e' W" j& h) ?. u
- Speed -= 5;+ g$ ]9 ` z! V4 C- Z$ X& a
- moto(Speed,0); ) `: r$ P. Q' |" h
- do* C4 P. @% @' X8 s, C
- {; G8 D2 q2 t n) E. F+ D9 V
- Err = getErr();0 I( p% v0 u) `4 n! @8 P1 X2 `
- }while((D1+D6) == 0);# i4 l. W! p3 i8 u+ K. K
- }8 y. g/ v. m3 p
- }& a7 v( _5 g( c9 b, \7 ^7 X% q
- else
: Z3 a2 w5 j5 I! \3 A6 i7 g) n - {
# g" T+ N4 b4 { - if((Speed < 160) & (Err < 2)) Speed+=1; & j. K4 p. [1 j
- if((Speed > 100) & (Err > 2)) Speed-=2; & u8 M/ Y: ?4 \' p# S9 b
- integral = integral + Err;+ k5 Q0 I* T0 `+ H& p
- derivative = Err - previous_error;: w! m5 q" M% t5 f. x
- output = Kp*Err + Ki*integral + Kd*derivative;& z* r, ` J6 J! i
- moto(int(Speed-output),int(Speed+output));4 R( i& L" R7 @
- previous_error = Err;) ~" Q6 Y2 F$ @- x3 U( c+ f
- }! X7 ]! c1 Y. t5 v. g# w+ j3 l; W' R
- }while(!((lightsensor_2.read()) < (10)));1 ^$ @- N) W( c
- moto(0,0);
8 Q% H2 ~. t% N6 Y. m R - delay(500);
/ \4 n" b# Y1 w4 V" Y5 R& h - buzzer.tone(262, 500);
, d/ u& ?2 _: N# F1 k8 j; \1 a y - }9 `1 F' {2 m, ^$ l- _ H5 B
- ' G1 V2 N" m* n
- int getErr()& {3 B7 k' n! u% q3 b* C
- {
9 w4 m" f: O) v$ T# \3 k7 L - sData = linefollower.getValue();
/ }) J, q1 n! Y$ ]/ f, ^, N - D1 = ~(sData>>0)&1;* c1 o4 W% b- h" J. M
- D2 = ~(sData>>1)&1;% U. r- w2 k+ [5 @/ Q1 {
- D3 = ~(sData>>2)&1;
. k9 k( ^% j% ?1 b# O2 i0 Y - D4 = ~(sData>>3)&1;
6 r0 L" n2 g8 m. K( B# r8 h - D5 = ~(sData>>4)&1;9 K% e& ~9 V9 X. t
- D6 = ~(sData>>5)&1;6 y" a. \0 [- ]8 [
- int downD = D1+D2+D3+D4+D5+D6;. B: T3 O" e: V( P0 N. }! @
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
8 F- k( I/ }6 l - if(downD == 0)
0 ?- `+ o! k: I( |3 s) Q: C; D - {* M. ]8 |3 x7 A: t
- return 99;/ y: e6 B) v9 }8 x9 `: ?; i
- }
, B8 w+ s/ P7 H1 f - else
, K, P1 i9 T0 v. I2 t! K - {
* N- Y4 k/ A) d$ ?7 C b, c% \" s - return upD/downD;
$ J! F- P: U& q. e - }
' |2 W7 X- o- Q; D# j# n0 |# c - }
; Z+ h. q8 n& T+ L2 a/ x) h
2 D7 J! r0 r$ M- void moto(int Speed_L,int Speed_R)5 J% @/ T. ^7 v2 q) q8 \' v G
- {
; d9 @& B2 f6 C& \. E) l - Encoder_2.setMotorPwm(Speed_L);
2 _7 x' _" f5 x1 F# Y+ ` - Encoder_1.setMotorPwm(-Speed_R);& S( Y. a4 Z9 Q, {
- }
複製代碼
0 ~4 ^0 n* i# G* W' g5 r6 J1 y2 W+ v# zMeLineFollowerArray.cpp3 A* z7 m8 Y. P6 A% |4 s3 g7 q% \* _4 d
- #include "MeLineFollowerArray.h"" K3 d5 M9 L. \2 q0 ]7 X
- 9 R/ S" \" e( l9 f$ f7 a# c) X
- #ifdef ME_PORT_DEFINED
1 b2 C/ ?& g( D5 W - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
' s/ J% T0 \. t% a - {+ x6 U% Q1 ^9 s& G; L5 f4 V
! F0 v% m" ~6 C: G: `- }
* f: D* g2 e( f( ^/ o - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
8 A1 t1 U/ I$ d* N) X/ l - {1 v5 N0 C5 Y8 D; u
- _DataPin = mePort[port].s2;
0 o0 d! H% `! _ - pinMode(_DataPin, OUTPUT); 9 @3 o* P! m( E2 d
- digitalWrite(_DataPin, HIGH);
/ m4 t( r& C, R& ?7 i& z - }! Z3 `: a7 K) [ ~9 h* J6 R
- #else // ME_PORT_DEFINED0 V( v: E8 q2 e6 y( t
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)8 {3 H/ }) F. B" y
- {
. H! h8 Y5 d6 S# O4 X) a - _DataPin = pin;4 v8 b; [0 z5 ~$ L. u
- pinMode(_DataPin, OUTPUT); / Q3 [' w2 u; o/ F5 y
- digitalWrite(_DataPin, HIGH);' F! G7 |; q6 p! z+ X
- }
& {( O$ | y" a. m, M; u - #endif // ME_PORT_DEFINED& u9 M5 n# q0 v2 _5 v1 H' H7 P- y3 c
- 3 ]8 k/ q! v# ~6 i; d# N: h/ B
- ' ^8 e# }" H# N/ }: G8 s
- void MeLineFollowerArray::setpin(uint8_t pin)
& x! o( U/ D- t+ ^) D - {
8 y' e7 |8 Z( C$ `, B - _DataPin = pin;
, o2 c) h* C4 I* _ U& ]4 t - pinMode(_DataPin, OUTPUT); ( s a C4 y2 N' d& i/ `; J
- digitalWrite(_DataPin, HIGH);# ?9 l6 L$ @+ ]& `' d3 `
7 g; p' @; w( c2 h- #ifdef ME_PORT_DEFINED
+ H8 r: B" g' U, R, V2 ] - s2 = pin;# r! f: q% m B4 u
- #endif$ w6 o4 M* O; K
- }7 p! S% W: c8 y' Q4 S8 r1 k p+ [
) C* H: J x( l o! |- uint8_t MeLineFollowerArray::getValue()
3 G- T/ R0 I) g% N, q1 F - {
* j( v& V7 A+ v% ` - uint32_t LOW_level_read_time;& y3 s$ Y8 H# `5 ~& y& A$ F) f
- uint32_t HIGH_level_read_time;( P$ M! C+ p2 |+ h' d Q
- uint32_t time_out_flag;/ e/ _1 T0 |7 b
- uint8_t Sensor_Data[3];
% ?9 S3 c/ x/ _" E3 [ l - static uint8_t old_data = 0xff; L: N! z% T8 O" `( h
- - a+ n- m+ U" W9 o1 s. K
- pinMode(_DataPin, OUTPUT);
. Q9 I3 a0 L4 H9 Y: n - digitalWrite(_DataPin, LOW);
8 U( g+ h) q( j - delayMicroseconds(980);
- t( O7 I# `. I1 T0 e$ } - digitalWrite(_DataPin, HIGH);$ j( }3 Q/ ]' ?" Y3 S
% `# Z, N" z, K- pinMode(_DataPin, INPUT_PULLUP);
( t" r4 N2 P+ ]- s - delayMicroseconds(10);
0 W6 X; }( {$ ~9 W) A3 \# X9 ~( ?7 T - % L( B4 j; F4 |" v7 R- }
- time_out_flag = millis();% v3 [+ d# A. k! i
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 E( F) Y, j* j - ) [! o, F2 q# B3 N% |, }
- LOW_level_read_time = micros();
( m( A/ B0 a! n5 }) D* \; F, y - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
; s) f% N" X6 I; a* j - {
1 X" e6 E+ e! D% H$ @9 N4 a0 E# j" N - return 0xff;
! `* l6 ^9 T2 L! _5 o3 ^* r - }
+ p: A1 s! m: f: e# W2 I/ [
8 k+ H4 C3 N3 H2 u _/ [' L" K1 I0 p. G- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 c# r* ]# W1 c9 c% e8 o3 m - ) t) j2 F, S* D. `( J
- HIGH_level_read_time = micros();. u, x% O4 w1 f: G
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
/ h4 e5 p- C4 k! \0 x - # u5 l0 n1 ]. W* |1 {, S7 {7 \6 s! |7 ?. ]+ Z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 T- i: I8 H/ P, Q, r) Q - {
2 W! i/ R2 H8 Z0 `; U - return 0xff;8 g4 R3 D: z1 W, ]6 B5 u, h
- }
~1 m6 C2 x3 u9 f
1 F7 @4 R5 b8 H- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))3 \. t6 |- f1 g- Z
- {
% E' }# P8 `2 O: W - return 0xff;) l7 H1 w& J- F+ Q Z- Q
- }
9 ~% n: ?3 q' z% i$ B
/ L5 a2 {. o% w- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- O, B+ Y% ?% @# F8 M- X2 N/ E - LOW_level_read_time = micros();$ k- x3 l b% ]! J5 O. m3 k
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
$ h2 @0 w- c+ \9 l7 H! J - 7 t# Z5 a6 ~, g; C8 f; U; E
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! `* q- b! a' @2 @" K
- {
5 J' k5 `3 o; H0 F# M' `! k" g, N" \ - return 0xff;; H) B1 K# O% q$ C" i$ l1 ^' f
- }
+ D6 I9 _0 ]6 [! R1 C* r, Y
^* G' I1 @% [; P. j/ J- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
, ^- n) `# a/ i0 o - {7 \: k& J$ I. }/ O# h+ j
- return 0xff;3 [; j4 {3 }9 @% J& V
- }7 K7 y: u5 D: M5 f+ }) C
- . s. V7 g7 J8 R
- for(uint8_t k=0; k<3; k++)
- b, }5 V7 t( q - {' s$ ?$ w8 y k, x
- Sensor_Data[k] = 0x00;
& n( m! ^4 g1 P* M4 t6 \- }5 V
5 t' h6 L# ^* v T& @3 a3 t- for(uint8_t i=0;i<8;i++)8 F) @3 p+ C. ^/ h) \
- {+ Y/ D6 C" w. H2 f# |
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level: }% N% T- L% q; ^7 ~$ J2 y
- HIGH_level_read_time = micros();
: p6 [. t3 h4 C1 s8 X - LOW_level_read_time = micros() - LOW_level_read_time;/ G6 t2 p4 V0 G6 Z& [ K( c
- 1 @6 W' b# C, n. K
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )! y5 o; Q0 z, H- H' n; Y" N! r5 u- W
- {
9 o1 X0 [! F) D: L) r/ r - return 0xff;
/ a& F7 ^: s! A' f# j9 z- ^: h+ G - }
4 V6 g/ _# f( b - 1 C4 d5 y5 v4 Z- m9 z k$ w/ E3 V H9 M
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! l) R: F- @7 l9 `: H7 M) J; X# H - LOW_level_read_time = micros();# |% i7 {1 ~& r i i# R. g
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
& C" A, Y t! {- T8 m" L
! F! U. f5 J# Z! q- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1# G# s" o8 y* {/ G
- {+ H5 W/ w8 F6 L/ E2 @
- Sensor_Data[k] |= (0x80 >> i);
2 F0 C, b9 l) d6 U - }
/ X2 {+ ^4 u/ U7 M+ O& F& B - else if(HIGH_level_read_time >= 100)6 Y3 M$ ]# ^& f2 G
- {! D0 ^* K, t3 _) p& r
- return 0xff;, l- J" g- p4 `9 e" @4 u; ]
- }
) ?, Z. E& ?5 A* a$ f - * l3 G. q: ]; f$ B; N# I
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
+ z2 Y4 b$ m. `0 F - {
) \5 L! j) L' j. b4 i0 N& \; ^ - return 0xff; K& p! R3 q5 o1 Y6 `
- }8 e2 l. ^( y% j6 G
- }
% \/ v# v& M' G0 O* d# @* w - }- v0 k9 B/ I( f- t) w8 p9 s! r
4 p3 c6 q2 `. g! G! \( b! {- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level+ E: n7 [4 K* d* a! D ?" q
- HIGH_level_read_time = micros();" r; M% {- G' A7 F, [9 k3 Q$ t
- LOW_level_read_time = micros() - LOW_level_read_time;
* L' a. t# L9 w- P( I
C; J7 R; B4 D& E- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
3 T9 T0 G. a) A3 H }; d7 G) Y - {
; ~# r2 i' h) o' A/ `' f4 `2 g - return 0xff; X7 x. p) @4 R& d! [
- }
( w& Q0 @1 W8 q6 y& }) ~' ]9 x ^5 ] - 3 G7 R; d0 I4 B4 X
- pinMode(_DataPin, OUTPUT);; d2 q8 e: N" k3 R5 D
- digitalWrite(_DataPin, HIGH);5 p \" K, a. f5 e! K
& O$ Y% M! w2 h* O; X" @7 ?6 y; p- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))& B: j! a- T* t. d5 g
- {3 `8 k9 Y8 n( D# C6 H3 e. C/ \
- old_data = Sensor_Data[0];
7 a# I4 ^1 c; F6 F1 H! b9 R! f - return Sensor_Data[0];
$ `% u( R0 k u" K( ?) ~7 l2 I - }% E4 G* W& \* g
- else: P2 I4 K# Z) m: i) \
- {
1 @& K4 [: v% ~ r. Z. W - return old_data;
( L% n$ R3 \+ E - }9 f4 p+ y4 b& P2 i
- }. e: ~; |! ]8 _# a6 B8 \
複製代碼 , M3 G2 w O9 `* e. a) P
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
' }! W& U# K" H3 v
1 s9 f1 }5 j5 i7 {5 ^7 L
|
|