|
|
( |* j% I6 C3 G$ K- #include <Arduino.h>+ R2 ]. _& E- v& @6 o$ E: A' b
- #include <MeAuriga.h>3 o3 S+ \7 S( x
- #include "MeLineFollowerArray.h"
( f2 ^% Y9 X3 t: ~0 j) K# E* \ - 7 ?- d6 V C% m! t6 I
- MeEncoderOnBoard Encoder_1(SLOT1);
! Y: q9 Z0 u1 k' A) f% E$ W - MeEncoderOnBoard Encoder_2(SLOT2);
. C" [$ P4 v: T. _ - MeLightSensor lightsensor_1(12);
4 S k# _8 T4 z1 Z+ |: o% C - MeLightSensor lightsensor_2(11);5 d+ ]' e: v* a& J
- MeBuzzer buzzer;! y1 ~1 S/ Z9 b0 ^. _
- MeLineFollowerArray linefollower(PORT_6);
q) k4 e) R8 J4 ?$ Y - $ H# V6 X9 W) Q, N+ u" ?% y. c
- #define Error1 19 ?: ]2 m& C" [$ I. u
- #define Error2 2
' |. g; B) y0 J/ o& ^+ e - #define Error3 34 E5 P+ x- R) l
4 M( D7 z+ w# t5 J9 b2 T- #define Kp 15
I6 ~' [5 ~: i# e7 \# T+ L - #define Ki 0.15
+ v8 X1 Q9 v7 w/ i" K! N( e" t - #define Kd 0.03- X8 P( I; k3 q
- # s- F9 p3 I6 o
- uint8_t sData;
0 z! k( q, J- I, } - uint8_t D1;9 n# x6 D* }0 [4 W. \
- uint8_t D2;
/ b9 @! E. }8 z9 L# d! a - uint8_t D3;
3 b' L8 [ h$ N q6 g - uint8_t D4;
. }: F4 {' Y* \1 |% X% m - uint8_t D5; q2 _% q( Q, n3 `+ D
- uint8_t D6;
2 }& M0 I( d4 k, A7 V- v, q9 E: g! | - 6 P( z& m+ c. N. d
- float previous_error = 0;" D x" O, X: l- ]
- float integral = 0;# \0 i {! f! M+ I; w( x
- float derivative = 0;6 t" q3 p; E5 @9 K
- int Speed = 160;- q% J7 Q/ x7 z6 C m
- float output;
/ ^, F: s T! e9 o' O+ ]7 e+ U
; N9 z# a" ]9 k' [- byte Left;* m- ^; b& G* }1 e' D
, _ e1 O5 }# M2 \- void setup()
9 y4 e G7 y: ~0 { - {7 j& z6 q* q1 U+ H# U3 x9 B
- //Set PWM 8KHz
& \/ x7 M% q& n- D: V. W' a - TCCR1A = _BV(WGM10);3 b" q) b8 U- D6 n
- TCCR1B = _BV(CS11) | _BV(WGM12);: m A7 i1 b# s4 S r
- TCCR2A = _BV(WGM21) | _BV(WGM20);
, G6 ]/ C w y5 [" P6 L" L# D - TCCR2B = _BV(CS21);
: [# S1 [; j/ _; v7 C3 [ - Serial.begin(9600);
5 h! M7 c- m3 \% F: h4 K6 P% C - buzzer.setpin(45);3 l. T' X X2 {% H
- }
# W5 i+ o5 L1 n' H( w4 ?
: y( C* o% B0 |4 A& d" I5 m- void loop()9 ]5 K) s; U. F
- {+ @, A8 F; F" \( w" y; f7 s6 C$ N5 g
- while(!((lightsensor_1.read()) < (10)));" L4 v. L, J2 w. Z
- buzzer.tone(1047, 500);
8 m/ z+ l: p0 m2 `8 b/ X - delay(500);% a5 @+ e8 R- ?% J# e1 m" H
- do
% k, _* M" h$ Q - {- u0 R# [5 ]5 K1 q
- int Err = getErr();2 _* S- [3 L+ z4 k
- if(D1 == 1). S' l, U: l. W6 m1 e" E) A2 [
- {3 W- z* h! ]7 L0 ?
- Left = 1;
" h7 m1 b$ `$ {8 x - }3 J% r" m% O0 S# r% u
- if(D6 == 1)
) Z" A$ t8 f+ A. l5 |! f! S: K3 s% S - {
8 T5 P, ?: D+ A$ [# i; P" V - Left = 0;0 q1 ^* m0 k1 N& E
- }
! U% g9 W$ ~! c' F: b - if(Err == 99)5 U9 d5 |5 E6 ^' k% \
- {
. O. k* \9 X' Y$ z - if(Left == 1)
9 l) `6 X: o3 o& L* S8 Y - {. f% d3 E4 h# v m3 y: C
- Speed -= 5;
+ h( i# c; s; s; K* M* i5 R - moto(0,Speed);
0 Q* r w; U0 F) Y - do
4 j l+ @# E7 J* ]9 Z, P4 ? - {8 X0 g- t2 Z9 I) ?" C; |, N
- Err = getErr();
& ~: L% M9 T& K+ ~3 {) i - }while((D1+D6) == 0);
) w' M f9 {% W# \: \" F( B - }
8 P$ i! v5 h4 B% @ - else8 b( X. @% {) E% x: R
- {
5 u5 Q& u* h; Y4 m9 D - Speed -= 5;; h2 T8 V+ {- K9 i
- moto(Speed,0); 2 t% d! M. @) I) y# G9 A
- do# ^2 {. e8 n7 ?
- {5 a' H* t: b4 r- V4 ]8 r
- Err = getErr();# e" X- D% ?% t
- }while((D1+D6) == 0);6 D$ h5 }. T; R* _1 [
- }6 I2 [9 A1 O! j3 g
- }2 C2 k- z! l, |9 f0 L
- else% k* f0 N o# s: g. V* Y! A
- {& ?* J" C$ p8 s/ o0 W1 b
- if((Speed < 160) & (Err < 2)) Speed+=1;
' o3 X E+ E! S) T - if((Speed > 100) & (Err > 2)) Speed-=2; # N; L, H( y- n
- integral = integral + Err;
# `. c5 O" s# C# e* G - derivative = Err - previous_error;
$ f2 P# W# n( P9 r! K" o9 V - output = Kp*Err + Ki*integral + Kd*derivative;# \6 `6 X( A4 e2 v
- moto(int(Speed-output),int(Speed+output));
! R, ~) g: ?2 A' M V - previous_error = Err;8 S* p9 {' A- d- R, ]9 F0 c% e
- }. T) Y1 X7 Z) ^7 I
- }while(!((lightsensor_2.read()) < (10)));5 t, S7 ]' i; K5 C6 B
- moto(0,0);+ `: r9 z; R6 r# y5 x0 e* ]) U
- delay(500);
& K4 H# U" U! c& t, `( Z& w - buzzer.tone(262, 500);
" B: C+ H ?( l - }3 Q- ^+ F) S) l/ e1 h
7 N1 e+ t8 t8 b4 a, A3 J- int getErr()
1 |/ D" c( w2 p& C9 r; k - { , \( p$ {- S( Y1 R8 H
- sData = linefollower.getValue();+ X' p# W9 n( _
- D1 = ~(sData>>0)&1;- r" h4 ]3 g B
- D2 = ~(sData>>1)&1;
) p! e0 L/ z3 t- Z2 Y - D3 = ~(sData>>2)&1;
0 a3 `, i& f' |1 K! w - D4 = ~(sData>>3)&1;% w& F. m; T3 H, ?) M# _7 k
- D5 = ~(sData>>4)&1; c' T) A6 R, v
- D6 = ~(sData>>5)&1;
' ~9 R2 y) |6 K! X - int downD = D1+D2+D3+D4+D5+D6;
% n1 ^ n! h/ i0 u - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
7 A, ~1 ~4 L7 H4 t- t6 P' k6 C" v0 o - if(downD == 0)
f( L6 `6 D0 O - {
2 t- w; h) y* Q - return 99;
/ d6 J. k$ V+ _7 C/ p0 G1 o - }& G2 ]! ]8 g" Y) |5 M9 U
- else9 r5 N( m+ G8 y& U- k
- {* W' Y( c8 Q1 W, m% ~4 I. A- I
- return upD/downD;
! W$ l" P% P/ q5 R8 z8 f - }
% ?( R8 Y) P" C& Q" o4 R" ^3 U - }' f7 y' S- ?" D9 o2 Z8 l
- - W4 i- l4 z4 w0 C# Z: |0 f
- void moto(int Speed_L,int Speed_R)" E2 Y' g6 B' |8 B( [. d! @
- {3 e! S, s! C" ~0 L- O$ ^
- Encoder_2.setMotorPwm(Speed_L);
4 ^/ ]1 Z9 X& D A0 ?; V - Encoder_1.setMotorPwm(-Speed_R);
# {: [$ F- b# | C& ^' S - }
複製代碼 & n+ X# _; e' ?7 w9 m: A, w6 s) @4 L
MeLineFollowerArray.cpp
( p( m+ a/ T( [4 ~1 d6 v: x, b5 i- #include "MeLineFollowerArray.h"4 ~) _- P0 ?, F# v4 I# W
- 1 U8 |2 ~. S0 }4 c; x
- #ifdef ME_PORT_DEFINED
& m: W0 I/ l" Q& W - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
+ U& e2 _$ N$ d! | - {4 n: D0 K9 B9 H7 _: `& e
$ ?& U4 A$ ?9 D) j- }2 |" `& H( m7 q" S
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)# v9 a( V: g1 Y5 f* c; E3 N
- {
5 j7 x& G9 l; I - _DataPin = mePort[port].s2;+ `3 b, C2 q) g2 Q9 ?
- pinMode(_DataPin, OUTPUT); ! r* Y4 {/ j5 W6 f5 J
- digitalWrite(_DataPin, HIGH);
# q2 X( t( D" o. K - }
; r' S6 Y8 ]# s* @$ h - #else // ME_PORT_DEFINED7 J6 a( X! T, a
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
) {& N; N2 X/ d6 s) L - {0 _0 n4 x+ {# `, a! C3 O
- _DataPin = pin;: J: A% k/ ^' ~# F
- pinMode(_DataPin, OUTPUT);
/ P: [; ~ _- s. s - digitalWrite(_DataPin, HIGH);
' |& O: e) @3 ]- x$ l. l9 a - }
9 {2 N; ?+ h" y+ [ - #endif // ME_PORT_DEFINED* k' g* y1 k0 M, Q- G* u# i3 _
6 g3 K: U% S* r1 K
0 n& s4 v9 r. B5 M1 `" _" i0 _- void MeLineFollowerArray::setpin(uint8_t pin)! ]* x% R; [0 `2 L9 o" b
- {, Q4 D' L* V) n- F/ i
- _DataPin = pin;
9 K8 Z' n5 l5 \# T( S3 T( ` - pinMode(_DataPin, OUTPUT); ) q. }* _; n7 N0 Z& k$ k7 l
- digitalWrite(_DataPin, HIGH);
9 O! S- w5 ]' ] - 8 p6 g$ {! q4 w7 c
- #ifdef ME_PORT_DEFINED" Y7 F" {5 ?- `2 S. S
- s2 = pin;
6 ?9 U9 _# X9 V3 |3 ] - #endif' |+ d# [- T, U/ K( U1 _& Y/ E
- }
! ]& D4 u9 q6 l, K# N
7 ^8 C5 t' K! H7 M+ ]) ^- uint8_t MeLineFollowerArray::getValue()
( U5 L1 A* G; M" f' t. ~ - {' R1 \+ X0 a% `( d8 a9 D
- uint32_t LOW_level_read_time;
& S( {$ X; B9 X8 J - uint32_t HIGH_level_read_time; P$ R3 d( p3 V2 T/ l7 A9 E
- uint32_t time_out_flag;
* A7 ^. v( k) `+ q+ l# C5 U- w - uint8_t Sensor_Data[3];4 ]% y" M8 Z& n3 T' ]0 E/ k
- static uint8_t old_data = 0xff;
: s5 a4 d+ V+ k* W2 d- `
; {" J5 ]. f+ ~- i" E2 J' D- pinMode(_DataPin, OUTPUT);; n4 j% O. v# |
- digitalWrite(_DataPin, LOW);2 T) V8 K; w1 \* A$ O
- delayMicroseconds(980);
4 Q! w- o6 @$ n9 ^' k - digitalWrite(_DataPin, HIGH);
5 G$ {' L9 |9 n. L& Q! X) x9 a: i - : o% c% V2 @$ ?7 f' i) k6 Q
- pinMode(_DataPin, INPUT_PULLUP);
# N8 |3 Q Q$ I - delayMicroseconds(10);
0 u, S/ Y1 @3 Y4 D% G+ b6 O - . v" a* E6 N: C# h, {. t V+ d
- time_out_flag = millis();7 b; p! v: _. L+ ]) b& C% m
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ W4 t6 R7 K$ C% k5 r: h8 B7 B
- 6 C- a' |- |+ O, @4 c
- LOW_level_read_time = micros();
% o( |6 C$ d3 T6 T' @ - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! J! T9 u; O6 u& K6 f* u
- {/ ^, `2 ]/ Z$ E0 M- C
- return 0xff;$ d' q' }4 X$ g- M5 K) v
- }
t, t ~5 H, d8 S - 0 q* ^* N0 p0 q' G9 `3 N
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );# [( e) G. X0 v7 A4 ~+ \
- & E3 D0 Y# o' Z7 I( k
- HIGH_level_read_time = micros();1 L& a" D7 w0 `! j( I- V8 b2 v
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level2 c) y' H9 k$ y' i `
- m' v5 S2 [+ H
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
& u/ j' c' y R! D6 }0 I& \ - {
( R: t+ U: U/ [; | - return 0xff;6 B r" @- {( \ t
- }
7 U' V* Z5 i5 {, U) k2 a6 n' o - & |0 K: |. u! P6 S; n3 V# b# Q
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
- p% m2 \* i9 H8 ^- c6 l - {3 A/ n( D& | J! ^3 X# L+ F, p8 D
- return 0xff;
( I5 U0 J3 c% o0 g. u - }
/ R' P/ u0 E) Q- f' A' A
( o3 r* K1 n. g" O- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( Z, [- q/ v) w+ |6 X0 g - LOW_level_read_time = micros();$ h+ H6 d8 P, n/ U( s$ }3 ?
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level5 h- n; z8 @0 {- o8 V7 K7 ^9 \
: p1 V" ?( ~) p1 O- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 ~/ u( Y; d8 F4 E
- {
* g. H2 K* n9 u! u M - return 0xff;
; w' u" h/ f1 Z; V - }
5 o: v" b5 r* m. b% {0 q. T$ o - 1 S7 X9 R, x/ y! ~) T7 b- X* N
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))* s1 I/ {3 Q5 d
- {
2 H# `/ ?& }3 B7 e6 A' I. g) l - return 0xff;. ~. R& \6 Y" V- O) C" w
- }
6 d. R& h/ ?5 V L( j+ }+ _( Q - 9 `# g8 O1 A. i& s
- for(uint8_t k=0; k<3; k++)
+ T/ Y4 Y& k5 r8 ]- i$ ?( _' a - {
5 w! R. V$ d) v. Y5 _# k - Sensor_Data[k] = 0x00;
* w1 h; x' ?% u
/ ]- S: Y9 q5 d- for(uint8_t i=0;i<8;i++)+ h W4 E- i+ Q) u6 q S
- {
3 U% G# @! n0 U5 i* E - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level5 C6 z' |$ C* x9 O6 m' C5 F( M8 a
- HIGH_level_read_time = micros();) {+ e/ E' p. v+ Q, c
- LOW_level_read_time = micros() - LOW_level_read_time;
- V1 z( l- y6 ~3 }: I - + p4 L! c( `7 I0 w4 q2 h
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )7 y! e. C: r9 s: N
- {2 T$ y( g! a: I9 M4 [6 x; p
- return 0xff;" H. U" z$ q( N; N; r* k5 ]' ^
- }4 l) F0 [: A2 X& O# r3 k
. E3 }/ [- P! m) J: y9 C- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 ^ c3 N5 X( U. H5 o - LOW_level_read_time = micros();' n; u( \; A( x3 U$ R, `! N
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level$ F8 R# i: y1 z6 f
) j+ i' y9 Y0 u- u* F. D- _- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1; x% K' s( W' d$ }
- {5 j: A$ F& }5 m9 w0 J
- Sensor_Data[k] |= (0x80 >> i);+ y3 D$ I: B. D# e2 K
- }
2 X$ C, \" m9 L+ m y7 Z$ ]$ S - else if(HIGH_level_read_time >= 100)" X8 a2 b$ D( V7 c9 O: Z% O2 ?
- {$ h" S3 \7 _: `
- return 0xff;0 A0 V& J" X# S/ c( u( Z% k
- }5 h T+ j$ e ?" o# i
* g! \$ Z# |7 k# c) _- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)6 `) [) a6 n! N) o* c
- {
; A4 [% X% F# }) J1 y - return 0xff;
# i1 `$ L3 o/ y$ O7 \( G% O% a - }0 |6 ]0 u$ C( O) K( y+ _
- }3 D# _0 p/ M5 i/ b( ?* q* r
- }
5 N# G. Q4 ? p4 J/ z0 s. u: m - 9 Z* T5 [; m% Z- W; g3 H' K6 M9 F
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level8 C8 z" a( m! G" }2 D4 D
- HIGH_level_read_time = micros();2 P% p( a" c- e' W% Z
- LOW_level_read_time = micros() - LOW_level_read_time;
1 `8 `5 }% |- J" h9 x4 [" B - ! M1 @: z4 Z* g9 n, }+ @
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
o; ~) F) \* Z8 e' m3 _/ O - {
1 x; X+ e# m2 P; t* O6 l - return 0xff;' R& C) v3 g4 c7 @
- }* u, r9 G' Z- |
- : X5 ?$ y8 b3 Y, i, B! r
- pinMode(_DataPin, OUTPUT);" h6 |. I4 _" E
- digitalWrite(_DataPin, HIGH);0 K8 u4 E) N1 Q8 m
- ^5 B) Q' \. w) E+ Y- Q: g- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))& u- S( X; }) U
- {
: q3 n) r" l. i - old_data = Sensor_Data[0];
9 r3 j3 e# ], K - return Sensor_Data[0];
( y. m# D5 ?2 T- r2 D0 J: l4 b' ^ - }
) a# }$ t4 E; E& m% H- B* t' L4 v - else6 p; k+ g" `% n" E x% z; A5 D$ ^
- { A5 R/ _% P& M8 I
- return old_data;. M) _: w- k: W' |3 ?& ?9 E9 m2 n% G
- }
8 N5 `2 c* j5 ?+ C - }
A- k3 ~/ u8 [9 q6 M/ |
複製代碼
7 F8 I" V/ r! Z8 L8 \3 e7 i& HMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
5 @/ l2 N: _% J7 i% a3 K/ B0 k: o: j
% S+ p7 a& e! H! l
|
|