|
|
* T b, A8 X) J$ ]2 |- #include <Arduino.h> ~# r+ d v5 U* F, k
- #include <MeAuriga.h>
9 t% _2 i: ?9 s! h4 L9 h - #include "MeLineFollowerArray.h"
1 U. `4 s' F3 H/ ?" [- F - ) a; B: g) q7 [5 X# M3 w+ `
- MeEncoderOnBoard Encoder_1(SLOT1);
; w, ~' j9 Z3 ~5 k( l - MeEncoderOnBoard Encoder_2(SLOT2);9 w/ T$ H" e" U- P; r# @4 l6 G- a
- MeLightSensor lightsensor_1(12);. v" v% ~3 H8 C7 M" m- {! @
- MeLightSensor lightsensor_2(11);/ `7 N$ C( P7 m; ~+ ?3 y7 M
- MeBuzzer buzzer;% q) I# i' m8 R1 u" `# U( n
- MeLineFollowerArray linefollower(PORT_6);) P1 q+ ~: t# L9 J( C
( k) M; y% j$ s7 M! P) O% ^- #define Error1 1
7 o- f/ m5 G# W3 q9 ^" p8 x/ Y: ~( m - #define Error2 25 l) V0 E6 I; w' h$ q3 ], I5 B
- #define Error3 3
% w9 S( I- p& p
" f0 [ h0 T: W: F: ]* r9 |: L- #define Kp 15
' X1 R2 [. a$ l H2 w - #define Ki 0.15: h# W! Y) ?4 N7 z8 B- W* V' U
- #define Kd 0.03
1 y8 [ d( ^ u5 A: X6 q7 {. G
; l, v8 ~5 {$ I9 G5 @3 j7 ]- uint8_t sData;4 ]5 S3 {" p4 K# Z L' s
- uint8_t D1;
4 K6 @7 Q5 y7 [/ A c; q* H - uint8_t D2;
* p5 @0 |1 v2 _7 j- W/ N8 v - uint8_t D3;9 Y' t* }: E- @% N! a5 i5 n& y
- uint8_t D4;
2 n4 b4 j- t! [ - uint8_t D5;
h7 |* ~% w- `* O& e* U1 ~ - uint8_t D6;: y$ P) E2 x% i7 [
s( c! D) r" S; B C' T: T- t. v- float previous_error = 0;
6 N+ a% Q$ V5 \" C7 u* ? - float integral = 0;5 }. e$ ?. A6 U" @; S: f
- float derivative = 0;
; _" Q0 K2 u j: H - int Speed = 160;" n6 o) H5 s7 s. n, u
- float output; v/ d6 X% ~! D; s/ d6 h3 V7 E
1 b/ W. U) p* m- P6 X& K7 S& _- byte Left;
7 h! i) u- d$ y/ A: \ - . Z* \/ P. {7 t& u2 O
- void setup()
e& P5 M6 F- D4 ^7 k - {
6 `, T5 x, M& N: e- @# ` - //Set PWM 8KHz
/ T" E' U' D% X* Y. T- h' J - TCCR1A = _BV(WGM10);
! M1 Z# H5 J4 B5 H% Y$ Q2 h - TCCR1B = _BV(CS11) | _BV(WGM12);0 I0 l% i* C) M6 h& ~7 a! E
- TCCR2A = _BV(WGM21) | _BV(WGM20);
, [; V5 b1 u( n% ~( H7 w - TCCR2B = _BV(CS21);
v( L, E- A2 K d - Serial.begin(9600);/ Y: Q* B% {/ @+ M
- buzzer.setpin(45);
- j0 Y( M6 M6 T/ ` - }
4 _7 _& ]' O1 t) C# T; G: @2 T* E
_' ^4 r6 t& v" M a# o- void loop()4 |) Y) a" K7 W- w
- {' Y, Z, r* E3 ~
- while(!((lightsensor_1.read()) < (10)));, Y8 p' k! e) Y# N5 G
- buzzer.tone(1047, 500);, g% U7 E* y! y/ r
- delay(500);& ?: n9 `* ~4 t7 V- g
- do
1 X. {4 u4 ]( R; H' D - {8 i2 b- E" S' A5 C. q0 O
- int Err = getErr();
) w. z, i7 Q1 @6 u8 C1 n - if(D1 == 1)+ j( k0 K/ B, E" b1 P! W
- {9 n; A) J) @1 { Q$ p+ z
- Left = 1;1 t# l& D9 o, ` x2 ~/ l& _/ X
- }
2 r# [: K" i$ S" T# y - if(D6 == 1)
: F/ n2 R0 B2 \/ O6 {0 \- [ - {: P! \/ M0 J# R( h6 y7 R, Q
- Left = 0;9 F; {) q& v, y# S( z
- }8 A7 I+ h; V4 B, c
- if(Err == 99), r$ v# a$ |8 ]6 e7 f
- {
- ^: s3 V. K% q& ~. K - if(Left == 1)
, }1 q0 X W1 z, ? - {; Y8 J9 L0 _9 i6 Y+ }
- Speed -= 5;0 h' o" f. i3 l6 m9 O
- moto(0,Speed);; j6 P* I2 L' Y b
- do
" Y4 T; [7 ^: U - {; E: Z' \ X: t+ G! f
- Err = getErr();0 S1 _) z+ q9 U5 r S
- }while((D1+D6) == 0);
& p0 w! Z7 P: d0 a/ M - }
# X, K) J) B2 |& R% K7 a - else- ^! ]+ N8 Q: @) ]" Y8 y
- {
& i8 q' e- l* L& S - Speed -= 5;4 M# [4 R. b/ r
- moto(Speed,0);
& T2 Q8 i5 w' o8 L' d% W - do0 q( a6 ?% }( u4 b7 n, [
- {
: i/ t! A d# J" \4 p: d - Err = getErr();
. r* B/ ?; l6 X7 K- d( [& M9 _ - }while((D1+D6) == 0);1 U6 `$ `. a$ f
- }7 _7 S) C' J0 U' T
- }
* a9 X8 y8 k( O7 h+ B" n - else. ^' o) \) O1 \* K
- {
/ A& Z( V) ]3 S: M, I/ c0 q - if((Speed < 160) & (Err < 2)) Speed+=1;
6 i6 Y/ D& i8 @+ C - if((Speed > 100) & (Err > 2)) Speed-=2;
5 F% l% U8 L: u$ R1 C$ x* B - integral = integral + Err;
5 ]$ ]' h' o& E8 N. s) u+ H - derivative = Err - previous_error; W" ^' U, }7 U9 \9 p3 a
- output = Kp*Err + Ki*integral + Kd*derivative;5 Z" }4 {+ u+ T" a8 _# I: F
- moto(int(Speed-output),int(Speed+output));7 v# {; }; }; t4 y U
- previous_error = Err;
& v4 p2 e9 v* t- n6 n I; F- ^ - }+ W8 Q: K" y1 o B7 D
- }while(!((lightsensor_2.read()) < (10)));
! T& I) a' R; I5 w \) v - moto(0,0);& A) p8 k4 ~, J6 X
- delay(500);; S# t1 a$ ?) h" s
- buzzer.tone(262, 500);
q8 `+ f+ K# r& G8 a. w {0 _/ _ - }
! i6 ]9 L$ d! I9 A" Z7 l. j& s
0 Y! q1 T/ W; o! [& E" I- int getErr()
7 C3 X7 |/ }7 X- |: { - { " D$ J' e: D8 B0 x& g6 d( N
- sData = linefollower.getValue();9 H- D( E9 m; N& \% n
- D1 = ~(sData>>0)&1;
5 c, S& m/ ~7 Y3 `4 Y1 n; h& e0 l - D2 = ~(sData>>1)&1;
& x% \! H9 U& H& F - D3 = ~(sData>>2)&1;
$ g3 x+ \8 l# `& m( r! o8 ^ - D4 = ~(sData>>3)&1;) N& Q) E4 e: a6 W) e! ]" C \
- D5 = ~(sData>>4)&1;* }8 g/ I& h! R# n6 p7 Z
- D6 = ~(sData>>5)&1;4 s/ ^. M8 ]- F [. [6 O3 b
- int downD = D1+D2+D3+D4+D5+D6;
0 Q& m+ |3 j5 Q% ] - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
) D9 L: s+ |. \1 c! ?. C7 m - if(downD == 0)
- h+ g' f5 F! l - {
9 h: `$ r5 H( C. Q: J: M- ^" @! A - return 99;
% G+ ?+ L; l3 a# \$ Q) b - }
8 ^3 ?: m+ \2 Z) [# {+ W - else- ?+ I2 _( j+ y H8 t5 r
- {. v1 Z6 @( f1 D k, Y [
- return upD/downD;* t* L) K d/ ?( S! H9 w% v$ ]) u
- }# ]- W/ x% C9 H e$ U3 W/ U2 z+ ], O
- }" Z a0 K6 L, d6 D
8 Q: j5 D( M# _3 S# b& a7 d* r' D- void moto(int Speed_L,int Speed_R) x" v! ~7 B- Q. R
- {
3 g! }! y6 ?1 ?3 b, Z - Encoder_2.setMotorPwm(Speed_L); s/ }5 w6 ?: Q( m5 w+ t3 M6 G4 V
- Encoder_1.setMotorPwm(-Speed_R);
; \" W# E% I& O9 Z- S! |9 z5 E5 d - }
複製代碼 ! `, L* ]4 v* N/ C
MeLineFollowerArray.cpp
* r. j1 S( b4 j- #include "MeLineFollowerArray.h"1 o: ]) L' R& r% W# P8 N
- 7 ]8 X' E1 e& s
- #ifdef ME_PORT_DEFINED
/ F. g$ B+ u' m ^* s4 A - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)* h3 s- A; }6 o! U+ b4 a1 s( Y9 Z
- {
1 `, S8 |7 P: K+ p- n' g; W- `
% }. w8 J1 {4 X" l& ]& G8 {- }4 x8 _& D$ J9 r3 T2 Q
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
$ I9 ~/ _# s6 |6 f - {. K! ?& t1 k1 g( K3 |/ @0 t! T
- _DataPin = mePort[port].s2;7 Y5 m% t6 B q4 ^. {* y8 {" [
- pinMode(_DataPin, OUTPUT); ) X1 I/ P3 s: _' M+ l# M* Y
- digitalWrite(_DataPin, HIGH);
: Q$ l$ {0 `+ A - }
- G+ } {5 ?9 a S/ Z. R* \' J; T - #else // ME_PORT_DEFINED) X2 P" F: f/ f6 Z
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
; n6 p$ F+ }2 s - {9 k% Y) A: V6 Y
- _DataPin = pin;
$ y0 W. C5 G% ]4 K: X - pinMode(_DataPin, OUTPUT); 0 {9 d( h* _, g7 _
- digitalWrite(_DataPin, HIGH);) I; a9 _% F' k5 W$ E3 p
- }
& k# S3 O+ r2 w# u# y. S0 Z - #endif // ME_PORT_DEFINED; a: l1 ^2 T8 y, U6 C( k) w
- 2 F) h1 M% n+ U) h4 i( t
- . ^- ]& T5 K' d2 A* I3 o
- void MeLineFollowerArray::setpin(uint8_t pin)
5 |4 [2 k% q, u - { ?8 O f: P& Y# F$ o& L$ E
- _DataPin = pin;
h) ? h; R& R) }/ q/ ^: a7 x2 X - pinMode(_DataPin, OUTPUT); ( x3 n* q' V/ }3 h3 T/ |4 L
- digitalWrite(_DataPin, HIGH);- O* J+ I& t/ U0 d7 P' M
3 o1 M4 f L; I0 ^+ |, n- j! n- #ifdef ME_PORT_DEFINED
' o# t8 {. E G& B5 h! }0 ?( n' { - s2 = pin;
( y0 j5 i& d# f9 a. x - #endif! H: \" ^: @) l/ `% g! U* r
- }& {$ u I2 V! \" ^+ y
- + ]' W& O& v. C% t
- uint8_t MeLineFollowerArray::getValue()' I* ~1 ^5 M( |
- {
8 M: a# C" Q+ P9 C - uint32_t LOW_level_read_time;
3 Y: J l5 N, C- ]5 k, x y - uint32_t HIGH_level_read_time;- E, ~ q4 [; T8 v2 E3 k
- uint32_t time_out_flag;
3 w7 m) ~, I( p- [8 S) t - uint8_t Sensor_Data[3];
5 g- D& {; L6 \0 X - static uint8_t old_data = 0xff;
9 y" y1 k" W6 d
/ Q2 {, }' [6 a% u; f- pinMode(_DataPin, OUTPUT);6 ?9 B7 ~3 S0 h1 P7 S1 {) N% Q
- digitalWrite(_DataPin, LOW);
! t* y# @5 C0 Y1 k# Y - delayMicroseconds(980);
3 w/ u& N+ [' B J1 D8 ]" T2 C - digitalWrite(_DataPin, HIGH);
0 c& i( n: B4 q5 Q5 R3 M
9 m) m0 z* H/ N: N: Q' D- pinMode(_DataPin, INPUT_PULLUP);7 _* L g- Z! x& G0 ?2 c: |
- delayMicroseconds(10);$ G( j3 M. E8 `- A: [
7 `% n, _" P0 z, w& ~- |, P5 A- time_out_flag = millis();
; q/ w' f5 O Y* v - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! W' w* G/ Z9 q, a - % c# g. Y! F1 R- P
- LOW_level_read_time = micros();" {) l: |9 M; V' g! B9 B* f1 E
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out& G) l. ^$ C5 ?1 X N
- {. i; S% R. u' O. i# m
- return 0xff;
/ I/ i! p% `, V/ r+ \1 d7 ] - }+ K+ O" D, Y4 l
- ' ^/ T* T0 K; F( {7 R5 I
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 a( `: g1 i& x; [3 |' |" z- a% d
- . C3 J9 x0 Y/ H
- HIGH_level_read_time = micros();$ H- E. \( u E+ i5 l; F
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
4 ^' O( m! i% x& o* h U - + ^8 b8 q l9 O% I y
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
+ ~- c' I4 R6 v - {
/ n" V8 [5 _; w. \; K: ^ V; V& Q - return 0xff;
1 `2 |/ \8 x! y3 v3 k2 ~5 t - }2 v. H6 w' m/ U* m& c/ I
1 A' T9 B2 p# J& H/ M$ u& {- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
& d8 Y6 [ r k - {0 q( b$ N* k! e. z- U8 A6 a1 K- O
- return 0xff;
, C2 j9 d) _9 g6 ?2 k, B - }
2 P- {. t- s# p - 5 i. I* g/ X/ X6 [$ |0 `! n
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
/ x5 K/ p, |) D - LOW_level_read_time = micros();: i! U; [/ z/ ^, y9 q/ _. h% f
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
0 b3 M. _4 }2 n8 A - 2 w1 q9 M8 `) j( v
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
c+ E/ X+ P$ Q. g' T! Y9 j - {" D- a+ @! o* W* x9 ]
- return 0xff;2 p$ l4 F) V; n+ E% o: H2 [$ q+ E
- }) ^# Q0 Y; ^) F" j: i ?9 E
, o4 ^* H5 c- M* A! D2 P- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))( Q1 J1 a. |; T3 z/ _7 P* j3 Y6 v
- {
8 N) _0 ^8 L5 d( R2 ] - return 0xff;
/ N7 R- g/ `: N1 K' m% y: Q1 `7 k - }
, { n+ d* b' E
: U- l7 I. ?: y- for(uint8_t k=0; k<3; k++)
- s, U& @: n6 p$ K. k - {
t8 `+ A! _5 G+ p - Sensor_Data[k] = 0x00;
3 Z ?' X8 W7 o# B6 k6 B
3 T$ K8 D" i4 ?8 p/ B9 W4 e1 \- for(uint8_t i=0;i<8;i++)
2 s3 `% R, B# ]" t - {
6 h5 m; T" `* N2 I( C - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
& w2 V' G! N8 E1 S: d5 H/ A - HIGH_level_read_time = micros();
' A T; C2 G+ D' h8 ` - LOW_level_read_time = micros() - LOW_level_read_time;$ O5 s& u9 }, S0 J
! H1 g7 ^& {6 u) m* V% E( y- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
: E' L7 Y4 y$ O2 w% _ - {, }3 [6 ^/ @6 c4 Y n, L
- return 0xff;
( _) H# T9 s' I8 F1 K - }
/ h* x( K: _2 C3 U
. A8 J3 X$ b$ \$ M2 w- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: Z4 X* k) V$ ]
- LOW_level_read_time = micros();. D( b! Z+ N) h: ?
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
, }6 t+ N, I, L8 Y - : f# c) }0 |. A4 b
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1/ a. R( F1 f2 D$ n
- { G% Q- F* O6 G$ f: |! [+ t$ s
- Sensor_Data[k] |= (0x80 >> i);6 y8 u6 D! q1 k" k7 b3 v
- }% P) G, a! P4 I/ W7 d" O
- else if(HIGH_level_read_time >= 100)1 G) l; {9 f" H9 ~
- {
, s0 n( j2 B- _6 R( D. q0 `! h - return 0xff;
0 E3 {6 k% k# q' p8 O - }4 v+ R6 Q7 f# r# u) h0 n6 p
% e: h" n Y9 h) G9 T% E$ Y- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)9 m+ v# i1 }7 O& {
- {
4 C8 r* C: x$ L6 B V& x3 [/ f - return 0xff;
% ]/ b) K" }5 `* G - }
; ]0 d8 S+ W; ~* x - }7 Q1 A$ g" C `! g* O
- }
3 n9 x# k. G% w! }# h - " e2 A* B! ^4 A* s
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
8 s7 O$ h6 g1 B - HIGH_level_read_time = micros();% C/ C% E! t4 l% V/ [. p* a$ B
- LOW_level_read_time = micros() - LOW_level_read_time;+ I( E) E O3 Z0 m$ {6 @
2 k: S" @. a. ]% m9 n1 @% ~2 g' ?) k- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )7 ` b) e7 Q( d+ F5 u/ {4 u
- {% e$ N5 O" e4 e6 Z
- return 0xff;
# ^3 K2 e5 `8 v& T' ^" K - }! R6 N8 u4 o) l; e/ b
7 C$ j% c# K, C2 u5 ?- pinMode(_DataPin, OUTPUT);
& [ K: n; l; Y& b. b' _4 b - digitalWrite(_DataPin, HIGH);8 ? ?) c' m; z8 H7 F
6 T& g9 J) x3 m4 |/ G! ]: E- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
. i- x- ~' v6 y$ X& }2 x - {" K+ ]8 y# U" q
- old_data = Sensor_Data[0];; f" @# u, h& o
- return Sensor_Data[0];" t, Q8 D# j; b) N" [3 |0 S2 y
- }
! L5 d8 G. o2 n - else
, }& E: a0 ` @3 c) q" e - {
. U7 l9 ^. x' L8 a - return old_data;
, a- j+ N$ t7 e - }( v3 P+ w2 ~: t
- }
/ [) L: @+ w; r; @: ]( z. C& u
複製代碼
) ]1 b, }. j- \5 W4 ^! a6 TMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
$ J8 t. I* M, s/ h- W4 c. r- `/ z7 L/ l# R
|
|