|
|
* l h2 J5 f$ O4 e) M, m1 h
- #include <Arduino.h>$ W7 l L5 v2 F( g8 l$ v: r4 L& y
- #include <MeAuriga.h>0 k. ?" @6 Z4 {! k8 O: E6 a
- #include "MeLineFollowerArray.h"& x* W, E/ d; T" ?1 B# Z0 l
- $ T+ Z" J! B7 _
- MeEncoderOnBoard Encoder_1(SLOT1);2 @& N& ]5 D: O' g
- MeEncoderOnBoard Encoder_2(SLOT2);
$ M$ H" }5 {8 x$ _4 L - MeLightSensor lightsensor_1(12);& N2 t- W) }+ ]* m1 f' ^, P
- MeLightSensor lightsensor_2(11); J0 B1 A! y" V8 {7 b6 u
- MeBuzzer buzzer;
! Y3 D9 c2 a% l, g9 j9 a - MeLineFollowerArray linefollower(PORT_6);5 H1 i% {3 N# Y( ]* W! V7 ^
& T0 ?& l6 I3 U! b- #define Error1 12 ]- i0 k; |2 X2 ~8 [7 s
- #define Error2 24 E I" @0 b* ^: |
- #define Error3 3
7 t3 r. U. d+ S' N/ g! i/ Z/ ^ - : T, N( Z, y6 l; m, L6 m
- #define Kp 159 a! n) R R% d4 ?, \# F6 ]
- #define Ki 0.15
- m; X* ^. {1 v6 X q/ ?) h2 u - #define Kd 0.032 a: E2 T5 \* }) a7 F: [, a" P
$ C# L4 Z, q# A% X/ a" l- uint8_t sData;: |3 W6 e$ T- m) S8 {9 ]0 F
- uint8_t D1;
5 B* b3 w, l* W3 ~8 M - uint8_t D2;+ m, W; [9 _5 k1 l7 e
- uint8_t D3;
& Q9 I2 R) M7 ]* }7 a5 O - uint8_t D4;! j3 y3 O! l) J {# ~( C
- uint8_t D5;& z. @6 r# g) Q. q i6 }
- uint8_t D6;' @1 ^9 {+ b+ [) g' o2 _" n" z' Z
- ) M+ X; }0 N& \4 X5 g
- float previous_error = 0;
# ]: A9 V; r, x9 F: C: G# }5 i - float integral = 0;
5 p4 @4 R+ N, Y - float derivative = 0;
o( \; I; @' D7 M$ _ - int Speed = 160;6 J. E4 v2 A9 `! R
- float output;9 P* ~9 Z4 p* b1 M2 S6 t
2 q9 k$ z! i. C$ V& i& H i+ I- byte Left;7 ^# b5 b2 d# V% T+ U8 J2 l" b( ^! v1 h
- $ H! l5 W _1 b g& Q
- void setup() - ^* ]1 \+ w6 D
- {
: d p' M3 Z* O8 ?0 } n - //Set PWM 8KHz
' H8 U) |7 O' t* V: K1 P - TCCR1A = _BV(WGM10); p6 h7 N% _+ D! u$ Q4 _
- TCCR1B = _BV(CS11) | _BV(WGM12);
% Q; _% G, L0 ^% D4 {& ? - TCCR2A = _BV(WGM21) | _BV(WGM20);" Z- H( o+ D9 J8 P$ x( W
- TCCR2B = _BV(CS21);# ^/ D" V- q8 F% p
- Serial.begin(9600);3 T9 \0 ~& K% C( I9 T
- buzzer.setpin(45);
9 Q4 l, C7 D e+ S# j - }" |- v m M) |# d- x0 v
" }( z, T3 ^5 P8 C* M+ G L0 e5 g7 {- void loop()' u% w5 P& {6 M- g) x: e( I: i0 T
- {; v0 Q L+ \: t! @
- while(!((lightsensor_1.read()) < (10)));
& o* }0 s0 s' X; t - buzzer.tone(1047, 500);1 b4 b3 G1 d/ L6 Z& a! H4 c, M
- delay(500);
8 ?" Q. _ G3 g - do7 M$ [* C+ P( g) T- k. X. M+ ?
- {
* m' t& N5 t( I- S2 n - int Err = getErr();
8 ^" x/ d4 B% b- q7 B) T2 S" x) d/ Z - if(D1 == 1)
H2 h {! I5 E2 V% w4 t - {
+ b& U, @1 S# x ? - Left = 1;' F/ K, \4 Z8 Q/ D A
- }$ d/ i; t$ ^2 ?2 R( ~9 {
- if(D6 == 1)0 B3 ]# V) I: N
- {
1 b# H; T& ^. ?$ f - Left = 0;, J, y" \) k& o& k9 B6 Z
- }
3 b2 _9 p0 X) H$ | - if(Err == 99)3 `5 W. l/ y. P: g+ n" K
- {
3 M- @* j3 K6 q! o" Z' q - if(Left == 1)
- U' T& _2 y" r - {5 q, M j. Y: B8 _, b* c/ o c
- Speed -= 5;7 P5 \5 W9 K# h2 U U. }
- moto(0,Speed);4 r7 [% _6 I! w( R0 E
- do3 v, |0 I% F1 n+ |+ d N
- {$ Q6 G& G& l7 T0 Q; y
- Err = getErr();
3 f& @, p. F- u$ b& l4 c - }while((D1+D6) == 0);
. }; s4 x+ x2 n, \8 l/ s6 G, u - }
$ V2 E. p% a* { - else9 {4 v1 j0 m' W H$ s0 m! \7 U
- {3 i# M0 L* \6 H, Q/ [( Z8 z" h
- Speed -= 5;
6 k0 a) {# p$ z1 A% b1 V9 v5 b - moto(Speed,0); ! a- F, u, u% Y. a3 v" x) k- h
- do' K+ A2 A9 k* r4 p* _
- { Q5 j8 V3 \* K5 T( [2 Q3 q
- Err = getErr();% ?9 n, Z/ Y M3 G) |$ v
- }while((D1+D6) == 0); I+ ]/ y9 B: H: u
- }1 k% Y: S! H" j" D6 a5 e1 \6 @
- }. J' i# a( J q5 l
- else. a6 v: \. ]5 k
- {; m9 ~3 I9 x5 L5 u6 w' k
- if((Speed < 160) & (Err < 2)) Speed+=1; 2 _, n9 B6 k3 {' L7 e$ {4 e
- if((Speed > 100) & (Err > 2)) Speed-=2;
/ j' h! u# p, K2 L - integral = integral + Err;
6 \- r6 f$ G/ v) }. ]/ {7 U0 y: ^ - derivative = Err - previous_error;
{: `& J. z% s; a - output = Kp*Err + Ki*integral + Kd*derivative;# B6 ?" M& c, ~( v, a
- moto(int(Speed-output),int(Speed+output));8 p5 p2 a3 i$ Q) n& Z
- previous_error = Err;
( a2 S& ?8 e3 K4 s, F) p9 N - }
' F% u$ j# [$ w+ e$ K- t& a - }while(!((lightsensor_2.read()) < (10)));4 C, S: f- p/ w% q1 a# {" G
- moto(0,0);$ y+ v* K' A" R+ X: J1 i# g: d( q
- delay(500);
! r! n2 G# F$ i+ w9 ?* A4 d - buzzer.tone(262, 500);
* K. ]0 [1 c w. h9 C - }' V8 c1 m0 l9 \5 M- W
: T! S3 H9 u7 M' _4 s& d7 _- int getErr()2 m: }5 G6 b- \1 d% D0 R
- { 5 x D5 T% p' Z% j
- sData = linefollower.getValue();
4 D( x x) @* _5 A( U - D1 = ~(sData>>0)&1;
k# W; h& f+ i7 y - D2 = ~(sData>>1)&1;
. ?7 o! @ z9 e. \5 F0 L- u3 R/ [- H - D3 = ~(sData>>2)&1;
3 H) t' w } [* e' O9 R - D4 = ~(sData>>3)&1;/ D7 b5 Y% V1 C: v0 }/ R
- D5 = ~(sData>>4)&1;( ?1 ?7 h: H. L* j
- D6 = ~(sData>>5)&1;
' r# H1 x0 h" F) p - int downD = D1+D2+D3+D4+D5+D6;
" S# d# o, k# r% z+ G+ O& x5 K w. |0 P& o - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);+ p1 C' u6 a) Q! H+ |$ P4 |2 h5 a' a
- if(downD == 0)
& z: I( N& u, j# A1 ^' h; E - {
/ m: }: A0 n, y7 `: Z - return 99;
% D+ e& ^2 ]6 J# X7 Q4 c0 v' l - }* B, z+ p$ d5 I$ \+ D
- else
: t) F( {' n7 A! F( G5 O - {
+ _8 w" d* J7 x+ u, ~ - return upD/downD;. K A1 A( Q' K4 ]) s: g
- }
- P8 C9 w4 p! s5 i - }
$ z3 b6 i1 i: p3 s
7 f6 i0 L* m. ^0 @- void moto(int Speed_L,int Speed_R)
) v5 ?% H+ H8 y0 I$ Z. Z - {* j7 S( u: A+ I
- Encoder_2.setMotorPwm(Speed_L);+ x$ i2 Q( ]7 s( x- _
- Encoder_1.setMotorPwm(-Speed_R);0 D- Y4 n' M! v& f
- }
複製代碼 4 U0 s K8 g$ ^" n, e3 I
MeLineFollowerArray.cpp
. o0 C) y! w' w0 I8 ~- #include "MeLineFollowerArray.h"
( G; G( R8 V) N* J5 m: ]) j2 Q+ W - ' e( u# }2 ]2 d/ Y% [9 F
- #ifdef ME_PORT_DEFINED1 M- y& {+ }1 h# U# J: P( ?
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)1 d1 s& x, ~" g! a* A
- {; \ a$ a5 e# n: Z) l7 j# H+ C) p3 O
- g b. M5 l' z$ f
- }
( ^6 e: F$ P" P - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
1 L2 A O3 y9 q' m5 T) f% e$ E0 A - {7 m S( S X! l( S) m
- _DataPin = mePort[port].s2;: u! f) A' H8 ]. S2 |: Q$ M
- pinMode(_DataPin, OUTPUT); 1 R' Y4 J$ l8 s6 J
- digitalWrite(_DataPin, HIGH);& J0 T. ]6 K$ K
- }
6 Y( e# p5 _, |$ U. {. e! d& [ - #else // ME_PORT_DEFINED% T' A2 `; }0 y& Q' F- W
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)6 U4 j: }4 h6 o4 ?- @) f
- {
: f, P, c( n: w* i - _DataPin = pin;
8 z8 m" P2 G- E( T - pinMode(_DataPin, OUTPUT);
$ P7 E+ P6 C6 V9 p - digitalWrite(_DataPin, HIGH);9 e/ Y! g9 n# `
- }# a8 q' ]& g! r0 X2 A
- #endif // ME_PORT_DEFINED6 c8 X1 D/ ?3 r1 [
( D( Z& G: e+ Z4 H$ m* n9 ?
6 n* `) [8 F+ v" |& P- void MeLineFollowerArray::setpin(uint8_t pin)
" d/ ]" v" z5 m. F( W - {
+ U9 k- Y, l+ n6 ]& Y, @ - _DataPin = pin; R8 p5 b+ ^2 D1 @: I
- pinMode(_DataPin, OUTPUT); & ^8 X/ n; i+ d( S, n" X* f% B& L
- digitalWrite(_DataPin, HIGH);
, B7 j, M5 e7 d; w - 0 V/ h0 l+ ~- v- @# V
- #ifdef ME_PORT_DEFINED- z3 G7 Y, x( G6 }; w/ o
- s2 = pin;
8 f& M1 |8 S) Z - #endif
6 \, R9 ~: L9 w& Y8 j - }
7 V1 z- i4 A5 m* e
9 x% A" p( z+ |$ L( U3 s- uint8_t MeLineFollowerArray::getValue()
' ^5 x, E, C$ X' y; ~. L - {1 @! j2 R: P( R: n
- uint32_t LOW_level_read_time;* X6 A" C0 h7 `* D1 {
- uint32_t HIGH_level_read_time;5 K: I& n/ a) A$ d7 P( d& \ h& l
- uint32_t time_out_flag;
. n3 O6 }# @4 m- ?/ g - uint8_t Sensor_Data[3];1 a- Y' ^0 c1 M2 G* k
- static uint8_t old_data = 0xff;
. h, A k3 i4 G1 D* M* _
: _# C7 w- N- p) `- pinMode(_DataPin, OUTPUT);& y; J# P% S2 j0 v$ F
- digitalWrite(_DataPin, LOW);
% Q. `8 |+ J% i. h% b/ L5 F - delayMicroseconds(980);3 i6 s- G9 s$ t0 a
- digitalWrite(_DataPin, HIGH);
! ^: _( X. [4 p0 N; A
- D7 u1 r9 u4 ~4 [- pinMode(_DataPin, INPUT_PULLUP);
8 m. S/ Y& J% A7 M$ V6 C& D: } - delayMicroseconds(10);
4 E& T( ~( y# _' @, {* a
: @. e( W' d2 k! g: F- time_out_flag = millis();
$ P2 T0 F5 p' [1 F! | - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* {6 W" W/ M; k
$ V# a2 D( |$ v/ e- LOW_level_read_time = micros();* ]3 {& ]" H3 f* m# ]1 G
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 |- m, }( g- l: ?! B8 J - {# ]7 e6 _0 Y5 F1 {0 I8 U
- return 0xff;' {+ P+ a8 u& {8 @ R+ a
- }2 E4 [% \8 S7 F# x7 P/ r. t
- 1 g& R' |) s. r6 N3 A3 x3 ~% [
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );2 y! T' D# P/ ]; y4 u
- 2 j8 j6 ^ d1 u8 Q z. I) C
- HIGH_level_read_time = micros();
. A" b( D& ?) T" k# j* @/ [ - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level: u3 K$ ]! B7 S! c, e
- 3 ^ s0 k# Y8 @: M9 G' o0 P
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 e- d' r- s1 j6 b$ O
- {
/ s) b# @0 ~( l q4 T. D - return 0xff;2 C- b7 C5 n- `2 C6 Q; [5 w- s
- }
/ i0 ?+ O$ |) X3 {
# H" \8 J* \2 f8 T- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
/ ^4 q0 |# a- Q( B) k( g! u! W - {4 G! s3 ?9 j* g4 c% @! O# t: v V
- return 0xff;
& ^+ P5 e3 h& i0 d- \3 _ - }6 X7 ^" m0 V2 }* C# ^
: p5 S( l6 M" S& c. l9 H( c9 }( H9 r- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. v4 i/ ?+ i. V( D - LOW_level_read_time = micros();- K' `/ S- I- z, v7 n5 r# K
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
7 y# q8 W1 M4 _; l5 m, f - # @3 {& O$ n$ A& ?: q% I
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 L. b5 y+ `) k: z+ z - {
4 U1 G4 O' E1 Y) g2 y% d - return 0xff;
2 u3 n0 R [/ ?! \9 `- o( U - }
; O3 m* h! A9 h- T- e. t& e. ^6 g
$ B4 c/ u- {" M1 E- y- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))4 f5 ^8 E* o8 m* t4 k; s
- {4 R$ G; M+ Q0 p1 G4 C8 j4 L
- return 0xff;2 A' \# n" j# T* x2 i+ y
- }
: M7 r1 m/ C$ z( C0 {. L% s# Z. F
5 `2 Y0 a' y# D6 E$ B" |- for(uint8_t k=0; k<3; k++)
7 p' C9 J, H* a% X8 W. Y- k - {
/ n s, E4 S* ] - Sensor_Data[k] = 0x00;
. R1 U9 w% M5 b# P$ Y# k: D- x8 E - Y) o( i- ?! b: ?- ?2 A2 `2 m
- for(uint8_t i=0;i<8;i++)
+ {& G$ [ p, t$ S$ B0 d; C4 d - {
{1 h" j6 k7 E1 K" ] - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level1 {( t8 Q) y% z0 v
- HIGH_level_read_time = micros();( t$ z& }8 s( q4 l" X7 x4 [9 s
- LOW_level_read_time = micros() - LOW_level_read_time;
$ G) `/ S$ ^4 x: }1 [5 Y - : u& |0 x% P% d' }# }# C. u
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
; [& O# ?5 w3 v - {
c5 R5 s+ g6 f# ^" o/ J3 R' `& ? - return 0xff;
8 K9 w1 Y/ g$ Y' b% t1 j - } k( q5 j; @! R$ h- f
- 0 @, w3 o9 O2 \. K( e
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ m* E- T* {' ~5 m. }, m/ P
- LOW_level_read_time = micros();7 s+ V+ o' S2 g
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
6 b' e7 G0 G4 g4 }
* a/ v! r+ b* p# S- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
7 k6 D- s5 t$ J - {3 G2 G2 Y' \$ l% o5 A% h
- Sensor_Data[k] |= (0x80 >> i);1 p; [3 W/ O% Y8 z) V7 F1 W6 o
- }/ g6 U. }) M% M' g2 Y* Z0 o
- else if(HIGH_level_read_time >= 100)8 q/ w! p. q' Z3 t) `' N7 M1 z
- {2 X1 H" E5 Y5 N
- return 0xff;
( Q7 |2 f$ d4 ~' q; |- p, M- s - }
! y1 o9 L: [/ K, v9 C& L4 T) c3 ^
" `$ Z- ^* a+ |. I* Y9 e u7 S- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) Y# m) h3 [4 H2 ^& [3 e
- {
/ Y; W+ P9 E" L, q - return 0xff;
3 ~$ l, W- w8 x - }
. Y& I. A4 @ o3 s& l - }
0 x) |8 i" J1 s* p/ W - }
. r: S: \) G, E5 V9 y9 B - ' c3 b# C% E( N `; E. L
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
! M+ g% Q; [+ ^9 j - HIGH_level_read_time = micros();
2 O& Z# \/ y2 V) j2 b - LOW_level_read_time = micros() - LOW_level_read_time;
4 Q- b- j' M# {. O% a1 o3 B) h
8 X$ B; e7 M, u- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
6 h& Q, E* l \# R# q4 N8 { - {
+ ~1 ?' x% A9 e6 R4 @4 w - return 0xff;0 o2 O1 P* E" k) ?4 @* M% e5 z) u
- }
" M* J; X- f0 y% o' D# e- Q
" B8 `+ w' H/ v; V1 o- pinMode(_DataPin, OUTPUT);( G7 S; z& v% B: r' c
- digitalWrite(_DataPin, HIGH);
( M! g( c/ h+ J2 c$ O - : p0 {" h9 a# O+ P! V
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))9 _7 S. I+ t+ D6 W+ U
- {4 e- @% E. H* j) t
- old_data = Sensor_Data[0];
5 z9 w/ N. {! ]/ @6 L% _% _ - return Sensor_Data[0];
6 Z3 N! H: `* X8 ]! P8 _5 } - }6 r& `% ]8 g1 n
- else
$ d& H$ \4 @# H1 g - {
' `% x$ m5 [( p' g2 I% ? - return old_data;
' w5 H* M- E6 R* e- o: ` - }
" _ V; p3 s$ g" b4 L( d - }8 T2 x, D0 o$ |- ~( ]) H* p
複製代碼 / g: k; `; ~1 U5 X! m4 F
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
# M8 S3 I+ \' V; Q, R }4 S0 K
1 z8 z) b* u, N |
|