|
|
* i+ k+ f, A- ?9 m2 J" j9 n- #include <Arduino.h>" k8 ?' o. `. J9 S, I0 L$ c' a
- #include <MeAuriga.h>
3 u+ w! ?% O( }" d/ X$ `$ } - #include "MeLineFollowerArray.h"
- m4 |0 _3 n& M6 ^* q& f - : d$ \) b8 K5 c% R; ~1 |
- MeEncoderOnBoard Encoder_1(SLOT1);' z4 _ o5 L* @% a
- MeEncoderOnBoard Encoder_2(SLOT2);. [( f+ ^' a3 I# a1 k
- MeLightSensor lightsensor_1(12);3 J, z; ^6 o/ P: c4 j1 W' k
- MeLightSensor lightsensor_2(11);- ?& \% t6 n4 F- t# H
- MeBuzzer buzzer;9 q- N0 V! V" B+ {; U4 J
- MeLineFollowerArray linefollower(PORT_6);
/ m0 F* @( D6 ~5 `) E1 g; a( g; E - , K4 H- Y' m3 `4 J! Q
- #define Error1 1
% [4 y9 T0 K- Z - #define Error2 2
, Z: W9 M: \2 I9 e6 X& u U! ~0 ^ - #define Error3 34 i" Y% H& r" B, A
- ' f# a7 `4 x1 P7 R9 x. m6 L4 B
- #define Kp 15* J2 x/ X: T2 h k
- #define Ki 0.15
; M( e/ X9 \) w% H( P - #define Kd 0.03
5 r& {' J: I, t; o, Y
) D( x6 F# ~1 Q- uint8_t sData;2 B( e* f5 G6 s& i1 h+ d8 u# ` x
- uint8_t D1;8 [! d# G& n% `. g5 }3 P: _' P
- uint8_t D2;
2 ^1 B$ w- O$ B7 z6 L% `; z - uint8_t D3;. m. R7 e2 [, L6 `+ c
- uint8_t D4;
0 s, s5 O; r; Y4 ?' E3 i" p - uint8_t D5;0 M/ {8 I" ~& q7 p: i
- uint8_t D6;
5 Y3 ~% b9 F0 [" y+ ^- @9 j - 1 A- r: T7 y( Y5 n, w% G
- float previous_error = 0;( @. B3 `" L. p( E& N* O
- float integral = 0;
7 ]* Z `3 P' J2 E( K8 v/ q" R - float derivative = 0;
& y6 d: l: U5 L# _ G& H& s( t - int Speed = 160;
- C4 A# p- i8 y/ B! B - float output;
2 w( ]" k, I7 P& ?" S! e
. V2 j0 m' f( P" d1 |3 j- byte Left;+ p1 ?2 E2 _' J) C5 v
' F6 r1 j2 _/ r9 N! ?8 q- void setup()
" G r" d' i7 n- M - {
( J% w2 v7 l0 w# I' T( Q% L. f - //Set PWM 8KHz
' Y, \6 r. `5 Y/ T- d- I6 O, S% g; Z - TCCR1A = _BV(WGM10);
( {' M8 {% ?, V4 }/ N5 M2 V1 d% x2 J - TCCR1B = _BV(CS11) | _BV(WGM12);
$ S! R/ o1 ]; Z1 T" U* p' g - TCCR2A = _BV(WGM21) | _BV(WGM20);7 W% [% V: Y2 C( M! ]
- TCCR2B = _BV(CS21);
$ _9 V6 I7 f# A, j( j* u- T# V - Serial.begin(9600);
( Z* O; T8 ?8 _! A8 @7 b' ] - buzzer.setpin(45);
, R) R. P' E5 R- q - }
: w3 R3 p# ^8 ~* w2 G. F/ g9 I
$ s! \8 r# o# i' e- void loop(). X& y$ s' v! V- o
- {" l0 W; w& t- g1 K5 v
- while(!((lightsensor_1.read()) < (10)));- x( W' K% w% Q! u6 h, @* I1 Y
- buzzer.tone(1047, 500);1 n& M7 @" C v' X, } |5 f J
- delay(500);
$ {7 S, b! p) m- A H6 z - do1 c1 _; y$ m L* q/ O& v
- {! x8 ?0 e. d# ~' q* v' h1 p9 H
- int Err = getErr();% a" G/ N; k- {4 q F0 f2 y
- if(D1 == 1)9 R! J- i2 K" O, o0 p
- {
" r& F& ]. m0 J0 i/ L2 E4 C - Left = 1;1 r* D- ?0 G- T, b" Z
- }3 R3 e; X+ m9 n7 \
- if(D6 == 1)
: b! U& x: n1 G - {
$ _3 W, ]+ G3 ^' ~ b1 P w7 x# H - Left = 0;6 s4 j) m0 Q* i2 x" @
- }
$ ]! ^$ t) w' ], P5 G - if(Err == 99)
0 w7 g- _! \: S - {. C" d4 J* ]8 {7 m) Z
- if(Left == 1)6 t; r$ ^! Q% c6 _( @" ]5 q
- {% H! w7 L5 d2 n$ h- h: ~0 a
- Speed -= 5;
7 n- q' |1 W- z" q E; g - moto(0,Speed);
# \& g5 c. W, M) m, K7 f& |2 n - do
1 b3 V4 Z/ I# ^4 ^) J# @7 \" w - {
Q; X" v* j0 S' ` - Err = getErr();
8 u, ]4 |9 }( S3 s1 U) P - }while((D1+D6) == 0);
1 b4 }! @0 q$ S# C2 w2 a - }
5 _0 I3 X# n% G$ g - else
6 `! K' E2 h9 ^8 s( b) W - {' r, b& V: _# M9 O
- Speed -= 5;" }2 {5 \. P" o6 G
- moto(Speed,0);
9 S, \+ t) ]' {3 y3 N, q0 q' {8 j - do
; i! }) P6 L/ p4 T& V% a - {
/ a( B, F; v: G9 w% _( V8 H% q - Err = getErr();* R2 c9 r' ?/ I
- }while((D1+D6) == 0);
$ A7 z- q7 y/ a, I5 t - }% F* I' f7 W. m0 n7 t
- }' l; L7 d7 r: F
- else
/ t# P5 ]5 n0 v' s - {, d; [: M! X" h. d. O
- if((Speed < 160) & (Err < 2)) Speed+=1;
* v) y8 }: v+ ~7 ] I0 S# i# y, @+ H, N - if((Speed > 100) & (Err > 2)) Speed-=2;
" |/ M( ^* M5 B: w9 d - integral = integral + Err;4 S: S! s8 b+ L+ g# o
- derivative = Err - previous_error;
( e& D% R* W. Q6 m9 L+ P0 @ - output = Kp*Err + Ki*integral + Kd*derivative;; u% J: _3 f0 ]3 v: {- T/ M
- moto(int(Speed-output),int(Speed+output));
; _; f3 p" \2 Q - previous_error = Err;- V ]- o+ H8 Y/ A9 h9 G
- }+ g$ a3 l% I6 ]5 u9 g) U
- }while(!((lightsensor_2.read()) < (10)));
/ A' }5 ~/ [# i6 _ - moto(0,0);
9 C8 {7 O" f* Q - delay(500);
. P5 Q1 z! S3 Q; u7 F - buzzer.tone(262, 500);
9 W A9 p L1 {# w- P9 B4 e+ M - }$ n4 ]3 }5 P* L* J# Q7 h
- / x G+ h1 r3 s" T$ _+ c
- int getErr()3 ~9 ~( w+ T/ L$ U6 z1 w/ }
- {
2 W5 ]% |6 s, c - sData = linefollower.getValue();; ~. D/ V Y: C& M) ?9 q' q
- D1 = ~(sData>>0)&1;
; d3 s, p2 v2 L. O+ P, M; b4 I - D2 = ~(sData>>1)&1;% U* a* k1 F& A% o. U
- D3 = ~(sData>>2)&1;# Y; d$ Q1 \; X, X& f' M* B
- D4 = ~(sData>>3)&1;+ T* o' L# ~9 J( G1 K+ {; h- x
- D5 = ~(sData>>4)&1;7 v% l8 u8 I% P5 s
- D6 = ~(sData>>5)&1;+ A* l) a- C' T' Z
- int downD = D1+D2+D3+D4+D5+D6;
4 d+ y/ o" k: _$ e: Y! W - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
' h; H1 {# f7 t# k* K2 y+ z+ e - if(downD == 0)
0 \5 h$ w; D1 ~( q& s3 \- c! a - {
% ^( u( `1 v/ ^( K- j) G& ~6 E - return 99;8 M# p( |: c$ W$ V8 j8 K
- }
( t; B: y% B3 v! y - else
2 n7 m) o( `0 ?; i8 O - {
: {5 H6 f$ z4 K. A. J* o - return upD/downD;6 e& O, X4 O' A2 z
- }3 z2 k# v0 W% \9 f+ `
- }
* ]8 W L6 f! _
. o& S8 y0 Q; t- void moto(int Speed_L,int Speed_R)4 n. W; G; D5 ~) U0 y6 s
- {- z. b6 ]+ l: z
- Encoder_2.setMotorPwm(Speed_L);$ a9 p. t- ]5 H, d# p0 p
- Encoder_1.setMotorPwm(-Speed_R);. e6 J% K/ C( F8 ?+ F* ^
- }
複製代碼
& `8 y* i6 \+ V/ K l( G1 \6 BMeLineFollowerArray.cpp% i' P* y9 ?) \; |: L! l8 f9 o
- #include "MeLineFollowerArray.h"/ @5 F( f# X& Z8 K
- 2 g( a: \5 K: P
- #ifdef ME_PORT_DEFINED) w7 O6 h! o7 J* z
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0); u9 h: ~. a: c- Q& N! x
- {
Z; { P7 F' m/ d3 }5 S
% T' C/ Z8 c/ ?: \- }
4 j; k/ H$ c# r4 j2 \: a - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)3 }% s6 S' q( b0 `. o# D
- {
/ K! P9 E6 l% h - _DataPin = mePort[port].s2;
) ?" v! f$ q- G4 Z - pinMode(_DataPin, OUTPUT); 3 ^5 A' }: u3 N
- digitalWrite(_DataPin, HIGH);
( J* j7 c* Z3 B - }5 K5 f; N( ` g8 Y7 I
- #else // ME_PORT_DEFINED$ I U B E, j1 T
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)# r6 N: z Q. E% u/ r" E, U G' q! G
- {
% B0 N% }' X: s/ n - _DataPin = pin;7 T {6 V- Q% ~0 v% y) r/ K
- pinMode(_DataPin, OUTPUT);
; j! P, Q9 u- n2 p- K5 v! c - digitalWrite(_DataPin, HIGH);
, `: M6 N4 v! }( c x - }
/ K# N/ p: O6 J$ ^! S: ] Q - #endif // ME_PORT_DEFINED
( x8 u7 F5 E# [3 z+ y8 c
! ^' }5 q) P2 ^; ? W" U6 P) l- : f9 g; ]# o, ^
- void MeLineFollowerArray::setpin(uint8_t pin)6 H) L/ y( J/ a' v/ D
- {8 K. z/ j4 a1 K* r' k' U
- _DataPin = pin;
- T% \' ?; j7 ?% _ p: h; Y - pinMode(_DataPin, OUTPUT); 6 u, Z5 Q0 m5 N! n2 H3 N
- digitalWrite(_DataPin, HIGH); W+ T) Q/ d _4 h ?9 |# C
- % W* j7 X5 @) ~6 L
- #ifdef ME_PORT_DEFINED3 R9 l; M7 T8 w% b( `7 G+ l! X$ |
- s2 = pin;' f: ?" f$ T6 ?% O* U* F$ t
- #endif! |( d' \ Q; h. f; ?; G* c P
- }
) a3 S, m3 u0 B9 F) E$ z( G$ y - w( O8 U+ B5 F6 D6 X' `( l* R7 k
- uint8_t MeLineFollowerArray::getValue()
1 M( A. ~( E) s* l& x- b+ [ - {
0 x: U- Z# ~. u/ A6 j% P - uint32_t LOW_level_read_time;8 b6 T1 S1 l) i3 \
- uint32_t HIGH_level_read_time;4 I' ~/ H9 z. t* X/ k
- uint32_t time_out_flag;
5 p# X; M! S+ @* u, R! ]% ^ - uint8_t Sensor_Data[3];
' `5 V6 _4 ^5 W2 d% ^ - static uint8_t old_data = 0xff;
; Q5 [& Q+ a, I: `( Z# {
: c+ C* |3 [5 d9 O- pinMode(_DataPin, OUTPUT);
: F2 P. B: s8 L( c+ b9 C C - digitalWrite(_DataPin, LOW);2 v6 A0 r5 K( s! O# s! }/ l/ }$ ]/ x/ l
- delayMicroseconds(980);
% q; Z) b; A. L3 F: v7 j* V% U! E - digitalWrite(_DataPin, HIGH);, @$ t! n: E! p- a$ j. c J+ [
- & q0 K. D4 G. V! Y
- pinMode(_DataPin, INPUT_PULLUP);) l- i5 l N) B' f9 [
- delayMicroseconds(10);
* \$ X; e9 b0 C/ J - ; g0 H* P$ V3 S# U
- time_out_flag = millis();# l, K* l. Y* m& I
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; c4 M" T0 q' U) y4 p1 r - 9 q; o: c/ d2 X3 y8 @9 \
- LOW_level_read_time = micros();! B* D' ^* `) q8 p
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
]' ~; j0 D0 Y$ y3 o: \6 o - {
5 W; n0 n- w+ L3 o: y; R1 W' C - return 0xff;1 f, s# N5 _; [0 y! D0 S
- }
2 g, }1 G$ m! L - 7 v d# B/ p2 J; g
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 m; }. B1 l6 D$ [2 }' _6 ^
- " \7 K& i; \7 u9 Z
- HIGH_level_read_time = micros();; J& B/ u/ B, m9 f& d% [; V
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level8 ]5 M) U3 k. ~. i/ S
$ t0 f7 z. Y( @" M, L- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out7 Y- H* F; H" [# f2 q8 Z+ Z& ~9 h
- {9 M0 o' P, x/ f9 |* U
- return 0xff;7 x$ d7 F* _# ?* M, l$ c
- }
+ [& `+ E8 b0 t# [1 a2 | - ) C* C1 y% q( G. ]4 s w
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
* [/ Q4 V! r! H& X - {
' Z2 ~3 b) p5 [" a' a& G - return 0xff;
- f$ b2 L! T! d0 R" ^ - }
0 t L% |8 Y6 g" a; d1 z - 3 u# z' l- g+ R' @
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
/ N7 j# m" t+ N$ v( S - LOW_level_read_time = micros();( Y: I' r1 t% ]7 ]& x! i9 E
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level0 d; V) l" F3 W; k: E
- - A& R6 m* G9 U5 }
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
( P4 @( e' E: c5 y/ Q - {
, N( B0 K( v. R( B- L. q T0 v - return 0xff;
% `/ J: y0 o! H; W - }4 S3 K% \5 [# P0 {$ I
3 ]) b% A4 p6 v- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))3 I: l8 ^( u- s2 U- V# e% X' {
- {
7 |7 t* `( Q1 Q& y3 f - return 0xff;
, f b2 g4 B- q, `/ `+ B7 ?) ?* C - }
& b2 o5 \4 z c0 |: m4 Q4 I0 E+ N
2 O u" Y) Y, }' B9 M2 `- for(uint8_t k=0; k<3; k++)
$ x! A& ~1 B( s/ n0 w0 U - {7 p9 j. _% Y5 K$ B& _
- Sensor_Data[k] = 0x00;! a% u! b* X9 K' `! d; q' L9 a2 K# C( Y
- 2 B/ p* U6 B' O% z) |- W' B
- for(uint8_t i=0;i<8;i++)4 k5 p- r$ D4 P, T7 b* r7 |
- {
% v4 z, G& e2 g' o* ~* C - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
( c' [3 A3 _7 I - HIGH_level_read_time = micros();
0 O4 M, ^0 k5 N - LOW_level_read_time = micros() - LOW_level_read_time;" E" I) F* E! C( K1 R2 c
- 2 Q% ]; ?8 O8 u( T% W
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
3 y% z. i, p% B) O - {: r0 J7 Y6 Y9 H# n
- return 0xff;
) Z+ Q6 F1 `: h& F6 ^; k% L8 z+ o2 \ - }0 o- t# n" A+ E. l/ ?
- * H& F/ q- ~# S% ^. z: P3 ^
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. e! v3 Y6 q0 U) W$ ?/ b - LOW_level_read_time = micros();
5 C! l( g0 @6 c - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level, t1 z+ ~+ t* O7 s0 `% t$ p C
- - M, u5 r$ A, J
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
9 U7 K4 m! z! m& i- q# ~/ U - {
6 C E1 o% W& Z/ r2 F$ t - Sensor_Data[k] |= (0x80 >> i);
O7 j9 l# q$ E6 A" l - }
! l- S( X: F# Y. ] - else if(HIGH_level_read_time >= 100)
/ @3 {3 m- x9 t. c! z - {1 b0 b8 F: H# m0 o% J% H2 ~
- return 0xff;1 r. V" q" e8 \
- }
( I$ H3 X6 E* u. X5 Q
6 w2 u* T5 _6 e5 L1 T6 Y# e" U: j! H- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
" o: v: w$ g8 Q' Z; G - { B8 G3 ]" s4 B/ U+ d
- return 0xff;
! S! ~. ^' A) ~6 Y6 Z/ T g* @ - }
/ k/ i! Z" X4 Q3 J - }
& f0 o- A$ R# P5 G/ E - }/ `" t/ Y% h+ q7 R9 p2 Y, y' {
- 9 {! v' s3 ]( H- o, n
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level' \7 E: D3 l7 t: c% K
- HIGH_level_read_time = micros();
' t# ~ `* b9 \6 X* c1 B6 q# W - LOW_level_read_time = micros() - LOW_level_read_time;
* T0 n/ j# H9 t - B7 T% R; v: `" y1 _8 K
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )( o/ W& a, n4 I0 Y3 h$ E' }
- {
# d' q0 Y' ?1 p - return 0xff;. O) ~. t8 w! M% v5 t; \# g
- }6 x. Y0 z& K2 O2 ?8 F
- * `$ `' h0 e7 t( V
- pinMode(_DataPin, OUTPUT);) `. p" D0 y" N0 L9 l
- digitalWrite(_DataPin, HIGH);, s' s6 K- d' H& |" b
- ! G" ~) `& l4 A& `5 {8 X+ `' D
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))1 I2 M& t3 ]0 L. u9 v
- {' x: P: @3 B. K' b
- old_data = Sensor_Data[0];# }# E8 w5 f% c# m, |/ g
- return Sensor_Data[0];$ C9 G5 Q' y" m
- }% }% U$ V* A1 R4 |8 _6 W H
- else
- r; v( J/ C1 E. z) o/ x9 Q. v# M" w: H - {5 E" \- ^! G* y+ i+ U, ~8 l
- return old_data;
; Z% I4 C" ?7 ?' o. Q - }
' @# k( M7 s k: D - }" ?& ]; y: k5 L# r8 j% |6 ~
複製代碼
. v7 `- H$ q8 q5 PMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
* ?/ ~+ g$ L1 d" \& [- s7 h+ E- I% S# N# U) Q( ?3 w
|
|