|
|
, U3 s, C ^8 m+ {
- #include <Arduino.h> H- X7 `4 T+ l0 R) `, q4 w
- #include <MeAuriga.h>
( X& G# Y1 ~$ d( U2 J; T - #include "MeLineFollowerArray.h"% a. ~4 z0 Z1 ~4 ?
- - `, T" {6 n4 m( O% b' i$ R
- MeEncoderOnBoard Encoder_1(SLOT1);
6 l# R" n' n- j. O$ f8 I8 j' A. V - MeEncoderOnBoard Encoder_2(SLOT2);
+ b7 G, m4 K8 m3 l2 \ - MeLightSensor lightsensor_1(12);
$ g- k) z) K3 i! R, ]6 [ - MeLightSensor lightsensor_2(11);7 i# F3 m6 x: ]& U# _+ k1 B4 h* \. C
- MeBuzzer buzzer;( W% c% V& K6 [
- MeLineFollowerArray linefollower(PORT_6);* \3 d- e* ^* m% L
, d7 I; m O0 U: _* \0 r/ ~: F2 g- #define Error1 1
7 \) [5 B5 @3 `8 n - #define Error2 2* v2 l5 K0 E+ k6 r: E& R. |6 e
- #define Error3 3
% P% R8 U( O9 _" t/ ^0 ]! J
' N' A3 H' b/ g. @4 m- #define Kp 15- Q" w1 E8 U8 n6 d# g& \
- #define Ki 0.15
8 G8 ]1 B8 F1 i9 \. T$ | - #define Kd 0.03
6 q3 G- U0 z( n
0 O5 i* k, _' W: ^. h; F- uint8_t sData;
0 f" \) k' |% X - uint8_t D1;- A2 r& T# _" y* [3 H
- uint8_t D2;9 Z. x! z) g) F @% d4 J
- uint8_t D3;/ l3 N0 j1 q/ t T* J; p
- uint8_t D4;
& D# G- Y. ?! [2 w- X1 |/ S - uint8_t D5;
- d2 {: J$ h# p$ P3 X0 X - uint8_t D6;
6 \+ b$ f V6 O4 v8 v) ^ - ! t$ b) V/ j, q4 D
- float previous_error = 0;
% X) R2 \. [* R) s- N - float integral = 0;
1 W* k7 h4 V3 O - float derivative = 0;7 Q1 Q4 ~4 p' w! e2 a; ?. k1 r
- int Speed = 160;$ S6 c$ Q" n* j# K( c5 o
- float output;& }; ?+ J( s! o: z( O0 i+ g. n- \+ H
6 j. n$ @4 F/ l4 i0 Z; i$ o: T- byte Left;. |% ?- E" J- P: d5 s$ Z$ J
- 5 k1 Y$ u* ]$ S. I' C- r
- void setup()
N5 Y @2 o% Q3 y0 P# \: {5 ? - {4 k4 W- j" j8 N4 d7 k% ?
- //Set PWM 8KHz' j5 |3 b- p" d3 Y& b
- TCCR1A = _BV(WGM10);
- S/ y6 e7 z0 [* W$ j - TCCR1B = _BV(CS11) | _BV(WGM12);& O5 J* ~( Y/ C1 s7 i/ b
- TCCR2A = _BV(WGM21) | _BV(WGM20);
9 W4 N) ^+ N2 P2 F; ? - TCCR2B = _BV(CS21);
) Z* _$ Y# [5 w/ Z; m - Serial.begin(9600);
) U. L) o* L" @% k$ x3 J' a# S - buzzer.setpin(45);
- U A# A( @; c! W/ \' b W - }
4 @* a7 g$ ]) m1 S7 J( o# h
4 D9 r3 G x8 X. W/ }" J- void loop()
% E' f! a8 }1 ]0 U( ] - {' ?6 a9 v% a, L! r6 \2 I* \5 L5 r
- while(!((lightsensor_1.read()) < (10)));
% o% w( I! `" ^/ s7 k - buzzer.tone(1047, 500);
- G! M2 F% i& y @5 ]8 { - delay(500);
0 E. T( c; {3 V& J3 d' u: x - do
. _3 A" D% {# K - {; p: ^0 d. a5 |( }9 B3 Q) m6 B3 |, \
- int Err = getErr();: g/ g; J% ^. \( _8 d+ F
- if(D1 == 1)" o* ]# m5 m4 U6 m g# n
- {
5 D) l$ z' d4 z& C) M2 w - Left = 1;; U) V0 S3 S7 {+ g! w! Q
- }1 @# S2 O+ J' v0 Q
- if(D6 == 1)/ ]& I3 r/ Z2 Q
- {% [+ Q) W- j n; |) i# \$ O
- Left = 0;' V6 V$ @: o% M2 U' j' f* x
- }, ?, w: i) m/ o4 X1 q9 Z
- if(Err == 99)) i$ ]: ~6 `4 u4 O+ j1 v
- {
8 v# B, [8 B( N. H/ q t# i/ E - if(Left == 1)
7 F7 p# E) }; A4 H) b9 @6 `* y - {
9 m) \- b$ L: c8 h - Speed -= 5;
' v% {8 {. S3 D - moto(0,Speed);
! m+ J4 |5 j! V3 z. Q2 ? - do* ~. k: Y0 p& y
- {5 \1 l) i! e B8 A$ H2 C; J1 b% p) I$ m
- Err = getErr();
# A4 ]; f9 a2 a; @, u4 ` - }while((D1+D6) == 0);2 {, d0 l, d1 W4 X3 |
- }: G, g7 j3 z0 X
- else
+ p& f+ l6 S. x6 r0 P1 C h# { - {
+ x, ~+ ]9 y, U - Speed -= 5;
* n1 R" p. Y+ o4 Q" L$ K: F - moto(Speed,0);
& j, J( ? {3 ~3 V) k - do, M' K- c: g4 U6 a) d
- {% }- l4 o& }: I' Y- R7 c* e: ?9 D; `
- Err = getErr();
' p/ k1 {% A) z$ z/ F% _( D. k - }while((D1+D6) == 0);
, S- N6 E' s% q9 \. N% W+ B" r& p - }
; v9 l( j& }( G - }
9 |7 O- m0 ?$ ]& C$ M Z - else8 [4 ?) T8 r; v& ~) h
- {
9 {' b3 A( t R+ I2 @- Y4 ^ - if((Speed < 160) & (Err < 2)) Speed+=1; 1 Z+ l! x6 k( ]9 s. r
- if((Speed > 100) & (Err > 2)) Speed-=2;
# _4 g- F$ @" k, A& v& ?$ w - integral = integral + Err;
, y' Z+ ~1 P: `7 D - derivative = Err - previous_error;3 L1 R, V9 S5 Y8 F2 c# p, h; M
- output = Kp*Err + Ki*integral + Kd*derivative;
8 q8 a# }: U' e; v - moto(int(Speed-output),int(Speed+output));
3 c+ |3 l1 I% H. Q' [ j8 M" C0 \ - previous_error = Err;
' {! y" X& Q% _1 w - }
: b9 I* T5 Q" u6 n - }while(!((lightsensor_2.read()) < (10)));/ [8 @# Q9 D8 _6 O e5 \
- moto(0,0);
) s' P' w, n0 t" ]. h - delay(500);: r7 S$ ]5 M* E: L; q7 s9 \5 k
- buzzer.tone(262, 500);3 Y6 V2 @+ {" w
- }# P2 b7 H( `1 w
- 7 R+ \4 y; f1 @; X/ v5 {4 }3 C- k
- int getErr()
o" m; ?& \# @/ j# h1 w& h - { : P& j3 b$ A( g v' l# t3 @/ T
- sData = linefollower.getValue();) C, H; i9 R6 F4 b2 r1 ]
- D1 = ~(sData>>0)&1;
% _. x" Q9 r- P* U - D2 = ~(sData>>1)&1;
# Y$ a5 e9 v0 v' W6 h - D3 = ~(sData>>2)&1;! r+ b# W4 r0 t# P
- D4 = ~(sData>>3)&1;
" L2 [' y+ O% V C% Y$ l - D5 = ~(sData>>4)&1;2 j# l1 i1 ]# \. Y9 K
- D6 = ~(sData>>5)&1;, h) b& S, k' c
- int downD = D1+D2+D3+D4+D5+D6;6 a( x! J! L1 A+ F7 d+ x$ ?
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
) C6 @/ r; e7 q/ R. J - if(downD == 0)
, E9 |. u" e$ x( }' @, ?/ _ - {
5 l( @1 P$ k/ \( q3 g$ e+ O+ G4 P - return 99;
" f$ ]% x$ i' j4 v - }" n5 g9 v* J0 u
- else
" ?# l6 e* r0 i& P# z - {% l( Z; c7 O" d/ V
- return upD/downD;9 o: n) o7 Y+ ]/ R0 V
- }# U4 A# V8 j1 `* l: J8 i7 v
- }
/ M9 l' q! {9 ^: z2 a - " l/ y" i1 j* V5 z
- void moto(int Speed_L,int Speed_R)+ B5 f4 ?) ^6 U& w
- {
4 |% v: X& d" D7 N% D - Encoder_2.setMotorPwm(Speed_L);
+ ^1 e2 g! |7 o- p( F$ m) E- B - Encoder_1.setMotorPwm(-Speed_R);
# ?8 e/ X3 }$ ~; K+ i - }
複製代碼
. W. {+ O/ y# a! h3 t/ t, U9 |MeLineFollowerArray.cpp
5 K6 `1 n; V2 ^1 q+ u0 w: J- #include "MeLineFollowerArray.h"1 v, y! l$ x$ F0 R$ D: T( @
- " L1 }1 Q; N8 W" G, P- k
- #ifdef ME_PORT_DEFINED) a! ~+ H2 z2 V# F3 {8 N4 Y7 f8 Q
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)! h! E( Z( t( ?' k6 H4 {( j
- {
1 `0 D7 a2 @0 ?9 \7 w9 z. i - ) R1 x5 K# v/ V1 x2 Z O/ Q
- }$ D5 E7 [1 l/ n; s
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
8 A, U% Q* f9 E* m V - {6 h+ b6 L j& ^
- _DataPin = mePort[port].s2;
- e0 R/ p U) k! v, B! b; U Y5 p - pinMode(_DataPin, OUTPUT);
1 O5 S4 \1 G1 B* y c, d1 { - digitalWrite(_DataPin, HIGH);
8 J7 L3 [/ r- X" S, k# E - }5 r' S9 {# W3 [/ ~. ]
- #else // ME_PORT_DEFINED2 u2 C5 a5 l0 ~* r
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)2 @* F) w: N2 I1 a- S& E
- {" D; I: j3 U2 q. y9 f" U
- _DataPin = pin;
4 `1 M9 X: d- ] R - pinMode(_DataPin, OUTPUT); # @ l: N) W3 y, w& P
- digitalWrite(_DataPin, HIGH);
* Y& A- [9 @& U, E; D( f9 J: I- k - }& {* n( Z) G% {/ p& V# b# W
- #endif // ME_PORT_DEFINED; P6 @5 \6 o3 r0 X9 v. E: f! ~( y
- " r6 x/ H# k/ C+ m
1 w0 ^( g! c( j" [- void MeLineFollowerArray::setpin(uint8_t pin)( ]! e$ r+ t/ O# L7 a K$ }
- {
6 V# ?; K* H" b" k. N4 J - _DataPin = pin;
9 d B0 r4 C: d - pinMode(_DataPin, OUTPUT); / T0 k' a8 x' }$ |' K
- digitalWrite(_DataPin, HIGH);; _5 e+ G3 n1 ~2 O( N8 P! e
- 5 L; Q9 _* R9 X# U9 F
- #ifdef ME_PORT_DEFINED
7 p& d; E) P! f0 u; k0 r4 | - s2 = pin;
$ @1 ~8 T1 \, `5 W3 u% y' W. I4 @ - #endif9 A; ?% P5 }* {0 S; l! E
- }/ x6 E8 i/ Y6 V% j. o' _: ^$ g# b
- * {2 u1 Z+ a3 f6 x; L- o
- uint8_t MeLineFollowerArray::getValue()
, M7 ]- _, Y+ x) \ ^ - {* T* F% M/ I4 Y; _
- uint32_t LOW_level_read_time;
9 c& H' b( u) o% f. P# Y9 I2 z - uint32_t HIGH_level_read_time;
. L& R/ b7 }3 s8 g: c5 x5 T - uint32_t time_out_flag;
$ {9 c* z1 O1 O - uint8_t Sensor_Data[3];
+ Z& a, b! {5 n2 y6 }; {% U - static uint8_t old_data = 0xff;& w, T/ l+ r; Z1 t+ M
# _+ j3 `& }3 S q+ M2 d7 s- pinMode(_DataPin, OUTPUT);
% h/ u0 ^! @! ] - digitalWrite(_DataPin, LOW);0 @3 Q# x3 a3 p7 H0 [% b2 A, P* Y$ r
- delayMicroseconds(980);) P! E. j0 n+ |9 M6 L4 \
- digitalWrite(_DataPin, HIGH);9 R+ Y1 [2 B6 K8 z p4 @. B; V f) C
" i0 D! e @" |; T; B- pinMode(_DataPin, INPUT_PULLUP);; z" Q! s- C( i6 H! |* Y) F5 ?
- delayMicroseconds(10);8 k0 _& E% ]( y% o! G: W
- ) j& b/ y3 y$ _) Q- N; B
- time_out_flag = millis();
4 q+ b; ?% [. }) U2 z - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. U, n+ W; R2 I7 | t; t
- `, i. E9 v; ?1 r# F4 M6 q0 h- LOW_level_read_time = micros();
4 x# u; a9 a4 S4 S: ?5 R1 ^ - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out. D3 @8 }$ n$ {
- {
+ m5 b. J6 i7 [4 F9 a, q9 M - return 0xff;
" @ K2 F& w) K# u( L8 Z, u - }
- D6 B9 }1 m7 [9 R0 R6 n - , N% `- r' y9 q, {1 ^+ ~
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* ^) K6 ?$ @% K' m) ?
+ [- _8 n) r) e6 Y( x- HIGH_level_read_time = micros();1 T5 d6 O2 _9 p B5 O
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level) C8 [3 O1 D. `7 P8 G/ H# Q
# H" ^1 q7 S" B1 S3 D1 d- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out) }. N2 l6 n$ U% j
- { e m/ a r1 D4 f, p& i
- return 0xff;
+ T) G% T3 S: a/ J4 K - }% d. y6 t7 j" \
- 8 e" ~7 u& `! L) Z- m# `6 `; k) D
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))$ ^+ j' O( g* D% V# Z
- {) a7 ]% H8 a. l9 e$ ?% M
- return 0xff;7 X5 M' c0 L. K
- }2 K+ j6 |9 @1 e
" ?$ ?$ C$ U. _- t- d. Z- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 P3 D, b) U- r4 ]$ V& e9 c - LOW_level_read_time = micros();4 m* z% N9 {% h- h6 X; O+ u
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level+ j: G; N4 u4 a! @0 `
5 ?" \/ N) ], t- r- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
; `1 g$ Y% k4 B( q - {
# e3 X* S e7 H2 i1 R - return 0xff;
, X1 j |% X1 K& \ - }: z3 [/ }# v4 S
- % z ` M! d/ c* r. J4 k- g0 w
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)), C7 r e1 [3 e) n5 L+ q+ `' f
- {
z7 C+ X; b; a4 X$ R - return 0xff;6 ]; }# U( Q) ~! b) |
- }+ ?) [3 ]! F) [" V R' f- ^
- + Z0 O, T) B/ c7 U& c
- for(uint8_t k=0; k<3; k++). w! s/ Q; d2 ?6 i/ s& f
- {
( M. H$ o) z8 D: d - Sensor_Data[k] = 0x00;
5 r; j2 ^5 ~; g - ; J* \0 Z7 C' ? [4 _
- for(uint8_t i=0;i<8;i++)$ `( z. N* L% R8 v. w4 c
- {- O/ } H X/ [8 ]( w" G- F+ _* G
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level8 r' `8 v' ^* v
- HIGH_level_read_time = micros();1 \5 B2 M) C7 Z( d' ?- E$ |6 M
- LOW_level_read_time = micros() - LOW_level_read_time;+ C6 O- V/ ^6 `" h+ s! E) T' o( d
- ' X0 ^ F; h9 p# P4 n7 C
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )& ` p) \% n" r. O1 a- i
- {
5 K# L0 V' h5 m( E5 ^! w% S+ @ - return 0xff;
% l; A; E' K0 q% k& v - }
' }. c. k/ D# v9 A! q
4 {6 j, X2 R7 j; f- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
1 E z" l* k. Y - LOW_level_read_time = micros();. H# i" ?+ M6 }( S+ P
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
. X* b8 f; B; k! H! T/ H
) V' F- b, `4 [0 z" }# H- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1; s: V6 u* Y# L) n
- {
* e+ C5 u& y4 a4 U- O - Sensor_Data[k] |= (0x80 >> i);
r T) A( A6 \. G/ V8 n - }, I D9 _6 }! u" f: p4 j- l
- else if(HIGH_level_read_time >= 100)
* c/ q1 L8 z, |" ? - {- z$ a, u; E( o2 T1 [) R. [
- return 0xff;
: l' Q4 { f- r: a% o - }% R9 |. R7 @# L1 u" m" l/ A; L
3 o7 g% k6 Y, N9 i2 ^- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)2 }* H/ D) ~/ @% |. z. h
- {
7 O. j1 q+ U1 X' z5 R$ P! [ - return 0xff;
0 W" N" A8 w; n( Q - }- l4 |( n/ h# E: y( T" L. |" r
- }
0 T) i/ Z% u1 s; T6 X- M: ~ - }, h0 a7 N9 M9 L7 l5 N
4 C/ E- j; }5 \: u6 C3 {- q- ?- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level a9 i' t9 [" I7 `7 i2 v
- HIGH_level_read_time = micros();3 _$ @0 v* Z( h0 O) U
- LOW_level_read_time = micros() - LOW_level_read_time;1 I' L' l( O, ^+ c" U5 k
/ N5 u; q1 t; g. M2 t. y# A- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
- w, P$ A0 b$ z. P+ I6 E3 M - {) h# T8 l$ \; E- k& z
- return 0xff;
8 Z' D9 }! l! p* ?, _ - }! f. Y% Q" w2 q& O
5 `% @3 e' V" k d& {- pinMode(_DataPin, OUTPUT);
, H2 A( \. s9 D$ ?% t - digitalWrite(_DataPin, HIGH);; `" ?. ^ e8 @: `
- 9 c P/ f+ L+ P
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))' L1 \. ] `: ?* v+ p. P7 E! d
- {4 W2 O( m7 G! V0 P2 {/ [7 y" F& o
- old_data = Sensor_Data[0];
. W# h+ E5 u( d- x( b+ o - return Sensor_Data[0];9 \9 C$ p( x9 f ]4 t6 L" N
- }
3 k/ v: m% ^$ @ t) ~ - else
$ G" p8 `5 t! J2 p. }$ V" L - {
% U& r, m- W+ V/ i7 e* y0 T - return old_data;3 z* t/ e& v; W: H& {
- }
; ~% w) a' g+ B - }6 E0 k3 H; F$ u. l, B3 L8 T- a. b8 M
複製代碼
E8 W' r8 O+ ]# `$ ^0 YMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
2 e5 E/ _7 y% K( U0 ^' u& Z
6 p; J5 u) F. @) _; M% ^
|
|