|
% j2 _7 b5 J9 g X% w- #include <Arduino.h>
7 q. O, u' g+ P( H8 \" z - #include <MeAuriga.h>
) Y9 g2 ?3 o7 _) n- m0 [9 Z8 j - #include "MeLineFollowerArray.h"
4 b- D4 N+ v2 l t$ l - . K& u$ O/ \. d
- MeEncoderOnBoard Encoder_1(SLOT1);
& Q0 A$ ~/ Z6 p q' J, \9 E - MeEncoderOnBoard Encoder_2(SLOT2);
( V, _+ A4 F, b( ]. u- A. d9 Q* n - MeLightSensor lightsensor_1(12);
# u4 {9 v/ Y7 O3 u7 w E - MeLightSensor lightsensor_2(11);- r1 q8 d/ V$ e9 \0 }1 Y7 Q4 D
- MeBuzzer buzzer;0 k2 w/ i- g# U7 x& X; F: L
- MeLineFollowerArray linefollower(PORT_6);: w6 ^" {0 S3 E& R, p
8 d+ @+ |$ ]* v6 L- #define Error1 1% f0 I- c* F* B
- #define Error2 2
4 b: g$ J y f4 X0 @) @: r1 ] - #define Error3 37 w" x# M/ F" _. ?
" R; R P1 N! [: K& [- #define Kp 15
0 ~6 u) O, |' f5 D - #define Ki 0.15
: t4 `. J P1 _" n! j% _" i - #define Kd 0.03
4 w, s+ A/ e& F
8 }+ J, `+ I7 K0 F# }- uint8_t sData;% U' r: c- c4 ]. q$ a, C& C
- uint8_t D1;
' w O! D' |3 ^' I0 h - uint8_t D2;9 c. X/ f2 _9 F( p
- uint8_t D3;: o+ h& G; e$ O/ Y- g! P
- uint8_t D4;8 V. c* |3 o) h3 Z/ F" t
- uint8_t D5;9 f! r2 ?$ Y. z( k- |
- uint8_t D6; D8 `: ?# R( F; ^
- 7 p ?7 T& }$ H# O% E# X3 K
- float previous_error = 0;- t% t* E; e2 }5 E
- float integral = 0;
$ d9 i8 o" q* x; c - float derivative = 0;, k" p: r' H" k: O) t' I! [* p
- int Speed = 160;3 E$ w5 s$ g7 n6 M3 n0 ]9 g
- float output;
) N3 Z6 v0 u% a - 0 L' u5 T: G( P7 {
- byte Left;
( c _+ A2 ?4 z h# n - $ L, Y! w: Z0 p" y" p' i
- void setup()
& [% @$ T& i# A" \ - {
7 y8 j4 H2 i2 p- B+ }1 |! \# T - //Set PWM 8KHz" \8 k. m, @, P( Y1 [: [* U9 i
- TCCR1A = _BV(WGM10);/ j. G; Q# F0 P9 p \2 A) I
- TCCR1B = _BV(CS11) | _BV(WGM12);/ }5 t* Z1 q4 A% r! }
- TCCR2A = _BV(WGM21) | _BV(WGM20);
+ H- N3 y4 p4 _; ^( z - TCCR2B = _BV(CS21);
~' Z0 P5 q! }2 S% v - Serial.begin(9600);1 @* T9 {. j; ~& i- W$ t( r- n
- buzzer.setpin(45);
& F' E" ]5 L1 s; Z3 T; d1 N - }1 L& r( t( r$ E. T, K$ A
9 B" v: c1 G6 q! a( G1 m) `2 _/ |- void loop()
8 x% E3 p' p* u0 J - {% Q" y. B6 F" E9 F) s {
- while(!((lightsensor_1.read()) < (10)));
: _! ~- [) Y& t* L, ~, [8 Z - buzzer.tone(1047, 500);# @4 { |! k5 {, j8 r5 o( o( @7 e+ F9 F
- delay(500);
) Q9 I3 k6 a3 L. f, c+ w8 `, p - do6 w; x6 H: _+ t% [
- {1 G; t/ F# h- ]( G/ `% O
- int Err = getErr();0 ]" S% S4 q0 A0 z4 N5 h3 I& I. ~6 V
- if(D1 == 1). l7 p$ i* l; T
- {
9 f) Q1 |$ k# O0 T( D$ [7 ~ - Left = 1;
( V. F$ ~7 {; g" t! j - }
# C3 U/ N- B w, m! U" s - if(D6 == 1) ]6 }4 P' i! m. G/ \( h
- {$ \1 j9 h: s9 ~% J
- Left = 0;
' Z7 u1 y; w6 ^( z1 C - }
! f$ A2 ~$ W# p. Y" ] - if(Err == 99)! b4 [9 W1 C; V9 f; }! {
- {' s( f& W! J2 M1 Z j$ }' c/ W( u
- if(Left == 1)
( G5 z7 G, ~1 q - {
# ~, S8 E) c, b* C+ v7 u D - Speed -= 5; ?2 ^2 Y& |1 h% s/ x- @) x/ A; [
- moto(0,Speed);; t9 d; ^$ U) y+ \- ^
- do6 V6 q; U k1 n1 ?
- {
% z) y! E C- j2 v6 z - Err = getErr();
& v7 [1 }# ]2 C - }while((D1+D6) == 0);4 \ ~" F5 y; ~( h, }& e6 D, X
- }
" j' T; [* l! B - else
1 R; u+ W* P3 l1 B9 y. i. x - {$ K: ~) J% Y8 U! i# {
- Speed -= 5;
1 |- @: U0 X, f9 ? - moto(Speed,0); 0 K5 g. D- v# O$ y8 @
- do
& B- l: T+ i; S- s* w: { - {
7 E% c" ?- ~4 C: L- F - Err = getErr();
6 k! e" ?5 c- X1 p8 N& }% i) C9 o - }while((D1+D6) == 0);
9 z5 x* s: k7 E% L0 w2 P - }
. N6 v, _ p" p; x) ]' m - }: \5 V* n, [3 f: v
- else
# Q, B& i% g7 A* T; W$ l - {
3 E1 y9 i- q9 y( C/ s) E4 H0 i - if((Speed < 160) & (Err < 2)) Speed+=1;
6 e+ {0 J7 z8 z# C8 ^; ]1 W - if((Speed > 100) & (Err > 2)) Speed-=2; ( |% c* _; E! O( Z
- integral = integral + Err;
U/ c# J$ i& P7 V7 x2 e - derivative = Err - previous_error;
# H K: O, U, |# q3 Y% B - output = Kp*Err + Ki*integral + Kd*derivative;
6 t/ Z8 K: e7 ^6 r - moto(int(Speed-output),int(Speed+output));
0 [9 W1 j2 @# k1 n - previous_error = Err;$ g; u5 ]/ j) x' z# |
- }6 C# m/ y# E+ G, |* e
- }while(!((lightsensor_2.read()) < (10)));7 L x# C- u( r. {# O
- moto(0,0);
8 v& ]6 t0 \3 @6 Y3 i* | - delay(500);
# D" E" p3 N9 }. R7 { - buzzer.tone(262, 500);! M4 |, v, J, ]* o$ Q
- }9 E! d" Y8 r9 T8 l
- 3 S' v7 r# v7 C/ L- L
- int getErr()
7 h B+ G! C+ F0 F9 Z% B - {
' Y9 {2 s) R: c. ^ - sData = linefollower.getValue();
+ A' i- b3 W! Z& Q - D1 = ~(sData>>0)&1;
9 f! Y$ L2 s2 ` x1 t5 I - D2 = ~(sData>>1)&1;1 x/ g+ Z q5 K* d" W; x
- D3 = ~(sData>>2)&1;% p0 y' f' I1 ^8 B
- D4 = ~(sData>>3)&1;
) v$ E# e$ s! H9 P - D5 = ~(sData>>4)&1;
% p! u! d C7 {" |6 p - D6 = ~(sData>>5)&1;
$ U7 T& h/ u0 u - int downD = D1+D2+D3+D4+D5+D6;0 p: J+ Q5 o3 m5 \2 x
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
A$ t" M& _8 J \+ P8 X7 u - if(downD == 0)
: r6 _7 s# G$ A2 R' c3 ]% b - {8 a% F/ j: @1 Q" M1 @
- return 99;
5 x' i" J" s( k' U! h - }
, v+ n) Z0 N/ B$ ?0 ], d: [9 W - else
, T8 O5 X% t/ D. J, O. h9 e - {
# z6 D' e; w& G7 d3 _ - return upD/downD;
v. c$ S: I2 J% q* e - }
# y1 U! ?/ f+ U6 w }6 q - }- W. R+ V1 d* P6 B( D e- z
- ( {: ^4 T' T- L: L4 @' ]
- void moto(int Speed_L,int Speed_R) W0 @, p0 Z. d5 o& G0 N% C7 |: l" L
- {
1 ^& G' E5 j4 B* K* X8 W5 B9 l5 \ - Encoder_2.setMotorPwm(Speed_L);
- _" m% e/ `% P+ b; ^, Y - Encoder_1.setMotorPwm(-Speed_R);+ `- Y5 R5 g6 ^! i- k$ f# I
- }
複製代碼 ' C% L1 [1 _1 U& q) J+ ~$ k
MeLineFollowerArray.cpp
( U L& c5 U! \' q- #include "MeLineFollowerArray.h"
4 k) [' r+ M6 y) b$ N' k+ F5 n% g - 8 Y! b- } Q# @3 W9 }
- #ifdef ME_PORT_DEFINED1 [" V* _, M+ p2 e: P. T
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
! C# g% `! d, S& k% J' M f' u& G - {
1 N+ Z: I1 [0 c6 I7 H* U
$ z& M! s" O6 Y7 `0 {: |& S- }: Q9 z8 I, l- {: H
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
1 \3 d0 v' J3 u) f/ F0 w A - {
; `! N' B/ q5 {; s. a - _DataPin = mePort[port].s2;7 {$ {3 d; ~* t0 v8 H1 U
- pinMode(_DataPin, OUTPUT); 9 r" `0 e8 D8 B7 C. R
- digitalWrite(_DataPin, HIGH);
/ Y i4 m- o4 f: ]3 y: [% S8 w - }' S4 A3 w9 `- ~
- #else // ME_PORT_DEFINED
" c/ H1 W( R/ r5 n7 g- F" O6 c& \ - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
* |- U; o# A- H: H+ r - {$ s7 R! k' {& O* ]! }
- _DataPin = pin;4 Y, F! c) }- r
- pinMode(_DataPin, OUTPUT);
q' J: ~$ `$ l5 J, s8 ? - digitalWrite(_DataPin, HIGH);
2 ^8 ~+ h" n8 \* N8 e - }: a, |0 L! P8 s; t, g' {
- #endif // ME_PORT_DEFINED
2 h: F- m0 x3 }: z
$ U# a+ f3 b, ^/ g: v1 p
, M# J* i7 p3 j$ r7 z1 x+ J- void MeLineFollowerArray::setpin(uint8_t pin)
. \7 I5 n( h+ s+ s8 Y - {
0 Y; F0 v# s: z: A - _DataPin = pin;
" |* Z, R$ x* G# I - pinMode(_DataPin, OUTPUT);
0 Y1 f, P- r- a+ m$ u - digitalWrite(_DataPin, HIGH);+ {3 V. ?# x v5 I$ @
- 7 X: ^. o! T# H f( f
- #ifdef ME_PORT_DEFINED
& d+ }# _1 |& j* T c! j- ~+ Y: J - s2 = pin;+ @5 v$ ~: l6 ~! q8 m) I
- #endif7 d$ l" V+ o/ g
- }
& B- G" d8 z8 a& R( g& e4 b& e
, v4 l2 t2 c2 a% A3 l. z2 d- uint8_t MeLineFollowerArray::getValue(), n5 x! N- J' ]; ~/ C" t9 L
- {' x8 W" Q% v; h) x
- uint32_t LOW_level_read_time;6 v& [5 @& _4 q- |
- uint32_t HIGH_level_read_time;( C7 E) y3 Y( T4 I& N
- uint32_t time_out_flag;
9 S# |( h. @, z3 ?4 S2 e, f - uint8_t Sensor_Data[3];
# {4 { Y0 h9 E - static uint8_t old_data = 0xff; Z4 h, @) [7 R" N. y
- $ |" x& G0 P# j. f" S1 e
- pinMode(_DataPin, OUTPUT);
& P# F0 I+ u# T! U' Q! r2 d { - digitalWrite(_DataPin, LOW);
* D w, q; j/ x( r7 S - delayMicroseconds(980);9 j+ D- X/ `# B/ P: p8 N
- digitalWrite(_DataPin, HIGH);# A2 Q, C; L: o, p
- U4 j7 \) L N- pinMode(_DataPin, INPUT_PULLUP);
& z6 f8 i! W8 J: n' B5 w: A - delayMicroseconds(10);3 E8 i- h' J: k, z
- . ^8 ^9 ?7 b+ |% |% k9 [$ c" @, X
- time_out_flag = millis();2 w, B+ J' c8 i. C
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 A6 }, O# K$ u; ^
* L/ z* {0 r p! a# f9 E" _9 [2 h- LOW_level_read_time = micros();& {. u, I" t/ d
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out% x0 X7 L* y9 T. q
- {; P# V) {9 V5 s, \( V- }4 y6 V
- return 0xff;1 i5 K: Y5 b/ c j( g
- }- f+ f K% ` y- r
- ; N: V; O" g. D1 ] ?
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
0 m6 N/ C! }( y/ @& D3 ^ - : ~9 ?+ @7 t6 z" Z4 u# t
- HIGH_level_read_time = micros();+ q" P: Q* N& c$ t4 p! W
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level1 |" K' W. p! H* a9 Z5 N
- 1 _" u5 M6 v. y. F4 B
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
. v' I" c) K/ n* j& t" U! e. Q - {
& I; }0 s& E {. u - return 0xff; l; P g$ b+ y1 J, K
- }2 {2 f& E2 s2 R8 E
) |. M: q+ [! [+ G- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))$ I$ S$ ~" y- I0 H" k( e0 E
- {
; t& r# d8 Q3 _& z! {- G% R" e+ ` - return 0xff;& t& X9 o/ y0 @, o( @0 J$ K( {
- }& c- p& i% N1 I3 W& K7 Y* \
- $ r N' f! L8 V4 p) n0 |
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );6 W& I/ E R2 Q) B
- LOW_level_read_time = micros();' [0 T6 `' E7 a- {! m' U; M
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level' Z8 q. V/ Y: G
" R& ~$ S4 V; B- s( ^: p- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out9 W! k- P) j+ M: j* T ~2 h9 p
- {# _( T8 J% N# F( H+ F
- return 0xff;* e. A4 r7 X$ {1 c& ~9 ?" N
- }9 W8 B4 X; v, D6 j8 t
P( Q. i+ q, S2 c6 D/ `! _- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
[* |: q" K& U/ X, ]4 K - {
. P, Z/ a7 E: T; I7 @0 @: Z& Y" B - return 0xff;
; J! D- ~1 K- g - }
9 `1 _! C- s2 U. X5 I3 o, N1 {0 K& ] - 5 ] \9 e: {8 ?7 `, ]2 X
- for(uint8_t k=0; k<3; k++)
2 @4 S" F0 B V! F% p4 F& R - {, b8 D% e, j2 l" V( u) t, O' k
- Sensor_Data[k] = 0x00;: r; Q- h, A6 j- Z6 N
" |- }' l( o' `& l1 Z; N- for(uint8_t i=0;i<8;i++)
/ e3 q( y& M) r# `8 p8 j$ Q - {
& _' q- `8 u5 v - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level6 H4 L0 N( `) D" J \
- HIGH_level_read_time = micros();
% U- H1 M2 ^2 D4 u; Z$ c) Y" i# \- D* t - LOW_level_read_time = micros() - LOW_level_read_time;* g# k/ c$ u& g; ^" X; g
- ) }4 S( h; W. H$ K; V
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
( x2 W. O2 @0 v - {
1 ~5 Y# }) ~ r& I$ o, i - return 0xff;- m. R+ b: c9 E3 ^* F" ^ E; D
- }
3 g, d5 ~ S- P( n& U - 8 V: o/ Q, W0 E" |/ b1 s
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
* b9 u+ a9 r& u' B0 L& u" i - LOW_level_read_time = micros();
1 M) w1 I9 j; L* O - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level6 C5 {5 q4 }* K; Q
$ I# x! e: D: o; F. _# P) @- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
3 H5 a6 D( R$ L. p+ R0 w+ D - {
: x6 g; F$ Q: R8 z* m5 ~ - Sensor_Data[k] |= (0x80 >> i);) n0 L( J e6 d; X, s$ l
- }
' d% O" `! m' D0 l" B7 ~8 f8 V - else if(HIGH_level_read_time >= 100)& ?4 t8 G* [ N( Z
- {
; l [7 z4 e( S+ U, B/ l+ D* u - return 0xff;
7 m: ?: [, N/ h Q" a0 F - }
% p8 }; U9 u$ x- ] - * H; \+ F; m. K- o8 N% l
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)1 y2 f# j8 s) n1 ^. N$ D# N8 Y
- {3 f. d3 n$ Y+ ]8 ^
- return 0xff;
+ h; N" ?/ F$ |; ~# ^ - }
_4 I: P2 p) `4 h8 ~8 U. p - }
8 c4 u" g" ]$ ~6 Y) G - }
5 i8 s8 B. K4 W# N; e7 C, C
( m% k, z: J+ K8 _- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
4 l9 a/ I8 O& q1 `2 t Q - HIGH_level_read_time = micros(); ~/ N* a9 g' s* t% `: `
- LOW_level_read_time = micros() - LOW_level_read_time;# b$ _- a. \1 V, R L$ a$ h
" }3 o8 t: b! x& N0 F- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
6 o( [$ U% y. Z; n - {; S6 t# K, H3 z" Y( v% p
- return 0xff;. \4 g l/ @. _' R _
- }
" Y6 c& D4 M- u4 ^4 G! ] - 7 D" p; B8 j/ I
- pinMode(_DataPin, OUTPUT);
% W! Y- d' h+ b/ o8 m9 C - digitalWrite(_DataPin, HIGH);
% e5 V" c" g3 v$ u/ N! @ - 4 [% K1 Q9 w- z) _1 q! d$ x$ p
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))- o4 ?9 n) @8 W
- {
0 \4 y% [; R5 @: z" g - old_data = Sensor_Data[0];2 g/ x- W# x B9 z3 V/ h
- return Sensor_Data[0];8 K. P# f; D& h1 U; O, P, l# `; B. D
- }
! m1 `; s+ }5 l( g - else
$ K! g' h$ Z! n! }( w - {# O2 l! |1 ?2 P* F* P2 u5 A
- return old_data;$ c$ [/ d3 c: X) E6 G( v
- }
3 P" ~; ~8 ]6 Y3 F% d - }( M! K# t$ o0 B' u) x( ~
複製代碼 4 z" D3 @ u+ V9 m% x4 h: M
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
. m/ t1 d6 K& a1 n! D4 V# u3 A9 i& j* H; L- I7 r
|
|