|
|
. T# A& O' O2 }6 i6 }0 p
- #include <Arduino.h>/ Q5 o6 A' d( C/ l/ Y
- #include <MeAuriga.h>
. V8 {1 {5 g* J Z. [( Y0 f - #include "MeLineFollowerArray.h"2 ]5 j1 Z* P+ O+ Z0 F
- * w5 L& d, K3 S8 U. }+ `: e
- MeEncoderOnBoard Encoder_1(SLOT1);; b7 }* M' j/ ^, @
- MeEncoderOnBoard Encoder_2(SLOT2);
) ~( R7 N3 x3 q8 {' K8 N - MeLightSensor lightsensor_1(12);
- c0 s% E( r% t( r0 D - MeLightSensor lightsensor_2(11);
: _* V+ m+ C. Y1 M$ ?1 X" Y - MeBuzzer buzzer;
' D9 r, D2 @8 X& X; Y" h - MeLineFollowerArray linefollower(PORT_6);
$ X% ^1 F! h- e9 g1 K
: I! S a3 h4 x, A% J6 t- #define Error1 18 y4 I) \$ o. |$ F" y$ x
- #define Error2 2+ Y1 @4 l/ M1 Q# ~5 q3 K
- #define Error3 3$ R3 ]7 K+ B6 {$ n
- / i$ y. c+ n: p D) R
- #define Kp 156 Z. i6 {3 H* ~& B+ ]8 ]; z: B5 C3 X
- #define Ki 0.15
" R% k# k9 D6 c - #define Kd 0.03
8 ?( w5 D r& |9 F0 w) v! r6 ^% j
8 d3 f3 S) f$ t- uint8_t sData;
" ~2 ^/ y8 d! d$ [: V* J - uint8_t D1;( e$ ]$ w M( h. N% W# B
- uint8_t D2;
$ t8 q5 m9 y4 b+ {* N! F } - uint8_t D3;
( }- S9 \% `5 V: J - uint8_t D4;
4 N5 L4 R1 c; L( K* H9 C2 z - uint8_t D5;+ K. w, Y" I. C# h" M" Z7 E5 M
- uint8_t D6;5 @ T8 x/ D: D( d6 p+ }
; m% s9 k- O. C: o7 Q4 J: Z" c- float previous_error = 0;
) j; M( j' r8 M) {8 _3 e - float integral = 0;
$ g% _6 y. E9 T, w. q - float derivative = 0;* ]! L3 [: E: H$ C7 p
- int Speed = 160;: b3 P8 B. {& w8 v& m- } ?
- float output;
3 `* y7 L& G$ o) }1 F
' x% d( s, i% Y" M- byte Left;6 b0 E- E5 P% A6 ]$ G
0 D0 d$ X% I2 q; m) ^- s- void setup()
& E& ~, n5 l- C' ]( ]. R5 i - {; s, M7 I2 r h& B7 h T) A
- //Set PWM 8KHz
- d L+ p- w8 e/ P: y1 M - TCCR1A = _BV(WGM10);
- Y6 Z9 ^% K( r: c - TCCR1B = _BV(CS11) | _BV(WGM12);. z# \ j8 i: i; |$ c
- TCCR2A = _BV(WGM21) | _BV(WGM20);& v0 q- P6 t& q* h. A5 Q
- TCCR2B = _BV(CS21);
' b* \- P/ {) G; y0 s' G9 _ - Serial.begin(9600);, S4 O8 y& Y+ |, E' J" F
- buzzer.setpin(45);
) j' K$ S1 z( p, B - }) z V* j: [8 G/ R) F$ i0 g- p
- 7 h1 G+ Z5 R- }" y3 a4 P* p
- void loop()
9 b/ ^# n { V8 \" F - {
( v6 z1 E8 \% M' v8 ?/ G7 S* Q - while(!((lightsensor_1.read()) < (10)));
$ U; F# b+ W. K. @6 t1 `! N& R/ H - buzzer.tone(1047, 500);
- w) f5 w Y4 a, ]. U. {, G - delay(500);
9 C7 ?9 ~' y S - do
: t$ D7 M: F V - {
. Z, Q1 s+ U/ _ - int Err = getErr();% v o/ I6 F% d8 m: Y% h/ f& ~
- if(D1 == 1)6 r/ ]& z4 f! O
- {
9 r/ w E9 u; A - Left = 1;
" Q0 d7 E6 O# e. \, } - }1 K5 q& D. y H
- if(D6 == 1)$ _+ Z: \; J2 {9 @7 Y' n) l
- {
% w' M0 f7 m* |) r8 M# T - Left = 0;
! E# p/ N0 u6 n1 ]0 p0 x3 j: F) L - }
1 d. E" {+ l/ m+ T0 K! F - if(Err == 99)! H5 ~5 q" @$ u5 C* _& I6 v
- {- |$ W, B2 k" l7 J0 {; l" u2 F
- if(Left == 1)
4 K6 P. p: E. \. p8 q/ o6 h - {$ F0 N& O) W ]0 B; s
- Speed -= 5;
) R4 u( g% p2 Y, M - moto(0,Speed);3 J4 k! ?3 w& z/ J8 N2 x
- do
5 b* h; |9 m- T t3 T: X - {/ K; b4 M) e: a, L7 K; W' O
- Err = getErr();" C. o# z8 V/ A+ X* d
- }while((D1+D6) == 0);2 N" W* S$ s8 z7 B7 k& [
- }1 M/ N& Q% u9 {: F" \* [4 `! T
- else
; D, h" ~5 B8 F" y% O - {
0 H; x" I/ ?! Q F$ ?1 M+ c - Speed -= 5;: i# W9 I( e$ B9 {
- moto(Speed,0); . s& X; m& w' j8 K' E
- do" o/ R2 K7 B o& O0 q2 N
- {
- y6 {" n! ^7 e! n8 }7 [. z: X - Err = getErr();
. Z5 y; q) h! }8 E( J' n& h - }while((D1+D6) == 0);4 i0 [- s" b3 b, x( c
- }0 p! @% q" S( M3 A; t" M8 S% ^4 l
- }0 d. k: O4 w2 i
- else
6 v1 d6 a0 y) d r1 n0 g - {# V* g2 [; r% u1 D' s
- if((Speed < 160) & (Err < 2)) Speed+=1;
4 H4 z( i, D2 W* \ - if((Speed > 100) & (Err > 2)) Speed-=2;
q/ p* g* z, ]( K9 ]- n* Q+ G, | - integral = integral + Err;) D0 k/ W& d; J: F
- derivative = Err - previous_error;
6 Z! Y+ l" x( b. A! I* e4 v/ F - output = Kp*Err + Ki*integral + Kd*derivative;
3 d# y! Z. p( { - moto(int(Speed-output),int(Speed+output));& I. M0 |' [2 T' f. {4 J/ d; q* e
- previous_error = Err;, _8 X8 j2 W6 C& E
- }
! T# H% Q) a' J8 y - }while(!((lightsensor_2.read()) < (10)));8 N- r. q$ V1 O, k
- moto(0,0);
' l' X# ^+ E1 d' ?0 f7 D: y - delay(500);; P% R% t' J% c+ D5 N3 @- B
- buzzer.tone(262, 500);8 Z, v6 }7 z$ [
- }- d. {5 b/ }% C" [, H6 R( w5 k
( p# n/ p4 ]- f! r, k! _; `- int getErr()
4 ~' J0 ?% W( m2 ]: N1 f4 I - {
) j9 ]9 i2 k1 D9 U1 f& U' n - sData = linefollower.getValue();7 R( C( E# `8 z9 ~
- D1 = ~(sData>>0)&1;
& Z) o+ v# ]2 e/ x, x$ _4 b+ b$ j - D2 = ~(sData>>1)&1;8 ^- z0 }0 W9 z; T c+ M* f) k% k
- D3 = ~(sData>>2)&1;
4 C; {6 L. B/ N' y* F - D4 = ~(sData>>3)&1;
: w+ V1 R; f# e - D5 = ~(sData>>4)&1;
: t' d/ f! @( Y2 I - D6 = ~(sData>>5)&1;; k( O- i4 B0 _ I
- int downD = D1+D2+D3+D4+D5+D6;6 V. u9 K' ?: c) g! X, H
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
! E0 L+ M) A Z& o6 A - if(downD == 0)8 e, d6 d8 H, z
- {
5 \2 j+ z8 I. G# k# X) ~ - return 99;
. P/ q4 D' ^1 b# C# F0 f& l - } E( k7 Q% j1 ~4 p
- else
1 q# F8 x+ a) d% s( i8 [ - {
, X' u* a- B6 d. w0 v) P ] - return upD/downD;+ x" G' k, u) z' L# B% g* E! ?
- }3 v! s | ^1 r. m$ P
- }
% U) u$ A# H4 ]$ w! v
5 b! b# N4 l- o# L2 C4 @) O2 q" l$ f- void moto(int Speed_L,int Speed_R)0 c9 v! \$ m9 A
- {/ r' v7 L: H, p2 ~: h9 g
- Encoder_2.setMotorPwm(Speed_L);* Y' Q3 q7 V. ?! b
- Encoder_1.setMotorPwm(-Speed_R);: q$ p. X. S0 h- C0 q8 d1 p s. |
- }
複製代碼
; I* u8 b1 M$ ^: p3 z8 eMeLineFollowerArray.cpp0 ^% ?" x- m7 w' d
- #include "MeLineFollowerArray.h"
) w1 s4 O- M( k7 m2 u7 n
8 W' l j& |$ n6 T) s- #ifdef ME_PORT_DEFINED, U- {0 c+ h. ~3 O
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
' E' p! F# N% Y - {6 F3 Y* T8 i2 u$ H# j+ C
- ' E+ d, c! _# G
- }0 f; G/ w, t9 [. V
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port); e* S3 y6 a- `2 t+ o! u6 K
- { y/ X- l3 ], V: Y. n& c5 @$ I
- _DataPin = mePort[port].s2;, Q2 S- c) f- p o# K
- pinMode(_DataPin, OUTPUT);
% C$ Q4 j- d* V! F, B - digitalWrite(_DataPin, HIGH); k" B$ \" t7 Q9 ^2 f
- }/ O$ j" N6 s' k- o" X0 h, E
- #else // ME_PORT_DEFINED, ~. F2 ^3 b% e* s
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
4 U" U3 a7 h& @2 [9 ^8 b! i4 Y - {
# ^1 i* d8 G [, U: x7 M: I0 r - _DataPin = pin;
. _" B S% C8 y$ K2 ` - pinMode(_DataPin, OUTPUT); 6 H' r5 Z5 O. R- K+ i N: b
- digitalWrite(_DataPin, HIGH);0 q8 J2 d' ~6 j# [+ N n; p
- }
5 `, C! l" ^' q6 v: O, `2 P - #endif // ME_PORT_DEFINED" D; G/ f1 A0 p k8 x
, @8 b& o9 X9 C- v0 ~6 p6 V- ! X0 b5 s. {6 d6 u/ b2 }' R4 h
- void MeLineFollowerArray::setpin(uint8_t pin)" b" s7 |- u' |) ]' R- p
- {7 y4 a8 u$ r0 X7 D6 F% Z
- _DataPin = pin;6 s* W" z, U J% o$ {
- pinMode(_DataPin, OUTPUT); 8 o; [% F$ ?: q! y T5 Y" l3 z; s
- digitalWrite(_DataPin, HIGH);
- @: u2 F% r* C3 M* E$ e
8 F R J% I x! X- ]1 D7 H- #ifdef ME_PORT_DEFINED0 u7 y% I; [( K* L( ~% `. \. `
- s2 = pin;$ w% X; D) N5 ]) L
- #endif# V" p. W; O) x9 k6 H; u0 ]
- }: c" Z' f: w( x d
" K& W7 _; }2 V9 h4 Y- uint8_t MeLineFollowerArray::getValue()/ E4 U8 A/ T; M$ _5 a$ Y
- {/ I; a# _8 o' L2 I# B$ o5 C# u
- uint32_t LOW_level_read_time;
8 `" F0 X4 M# R& l, I - uint32_t HIGH_level_read_time;
, L% t* H) g. h - uint32_t time_out_flag;
$ G- |0 \" a6 b) L# a - uint8_t Sensor_Data[3];
) A6 E; F) p: }1 r* W: X - static uint8_t old_data = 0xff;/ \: {4 y; }$ O1 R* N
- / Q" t+ I/ ~# J! J1 V
- pinMode(_DataPin, OUTPUT);
, K; A5 _) k% d, w9 O6 k1 o/ I - digitalWrite(_DataPin, LOW);$ W! G; [4 T9 k; s
- delayMicroseconds(980);
% R# ], G( Z2 U - digitalWrite(_DataPin, HIGH);1 M& i/ e: M4 R/ b" s
- : y; m7 {" ~/ P! [8 F* R$ h
- pinMode(_DataPin, INPUT_PULLUP);$ f, W( \; ^0 @* T4 h, \' j
- delayMicroseconds(10);
1 X$ s8 W' ^' {& |/ C& @. \2 N
9 U: j8 r9 y$ ]! u- y- time_out_flag = millis();; d. ?* \; L1 ]( ?! }, D! z
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 f& A* |( t- Q# L
6 l4 W0 d& n& M% j. F- LOW_level_read_time = micros();) f' P7 ?! F- W
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out' K# I- p9 O0 j: N. S
- {+ |. _7 |7 e( _/ w8 {! ?
- return 0xff;
5 h9 I: t$ A6 p# v8 n2 b - }" W) o. Y. Y. N5 @
- 4 e/ Q# O R3 M9 Y; O) H
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: _" y1 V# B% q5 @! Q+ P8 c
& M8 }( g1 j/ x3 k J/ R- HIGH_level_read_time = micros();8 L- Y) z. H {& V
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
0 g+ `+ n2 k% Y# l - 0 U9 J0 F' P2 f5 q$ p9 S- _
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out: A" B" M& q m; Q+ L2 f
- {/ N& B9 n( Y6 `% z! C
- return 0xff;: @2 p9 |# o6 B. S X I, F2 @5 K
- }" W6 B( m. ?' w3 `
# c) d5 M4 r8 y. l- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))1 N2 H% X/ e5 a8 o+ I' P5 e1 W
- {( f9 t+ G2 r9 s+ D( Q
- return 0xff;4 l) J6 R- {2 k0 S. i
- }
& n; f6 }" D# a - ) J5 c; i6 T- A. R" l$ E6 h1 r
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );& ~- Z0 W2 E3 p4 p5 _
- LOW_level_read_time = micros();
7 i, q# V& h/ X) w' Q8 Z - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level4 M4 U3 n9 J# u: k
/ o1 l4 K/ u7 ^0 {. g: l- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out. q/ s, o* u3 S
- {. z' E4 f8 O" g# Y/ Q5 K7 y
- return 0xff;% n- N) Q0 f ?! ?# b3 W5 {
- }# e0 \- e a; ~) \
" e3 ^3 A' E! J7 d- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))* C+ B/ X# {6 s% ^
- {- |1 A0 j; v+ G) |& Y
- return 0xff;
- x% `, X, a& ]5 L1 V8 U: P6 g - }5 D7 |+ p& [- Z" T; ^) V4 E! y u
/ T) z1 }3 s/ @6 J+ v8 w# f- for(uint8_t k=0; k<3; k++)6 O8 q& R& @+ P0 X% @# d4 l
- {
% x! t w' |3 o - Sensor_Data[k] = 0x00;
1 v& t4 X$ m, l" _) O6 O" E7 ~0 _% G
2 d$ F! x6 T' ~; G6 ^- for(uint8_t i=0;i<8;i++)
; t4 q9 Q+ E _7 G! c( k1 { - {
/ c8 P# P3 r1 I - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
6 N. W; `. I- u: Q+ i( a - HIGH_level_read_time = micros();
# K+ N! u4 p9 |" ? - LOW_level_read_time = micros() - LOW_level_read_time;' Q5 r0 {+ V2 Y$ P, t
- 5 g4 l. X1 y/ j
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )8 W& O0 ?: m) j( h/ y0 @0 ~: ]7 I
- {, A9 i/ A! W8 I# E7 D
- return 0xff;
- {* f( s2 N9 } d" u- \& d - }* |7 v: Z- V2 r; V6 L. Z" A8 R
! i: y0 ^+ f1 P; ^" |0 x- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ M; o. |+ n% k$ m7 g0 `) M. q
- LOW_level_read_time = micros();9 ~& \* n: ~3 A4 m1 U0 e4 W
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level- B' y: e$ p4 E
- # g- o N* i" K7 I/ u }
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1' P+ E9 B4 Y0 r0 |
- {
9 e b: s9 C/ r3 b6 P( Z6 ^ - Sensor_Data[k] |= (0x80 >> i);' n* e6 C) }( I1 G. j
- }; D3 k! k( `6 t" R# k/ O' K$ D+ b* J
- else if(HIGH_level_read_time >= 100)0 O3 g# w; |2 e3 Q8 ^( ~
- {
- n# R' i! f: U, Y& ` - return 0xff;; T2 Y( a& i! N1 S3 j
- }
( r% A. l F7 ?5 H
0 D2 L- T# f' n- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out); N, W; k3 r4 ]$ g
- {3 t6 j/ M' ]( O
- return 0xff;
% r' J$ p) Z8 A - }
1 F$ }) o. ^. Z6 D1 f - }
, Z$ F8 }( m! z( i; }$ @" x% q - }
+ P- l9 r/ ] e- j! ?8 F: | - / k+ [- ]+ D4 c+ w, \& @6 [
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
) {4 p* j, m' U6 M n( ~ - HIGH_level_read_time = micros();
( O1 w% A H* [2 [ - LOW_level_read_time = micros() - LOW_level_read_time;/ @+ S& H1 z. ~5 s9 ]1 @$ b) i
6 G1 Z) e# n7 i- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
6 b/ E* `2 }$ h9 j& p7 V9 s - {
- \" V+ U& a$ p, p- v - return 0xff;8 t: C; F0 l6 R& m* P
- }
9 W% n! l( n. c( o3 y0 r$ Q - + q0 G" V/ x9 }
- pinMode(_DataPin, OUTPUT);
6 r/ |2 F: U2 X+ w% I - digitalWrite(_DataPin, HIGH);
- C I3 a M+ u Z% \5 P* K
2 S5 ^- h# j$ ~9 v8 e0 d4 S- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))( l9 t: r, B: C7 e
- {5 z2 g; W, h$ y" q
- old_data = Sensor_Data[0];
7 B ~* s4 _, ?" B4 [ - return Sensor_Data[0];
6 Y( t1 L6 Q4 H9 d }: R - }
+ E* w: E. F! u - else) }- N3 W) {& A6 G, p. z5 R
- {
% ~# O' o3 p: l( R - return old_data;
, Z, ]/ T% M1 J* L( F0 z - }# }1 h) ?' |, z% R
- }9 c- E3 b. J5 C
複製代碼 " [, t4 E5 P. z' d
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
/ o3 n6 N- }% S" W6 X
) _+ _/ V8 |) A- ?3 M |
|