|
|
8 Q( f+ B5 w- K
- #include <Arduino.h>7 V& \0 D& ?! r3 m2 j( D& t
- #include <MeAuriga.h>* M2 Q% C) Y2 @
- #include "MeLineFollowerArray.h"% x' s3 v! w: n+ ?
- 1 q& a8 E, B/ p1 a
- MeEncoderOnBoard Encoder_1(SLOT1);! e' t% |! e% S( f' G# J2 x' \5 m `
- MeEncoderOnBoard Encoder_2(SLOT2);
1 I& B' ~5 e' x2 u3 @ - MeLightSensor lightsensor_1(12);
; Z4 u0 X$ p" Y2 F7 v - MeLightSensor lightsensor_2(11);4 D# m2 u* n+ V) S
- MeBuzzer buzzer;
$ K# [1 ]/ B4 b; O6 [& s! S - MeLineFollowerArray linefollower(PORT_6);6 V% i8 n+ Q4 @* b6 o" W
0 m# ^# D+ K7 {4 q- #define Error1 1
" t/ }% f" q) V+ c# v' T - #define Error2 2
" f) O% l& f( Q: @3 \ ~ - #define Error3 3) y4 p0 w$ w& s& I/ \
- + X. Q7 k! ~' n! d2 p; g
- #define Kp 151 x5 M6 o) o0 N' O
- #define Ki 0.15/ C c5 R# m& Y( j3 p# c
- #define Kd 0.03
$ v. k( s1 j# s' P! l% O
( Y; V7 H! a) x$ E% r- uint8_t sData;; k2 g, r; `& Z
- uint8_t D1;
$ k, a+ ^: ^; N, M. G- R1 } - uint8_t D2;
& x) X9 x# o4 v) L7 h9 q, H - uint8_t D3;& S. z& H1 M' e5 |4 S7 l: C
- uint8_t D4;0 G* Q: F4 Q& L5 e
- uint8_t D5;0 h8 N* Q5 H4 t% u; R- q
- uint8_t D6;
- M! N" t1 Q6 z, w: g! I - 9 C( A" M: v7 ?2 h
- float previous_error = 0;3 [; }! l9 Y; H
- float integral = 0;9 g! Y6 L" P" A6 T
- float derivative = 0;
( _$ s* B9 J0 s. w) W - int Speed = 160;
/ H8 ~0 O' L, g" w8 ? - float output;
0 `8 L1 f, M5 R" n0 Y7 T
6 a E" m/ y ~$ T7 i: ?0 `+ p- byte Left;8 v# S+ g, W/ E( p& u g: @
- - i: r \1 H- m) o% a( Q8 }
- void setup() - o0 [9 S5 G/ j$ I, ?
- {
7 n/ a7 V6 V! X! H - //Set PWM 8KHz9 e$ n& M& n- i3 \$ w% ~+ `' }
- TCCR1A = _BV(WGM10);
* d) {8 ~+ A# t" ], I9 J - TCCR1B = _BV(CS11) | _BV(WGM12);
* @% _8 L6 }* {6 P* ^ - TCCR2A = _BV(WGM21) | _BV(WGM20);! P8 t6 ~# y- K9 s. f
- TCCR2B = _BV(CS21);" T2 N2 O! ]% ~* ?+ j, T _9 V4 n
- Serial.begin(9600);4 |4 m9 s: S! Q G/ Q) n
- buzzer.setpin(45);
2 t9 c- h5 F0 R* T; m( y8 P9 D" z - }, Q/ u7 y3 Q+ B, f0 k" ?4 }
- $ M# ?9 I" g) R% j
- void loop()* ?1 l d3 J( |9 c3 v
- {
5 V; [7 h0 l6 Z9 o; { - while(!((lightsensor_1.read()) < (10)));
- `& b+ E% [9 }2 \ - buzzer.tone(1047, 500);/ p @+ v/ E7 b/ x
- delay(500);
4 ?- q' Q( [$ q1 O( ?9 z - do
& ~- V2 Z( D* O' p - {' ?' x1 O T% K
- int Err = getErr();% {9 M/ R% x! U5 u4 a
- if(D1 == 1)
$ Z+ T. Z2 j' l4 L - {) `& C8 Z1 Q5 r" U. W
- Left = 1;7 g% a. w+ A4 r% G5 F
- }% B3 O4 t+ E( N; f% W. u' N
- if(D6 == 1)
3 ~- j' F; ^6 B a - {
1 J9 [; v5 _; c L) g) S6 a! Z - Left = 0;
! N$ C0 v3 S, W* } - }
# [. W/ B! a8 T1 y+ j6 z7 Q4 X8 U - if(Err == 99)6 V0 U; \. D$ ]/ b5 ^- K) y. v
- {
/ t3 P5 z3 | a, D - if(Left == 1)8 @& p; X- }, |
- {
: P! s2 T6 F+ c% P p# { - Speed -= 5;
( x, w! P# p1 I& `. C' f0 T - moto(0,Speed);
$ ^/ T1 ?1 M$ f4 t$ J. @ - do! R" {/ [# K a; d N
- {% Y+ q. ?, x) P0 x# E0 ]* M
- Err = getErr();
/ T( x( p2 _* S: G: `/ T - }while((D1+D6) == 0);- k- q% N* R2 I& j; R
- }- C4 B6 W2 F/ E* `- Q5 E
- else
% J R) @$ E/ `7 e0 G( } - {
. T/ v" Y) e8 e9 z8 G - Speed -= 5;/ q8 Z8 k, E( P/ q: z3 M. m
- moto(Speed,0); ( Y% r0 _; b, m& E; n3 v
- do' }2 H1 q2 d/ D/ o/ \4 v$ f! y4 ~. ?
- {8 p* r! |+ p. R( K7 j
- Err = getErr();
- N# }" O* |$ H; O: {( v - }while((D1+D6) == 0);: i5 j( J# T$ s( [0 {' |) v# m7 G
- }; v5 X3 L5 y# H; H
- }
; t: H. U9 ^" g9 z% ^ - else
% l" \% P$ t1 W* ?) ^; _; E - {6 P7 k5 W% {+ s: B, X. _8 }
- if((Speed < 160) & (Err < 2)) Speed+=1;
( G3 G5 ?: a& W8 Q6 F - if((Speed > 100) & (Err > 2)) Speed-=2; % o2 b) }: w, t4 X* Z
- integral = integral + Err;- o, u5 K6 W# Z& P {9 [$ u2 z
- derivative = Err - previous_error;
' V$ e1 G* R+ I6 a' X" ?- K - output = Kp*Err + Ki*integral + Kd*derivative;; T7 A- R% h; J& W
- moto(int(Speed-output),int(Speed+output));9 Q- Q/ q9 Y0 Z5 ?. t
- previous_error = Err;" z! B8 u0 e. I# I( P* \( j
- }2 f" Y4 t+ P$ N. b0 ?( T) L) ?/ f
- }while(!((lightsensor_2.read()) < (10)));
( a! p. K! J: x - moto(0,0);
! ]% j7 D/ A8 e- q2 W - delay(500);
) f4 Q# M a/ V - buzzer.tone(262, 500);
) ? X% ~5 h) {7 ^( Z% ^4 C - }
$ ?( H4 ~7 \5 Z - 6 l; f3 E2 X1 Z/ O* ^$ _, a T
- int getErr()' S5 c4 q" r, Y6 l' n
- { 9 R& [5 W, ~# e# N9 r z
- sData = linefollower.getValue();
* x3 N5 I, X- H0 l) I+ J - D1 = ~(sData>>0)&1;8 [; s, X9 c+ L$ `( R* L/ O
- D2 = ~(sData>>1)&1;9 ^" v4 w1 n$ ~& _! m2 Y( D
- D3 = ~(sData>>2)&1;! X) G/ A) H$ R1 i9 m* a( @
- D4 = ~(sData>>3)&1;
8 B# H* \# X0 h7 p3 @ - D5 = ~(sData>>4)&1;
& `5 d6 r1 `& K& y# J - D6 = ~(sData>>5)&1;
! ^6 B Z! x% ]/ n! w& K - int downD = D1+D2+D3+D4+D5+D6;. y4 m5 f8 Z, e8 S# o e
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);0 v% [/ ~& N5 W" [" R F1 R
- if(downD == 0)2 ?4 t+ n \4 w9 F6 j8 ~* c$ ~" A/ o
- {' g/ \9 G+ }8 P6 P
- return 99;
$ O1 Q0 b: N$ B, C, u6 n - }
/ g" O3 F/ f7 L9 W1 |0 x: g+ V s - else
5 x! e0 L) j, h. B! S - {
, N5 Y. g# r' P, Z3 G/ d7 R - return upD/downD;+ i& \* M% Q$ G* `! h1 g5 C- F
- }; a$ A" i0 l0 e( G& T3 {
- }
; _: f% q5 b; o1 r - # f$ G; Y' j9 [8 _8 Q6 N, @
- void moto(int Speed_L,int Speed_R)
9 p$ m. |& ^8 @+ S0 ~& K - {
4 N0 P+ F+ x: p8 K3 v - Encoder_2.setMotorPwm(Speed_L);
9 V$ s4 X! y) v! Y& X9 @2 _ - Encoder_1.setMotorPwm(-Speed_R);5 j9 w, F) m/ G8 L2 P
- }
複製代碼 ) V, S- B; j5 B" U- A
MeLineFollowerArray.cpp) r$ F' S* C3 I) c) Y& W
- #include "MeLineFollowerArray.h"
5 S9 y, Q2 i$ C( y$ z
% s* U& _; I$ j( r' r" X- #ifdef ME_PORT_DEFINED4 q0 p" i, r ]0 D& R2 M# Q' T7 H+ ?
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
% {: f& j) p, P2 x2 w( c' T) x - {
9 o. n g, d! J# k1 m# y3 H& G) F
9 y S6 T# h$ g f. F- }
1 }: n/ V3 C3 G2 X9 E2 B$ ~' G7 s - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)0 I* d% |. K& w F1 G" f- z
- { m2 f0 F4 k' l
- _DataPin = mePort[port].s2;
: x7 X$ F" O& n2 j4 O - pinMode(_DataPin, OUTPUT); , T0 w- P2 d! [( i: i0 |: k7 t
- digitalWrite(_DataPin, HIGH);
/ R6 s" c: V* {8 V; n; x - }
" X, }1 S/ _' ?( K - #else // ME_PORT_DEFINED
1 e. Y- T* H1 f1 B0 W - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
5 Y) d4 ?+ V3 J! k E - {
8 E& n3 G- z/ }3 T3 g2 T - _DataPin = pin;
& }# W j& O% r3 p h1 | - pinMode(_DataPin, OUTPUT); ) b8 y& i1 g: @ w8 [
- digitalWrite(_DataPin, HIGH); ~3 ^+ [* n' z4 t
- }' Z% j9 k( \1 ?8 v y+ u o5 \
- #endif // ME_PORT_DEFINED2 U+ C" O! h, K ?3 Y. N+ `: U
- . k* P2 V# n0 y% K) C; ]
+ g- S: o* L3 M# D- void MeLineFollowerArray::setpin(uint8_t pin)! N9 }7 r+ F9 Z7 j
- {& S) S+ m( U: O0 h+ I8 |
- _DataPin = pin;6 R) u: G1 g9 y* q( G1 ]
- pinMode(_DataPin, OUTPUT);
# b$ F; m9 {- |' r, _; X# r - digitalWrite(_DataPin, HIGH);- z5 O; u1 n* z [5 [0 h
- # j: N) Y/ J6 K3 @3 d! z
- #ifdef ME_PORT_DEFINED2 a9 P+ K% K: ^' Z+ Y; ^' n4 I3 ]
- s2 = pin;& Z* D4 x7 g' h9 w
- #endif8 [ K4 U! P/ a; C* f: b# w
- }) `8 D. r5 r, D5 i$ D
- 3 Q5 o0 J# z# `0 W
- uint8_t MeLineFollowerArray::getValue()3 `; x3 U F, n. [3 L' g
- {
. s1 f+ g0 F H - uint32_t LOW_level_read_time;" Z3 e z& U5 z; B, n: d
- uint32_t HIGH_level_read_time;$ }; g$ E; a+ s l
- uint32_t time_out_flag;
: q2 Z# H& d$ v+ Z1 Q' ~( P - uint8_t Sensor_Data[3];$ m5 f/ O" I: a$ C3 N% O- B2 a
- static uint8_t old_data = 0xff;
5 l3 j6 y* X/ p7 k0 |2 \8 `' I6 b
! v0 @0 k/ L. g( O% G# s% [- pinMode(_DataPin, OUTPUT);% T1 @# O& G8 j. ]
- digitalWrite(_DataPin, LOW);
: C; Y& Q) H6 K- U! o - delayMicroseconds(980); d( j$ V3 m F5 ]$ |& r
- digitalWrite(_DataPin, HIGH);
0 {! G2 q6 I% _9 R; U! `
( V( F( g7 W- Q% i7 N0 }- pinMode(_DataPin, INPUT_PULLUP);
! F6 z* W: w% L: h6 e - delayMicroseconds(10);
' l& B' C% F8 a3 w- e4 C8 y - & p% s" Z% _! U; k- U
- time_out_flag = millis();
, y: u9 J; C# _2 p* v+ m - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( p" ^, j+ G& v( ^ C6 e/ p
- ( E, \' e7 R/ A
- LOW_level_read_time = micros();
5 z& i+ |% J0 p8 ^$ m5 P - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ }" e# P. J% j. b% P+ p A
- {% _ _. ^; K' M
- return 0xff;
+ p2 q7 f& ^! p3 M: V4 L. I - }
: A# f+ X7 U! i! |# R - ! u+ Z& l/ w# g1 Q1 S
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ s6 M- H9 v; n
6 n5 r# i$ J) c/ ~- HIGH_level_read_time = micros();
+ Z& D! x7 |( {, y* Q, i - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
5 C3 T. p+ Y: F* V4 A# z' e7 @
9 _. M: d0 k/ r3 E( i& I' J- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
( u1 |) W, ^% T( W! t - {
s$ |' [$ E/ p- c4 }1 z X - return 0xff;5 C: U5 N2 b+ w0 d
- }( P0 \0 b) g9 ]/ h- r: i# D
, p5 Q" E' E b6 ?- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
! b" }# p; \4 ]9 P - {
- X* C. U5 o- q7 Y( _) V2 |7 n, j - return 0xff;4 o8 g! E' p F5 U; {/ R
- }6 h( Y% C6 S# p; N/ j3 J
5 @2 A3 T3 O+ C) ]9 a# `- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: P# f& O1 [6 ?: D t/ Q
- LOW_level_read_time = micros();
" \; k9 M* t1 w/ ~* V( b1 U - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
) D& B6 j6 X- E* u+ a S) B
) ?' K5 q: s! V$ X% l: Z- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out9 W2 H& j: v) b& l1 x% e5 |/ c9 a5 P
- {
' n8 l) E7 G' \2 V" }/ C - return 0xff;) G. u6 ~1 p D9 s! s" X
- }% j3 [8 U0 ^' p! f- @5 @
2 C7 o3 P. K% V+ ~- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))) Y' z6 Z* d9 y! Q, {2 I- g
- {# g. p' a1 j0 S4 f
- return 0xff;( w7 L# y! f L' n& Q5 z9 S
- }$ M# o9 R7 d% p4 d2 f
- 2 H* X5 ?- C$ X- Y$ U
- for(uint8_t k=0; k<3; k++). D [7 B8 L5 s, c/ x- U
- {/ f" d2 n5 j* u+ ^' N7 D' ]7 w( c
- Sensor_Data[k] = 0x00;& Q% k3 S) X6 u( @+ v
- 4 E8 |$ p7 V, V0 `: Q
- for(uint8_t i=0;i<8;i++)
+ J4 R0 g4 c( R o - {
% P8 I' G- B# F* I8 t# f - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level* s1 g+ A S5 y z! j" V
- HIGH_level_read_time = micros();7 b m2 U* c0 {- c
- LOW_level_read_time = micros() - LOW_level_read_time;8 |' A8 n- I& D: c1 ~: L
- ; b: W6 O9 m- W( E6 Q9 T/ }
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
0 H1 [$ D& Z; x, L" @: @) p4 ` - {
9 z6 [# ?- X0 k8 l2 Z* a - return 0xff;
! M& y* v- S& a0 ` - }/ q* F3 Y. C: f" i7 V
- 7 A. z% y# t9 z3 b5 A3 Z" ?
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); j$ G! D; ^2 Y7 q
- LOW_level_read_time = micros();
3 h d o& N$ x- {' Q - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level2 z" ^2 A# `* {- j) [6 y; Z/ L1 W. z
- . c3 S _! j! U0 U. }
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
( g% Y# @6 y/ G0 _( [ - {5 Q# y: `2 `0 H. {% F1 x
- Sensor_Data[k] |= (0x80 >> i);
3 F; W, \$ n! U* C& R9 f3 \ - }
0 m: i3 c" m3 n# g% c) H8 _ - else if(HIGH_level_read_time >= 100)( a7 Q3 U3 y4 W( j8 C9 w: }$ v; l
- {& P! L! Y7 Q) u; `# F2 E
- return 0xff;
$ j: J' F) i' @/ o - }
' |$ H0 I8 C* M" F; g - $ g+ B+ B; g, B
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
# T# o u0 k- o# D, y - {8 M) K5 B% @. {1 q; b0 U. U
- return 0xff;: i/ I9 N" c# d$ B% e
- }
! t+ y f6 {% v! [' K/ M, @( p - }
- F. y) q; N& R: ^* S/ G' _6 ] - }/ ^6 i/ F+ G' T" f: U/ W
- + m$ f& a; T; i
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
6 i. d# a3 l# Q. \ - HIGH_level_read_time = micros();
' G. |6 a" n% T8 j1 p" [3 j3 O; E - LOW_level_read_time = micros() - LOW_level_read_time;! M4 X: f, ^' r+ K( H; i/ Y0 h3 `
8 l2 a9 b0 O. ]8 F- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
) g% r! b6 k1 j7 `4 ` - {
% K4 I7 `; {# N1 q% ~! k8 { - return 0xff;: ~* r2 R S/ a9 X4 f* g0 R
- }
' O! P5 ?9 v+ D1 b2 P6 ?' ~4 H
b! V3 P, _) i& p- pinMode(_DataPin, OUTPUT);
& A7 `; B0 v9 Q! l - digitalWrite(_DataPin, HIGH);
$ E5 W% z# W) \5 f
/ I7 i9 p3 j2 J9 I7 Y5 p+ z- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0]))). \. c6 R4 h9 [, \9 A
- {1 X" J1 }3 X! I, x5 \2 y
- old_data = Sensor_Data[0];3 b' D: }; O% ?" A
- return Sensor_Data[0];* i2 T( W2 D- E5 L0 [* U3 o
- }: R0 Y) x/ w7 A
- else; S* v; R O" @2 I3 I( k
- {
, u& q. G9 h4 ^ u - return old_data;) D* w$ H( Z5 a9 |
- }
0 T+ v; \9 y/ j% G- { - }
4 R+ o$ ~! J' V- i
複製代碼 $ m7 n( c: x# Q
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
z( q6 N) E1 g( i: s# H6 D C+ W- @
|
|