|
|
& H$ n6 J; z O- #include <Arduino.h>
0 s) m0 [7 ]# c0 ~7 _$ p# L - #include <MeAuriga.h>
k# S! J4 V+ d. ?; `3 c. { - #include "MeLineFollowerArray.h"' S9 J7 j& [3 B7 G
3 p* R1 R; T: U) B0 n, j- MeEncoderOnBoard Encoder_1(SLOT1);4 j# u7 c1 G. d3 o, C6 l
- MeEncoderOnBoard Encoder_2(SLOT2);
5 P* p2 f) ?# H0 v3 I/ j4 l3 G$ ` - MeLightSensor lightsensor_1(12);
# V0 d% ]+ c9 B - MeLightSensor lightsensor_2(11);: Q2 |2 F+ _( w: p. X( P* Z0 b
- MeBuzzer buzzer;& G$ E. S! Z; o
- MeLineFollowerArray linefollower(PORT_6);
' l b! u5 \6 I5 P
4 m8 d& q6 o& H- #define Error1 1
: b! y4 u1 \* f4 I - #define Error2 2
& ?, d# R5 }/ \, r" d2 ? - #define Error3 3
. J1 I/ u% b8 \! P
+ ?. n' \( S; `6 s/ c; U6 P0 h- #define Kp 15
4 f- Z2 l8 s% }6 X; Y/ ~% q - #define Ki 0.15 [/ i2 T( k( x) s1 X3 }$ ^9 C5 t/ Z
- #define Kd 0.03- k) v2 F3 k1 Q4 j6 V
- , c' s: S& R% Z) K2 ^
- uint8_t sData;& Z4 r( r! h8 q1 B) [. d
- uint8_t D1;
/ p; i/ n! D7 I: O* q% i1 D$ F - uint8_t D2;
( |% t8 D* O# u3 W# g* r3 [ - uint8_t D3;
3 T4 Q1 w1 i/ ^, e- A* n: f) H- w) z - uint8_t D4;
- }1 T+ J" J N/ A/ t, @, L' { - uint8_t D5;- @# b4 N3 z- g* F$ `: W
- uint8_t D6;
8 m" O. [3 {7 \5 J4 `, r. K, u - 9 Z ^0 {# i' N2 K! ~0 `' `! ^) [
- float previous_error = 0;
V1 n8 S& b# | - float integral = 0;
; |" e, z( ^% V5 w - float derivative = 0;3 n$ t1 a0 ?. H% _1 X) v/ k
- int Speed = 160;5 `& i4 ]# A T6 P& K) g7 o0 z
- float output;
. L' s# X1 W# M; u3 V- t$ c) o# m - . `+ y% R7 [. {* w
- byte Left;
+ N) I: z0 n# M7 V1 e0 p
; T! R" h4 I* b- @; ^* s. |0 U/ r- void setup()
2 \& ~2 I/ F! T: g - {7 Z8 c$ Z ~$ ?' Q
- //Set PWM 8KHz
1 x3 T Q! |9 O - TCCR1A = _BV(WGM10);
}2 f R& f5 J6 j7 o - TCCR1B = _BV(CS11) | _BV(WGM12);8 b' W& s/ H) U: x$ ~+ A+ Z9 L
- TCCR2A = _BV(WGM21) | _BV(WGM20);2 D% |( j- U' |7 W4 z) p
- TCCR2B = _BV(CS21);
6 m4 T2 n* M+ H, m - Serial.begin(9600);
/ a/ g" l% j( k& o - buzzer.setpin(45);( ]$ w4 c7 K, z. p x
- }
7 V; n: X( ?3 Y! I
# ^; \0 c/ h% Y: H6 X% s ~/ F% J1 b- void loop()" N2 ~' }3 o1 w* H
- {) K- y5 l: ? o1 q
- while(!((lightsensor_1.read()) < (10)));6 t w+ ~8 @2 }/ F0 M6 C4 t' ]
- buzzer.tone(1047, 500);! D/ z% `4 _8 |% ~1 |( z! X
- delay(500);
* i/ h; U! J0 @# N9 A - do8 Z. P7 A5 t# B" v1 d5 d1 L- f
- {
+ f- s3 d$ S# q: x/ q( C! H& f* H0 Q - int Err = getErr();
% [5 v" A' W! P& Y1 G, z - if(D1 == 1)7 ^9 P6 E. ]$ o; i: ?7 `% q+ g6 r
- {6 a- ]( Z. V1 S/ r: N* E
- Left = 1;
& B/ J8 S* \9 B* m1 N, v - }
7 s9 {5 {) r h2 }: G2 M - if(D6 == 1)5 r) s% Y/ S5 t' A7 a
- {
1 i! d4 G: d% J+ o0 ~& T2 O - Left = 0;" }" o0 r# c' T V3 |
- }
1 e9 Y! j* T# E+ M3 P - if(Err == 99)
% R6 a) g5 H* \3 Z) t3 e - {$ O+ @" [ l% b- ^; U
- if(Left == 1); _) b, e3 j7 ^3 l; S- t
- {
) [9 d+ z0 e6 l; q - Speed -= 5;; p( f% ` W- z( }- c; j/ Q* y
- moto(0,Speed);! ]( i) e- l* F1 Y
- do
5 T- L6 V J) i: R2 z - {
C7 h# Z% ?! t; A5 S" j - Err = getErr();: l- y& I7 a/ N. ~
- }while((D1+D6) == 0);$ B+ P X3 U1 q4 n
- }
; ^4 J* L4 @' U0 C( {/ k - else
; ~7 p& n" |, B/ n. }1 M - {# q: Y: k1 Y" }& J) W* h
- Speed -= 5;" ~/ C) {* U2 e
- moto(Speed,0);
V- ]& H7 g: O - do) o4 G t/ T0 |! b$ M. `
- {
- t* e- _: H. O6 k) ^% s' d - Err = getErr();: }& Y! |" D' K, W
- }while((D1+D6) == 0);6 d, m5 j# O) J' S. N+ U3 ?) ]% U
- }6 r% W4 q, Q) W4 A
- }
7 c) V }" P8 U# u" X0 h - else$ x) X9 M2 V8 }5 M
- {
6 C8 z7 k) Q! F6 u - if((Speed < 160) & (Err < 2)) Speed+=1;
4 C3 B% i" u4 [7 T t R - if((Speed > 100) & (Err > 2)) Speed-=2;
/ X' D/ b8 \$ c4 J: t" N - integral = integral + Err;
0 V0 Z# D( L# @ W ^4 { T, A5 O - derivative = Err - previous_error;1 n: u3 n$ j$ Y# |
- output = Kp*Err + Ki*integral + Kd*derivative;3 O, H4 @( P4 K- u# o; N
- moto(int(Speed-output),int(Speed+output));
2 S. U6 ~' X* T3 H& E; ? a& e% h - previous_error = Err;: h) r" U( F' h$ G6 }* y" Y" h* N& e, r
- } x/ j: L$ E5 Y- m# n7 h) {
- }while(!((lightsensor_2.read()) < (10)));2 ]7 o- [2 j) x+ _7 N: Z7 ?
- moto(0,0);
& F* |3 \( b6 j( u - delay(500);. B6 l3 ?4 H4 L6 B8 F6 A
- buzzer.tone(262, 500);
- o6 G. r7 N6 L - }
. X& n5 i* ~/ z
0 c" p& Y) j! E) _; a- Q- int getErr()
3 ?, ~* O, ~* D6 b% a - { / A; Y- S4 \0 U: w c1 M- c# T
- sData = linefollower.getValue();
% f4 i. t9 H& e* o* v& D3 [4 d - D1 = ~(sData>>0)&1; h. B% {5 F& l6 V
- D2 = ~(sData>>1)&1;1 v9 c4 {+ u2 x- k' E& G. W
- D3 = ~(sData>>2)&1;" o) ?( @7 j& u. g1 r0 I a
- D4 = ~(sData>>3)&1;
5 ~) V) A, X- J i - D5 = ~(sData>>4)&1;
: M* @+ O2 B# B( h0 ^ - D6 = ~(sData>>5)&1;* i- _: I8 @6 P4 |
- int downD = D1+D2+D3+D4+D5+D6;
& ]. J( i/ I( k - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);+ W5 P5 i2 v4 I: v0 t6 y2 x1 P
- if(downD == 0)+ F/ z0 f, h7 F+ s- c
- {4 H# S$ i4 L& Z! a" V) f
- return 99;5 @, S* z7 D; Z* s& A, r% D
- }" D9 K( r$ c% k$ u& Y; d' Q" t
- else
2 W' R/ [ G/ L5 A - {! m+ u/ T# p6 a) E( [
- return upD/downD;. }3 q3 e1 q. G: }
- }9 W* M2 M6 l+ f/ {% H7 t
- }
! |' e# j1 a% j+ M; G
8 B, V+ D O R7 U! Z. ]+ t- void moto(int Speed_L,int Speed_R)
* n/ k2 J5 r% f. _0 U" ~ - {
) c* z7 u2 ?/ s: u8 V - Encoder_2.setMotorPwm(Speed_L);
7 I( F; K- h& Q y" d- `# S8 ^ - Encoder_1.setMotorPwm(-Speed_R);3 W7 k& F" [8 b @, A
- }
複製代碼 / e" G$ N2 @( n# |6 Y
MeLineFollowerArray.cpp
# {2 h/ D6 e# T# W2 p- #include "MeLineFollowerArray.h"
# I! |+ e& D; w/ W' r3 |! B0 `3 D; u - . U1 E0 h3 I8 p/ l V0 v! @2 M
- #ifdef ME_PORT_DEFINED! E4 H6 ^# g7 ]
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0): v: [2 S. n& t! p2 X
- {% w% D* I7 g' F/ B8 U; F/ G) A! N& B
- 3 ~' j8 x1 [2 r7 {0 q! m4 a
- }
5 F! m- k% k; r, [. r - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)1 Y3 V9 r+ O3 P* s. f( x
- {
0 y3 o) h( ~8 a2 |6 u5 q - _DataPin = mePort[port].s2;8 ?7 \8 @* }; V* r& I6 ]
- pinMode(_DataPin, OUTPUT);
5 p5 \2 O9 y4 m - digitalWrite(_DataPin, HIGH);( `5 t. |6 r7 j7 t7 n- @, j( B
- }( {( Y$ {/ X$ D2 ~9 x
- #else // ME_PORT_DEFINED9 ^' G y+ r5 l' c( R
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
8 j: j3 l, L4 z5 ?- J7 o - {
! Q" S e4 U1 q - _DataPin = pin;( W* g0 b5 w E; t# u
- pinMode(_DataPin, OUTPUT);
) ?8 C c: Z* `) w* r - digitalWrite(_DataPin, HIGH);% r5 v" p0 \" r/ R4 L3 {( ?# q
- }
3 l4 z- S$ n" q0 q - #endif // ME_PORT_DEFINED
$ C3 b3 M) W. Y
: X% E* s/ @( D5 `+ x
( }& T& T4 e, t {- @- void MeLineFollowerArray::setpin(uint8_t pin)
3 S4 \3 w* Y& q - {- {1 z: [1 D" h, m% j8 Y* v* i; R
- _DataPin = pin;
4 p% f9 u6 F/ I. d S6 S - pinMode(_DataPin, OUTPUT);
# v% @, j T n7 n - digitalWrite(_DataPin, HIGH);
" `# n: X S( K0 c4 C8 G/ w - 0 l7 \( I9 `( ?" r3 {
- #ifdef ME_PORT_DEFINED
/ }' g5 `) L" {. N- L' h - s2 = pin;
; X2 U) h9 Q4 S/ F/ C& y - #endif* U$ Q' o! b. G/ f% r
- }
/ z9 K h: a2 S& O+ v) Q' P3 K
5 Y1 |9 G0 G# A3 S5 i- uint8_t MeLineFollowerArray::getValue()
8 c' ^' v# N4 H- o - {
% r3 m* }" K* b3 B- _ - uint32_t LOW_level_read_time;6 v; V% L$ u9 Q K! D" M
- uint32_t HIGH_level_read_time;
) V1 H( c: S( M$ F - uint32_t time_out_flag;( w/ ] o. ?0 f. a [* t J
- uint8_t Sensor_Data[3];
% h$ z7 q! m8 N' T1 r! k+ h - static uint8_t old_data = 0xff;
$ F9 o0 r* s0 D) K. C- U
) u$ x# b" N7 {' N$ S- H8 N0 q7 _" P- pinMode(_DataPin, OUTPUT);
, Q- L2 g& X# A& M6 a - digitalWrite(_DataPin, LOW);
" l! d" \ o1 G. ^# o$ p6 K - delayMicroseconds(980);1 v% D4 U1 n2 b3 I
- digitalWrite(_DataPin, HIGH);8 v/ ~) A' I7 p K1 Y( ]! L
! i+ o' H+ b% l! ]- pinMode(_DataPin, INPUT_PULLUP);- }$ P& ~6 x( f5 H$ ?1 b+ {6 e
- delayMicroseconds(10);
' f8 Q2 p @ q' U3 T* U - ' [5 Z$ N) @( q( Q" x. ^6 _/ ~% H
- time_out_flag = millis();
+ p' f5 h* s8 Z4 b6 L6 S: j - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
" c/ O5 B; K3 b7 F/ \$ B$ s - " P+ J2 X5 ?! w: X: D
- LOW_level_read_time = micros();
. s1 W6 q$ X/ a% A - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out( G; d* ?& Z' d9 C, b) W
- {) p' y Q2 Z2 x
- return 0xff;
6 ~& A3 S0 D8 [+ f: o - }
) v. K/ A9 d2 f" b9 q0 v/ @' M - 3 s9 A) C% a$ L! w; K
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) c6 z* a/ h' K3 {/ _7 t
9 n' S j! Z# g1 n& {- HIGH_level_read_time = micros();" i9 X7 f3 c$ M
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level6 D7 U% [: `* i
- 8 N' I4 _: x$ c0 V9 O
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
! e2 c& N& e. G* `# u - {( v1 o" ?- [- [2 M7 m% R* K1 {& X
- return 0xff;1 j# g" o+ v0 j. e. @% _
- }
8 O8 v# ^6 U! C% @" s
7 i1 w2 H: M+ t i/ V! f0 f1 h: X- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
4 J$ e& I. R( i( }! \2 i2 F - { P! q w2 F- P4 K& q5 n
- return 0xff;, v6 A& C. g, e% v0 N
- }" n6 J0 N* A' u; J# f
- - e! f0 k1 q$ h- ]3 W
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' }4 o# P) L- h9 J0 }& ?
- LOW_level_read_time = micros();# S8 a& k6 ^, f* u$ O2 h' U4 u
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
" R1 w2 K* b/ s% j8 ~
/ ^+ i, w1 n ?* _, ]- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* d! I8 M* Q D1 a$ c8 i$ @
- {
3 M- e& {3 d! r1 C; w7 b. e6 I$ t - return 0xff;
, m1 B" |' [; G2 j3 y. x5 I - }
& o$ y2 |( i4 z4 d7 R - 3 w; G9 v c3 [; @) E
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
u. g# G8 }. i$ W' P* d - {2 r* ?: p" z: O1 M8 L# D2 C
- return 0xff;" ^2 y3 P% [- |0 Q4 b, a
- }6 D' x0 c8 U7 N2 V0 _5 ^0 N
- 3 C4 ?! w% h& g" q% T8 K/ u ]8 u
- for(uint8_t k=0; k<3; k++)
$ Q& S/ L! [" Z - {
0 V: L7 ?5 ], O$ @: b, k* S- |2 b - Sensor_Data[k] = 0x00;& V" T5 ]- F2 \9 a/ ]% Z5 K6 L
- - X* O2 b5 H. ~
- for(uint8_t i=0;i<8;i++)
I) M1 j4 x& O7 p) y: m - {
4 ^% d, {% N" {7 h; p# w - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level: T3 g4 F0 n8 _( s+ ^' Y/ N- x
- HIGH_level_read_time = micros();; L# e0 @, W# V: e
- LOW_level_read_time = micros() - LOW_level_read_time;- q @2 d+ a+ U
- & a( y- `& {$ o1 @# t8 Z
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
1 U8 G( w( ?$ ^; e: l; I; O- S - {
; [. N7 p% F9 S1 H/ M( O% H - return 0xff; z7 l4 Q6 H! ]' ?0 d6 W- c. E7 X
- }
6 _, @8 g, q8 g J. S8 h
! }- f: N; q g) Z; u& U) K0 p- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- u( K/ e% Z3 q; Y
- LOW_level_read_time = micros();
( y* n/ V0 I+ F) P p2 r- G9 o - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
& H6 { U+ p( f8 c5 k C( Z - 7 Y# `& D! }* i3 Q6 ]
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1; \- ?& @! {6 ~. l
- {: B; G$ Y6 X! R% w4 F
- Sensor_Data[k] |= (0x80 >> i);& }7 |! y" @7 T( E" D
- }
/ y, k j/ m6 p9 Y - else if(HIGH_level_read_time >= 100) ^5 Y( Z' {* y
- {; K# e) U3 \! o/ H6 Q
- return 0xff;
- c9 A/ r: s9 V# y% d; Z - }
7 J. i: g1 b0 {( }' X$ ]4 b# h
/ ^" u; G7 `8 @$ z. U, e- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
0 [' x2 U) `: U" ]* x( R - {
, E+ K2 g0 J' N5 e( V# l; b2 i - return 0xff;5 c9 E) y; t& I0 `
- }; P3 }: C: N: y+ k8 h' o0 n0 h
- }7 m4 Y9 b% y+ L6 R
- }$ t5 k8 j# N4 Q" `* ]
/ ^: j6 K$ w+ C: O5 z+ Z Q1 _# o. c- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level7 d9 d# i4 \' T# A
- HIGH_level_read_time = micros();# L$ R" Y5 G( y/ F9 o# P) G" v3 `
- LOW_level_read_time = micros() - LOW_level_read_time;8 b0 r( T. M" G" r
. N/ ]9 G% L5 \/ d+ R0 }7 l- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) z9 c: ]% M! _- E# O/ _
- {8 K) ~3 ^" K. v8 V' D* P7 j1 N
- return 0xff;) r' v3 p( F, M
- }
( K6 Z8 p) w9 ~3 r( \
1 n2 l( a( T0 @4 V! B7 _$ a- pinMode(_DataPin, OUTPUT);
; |+ }* z, v7 e" p5 m: U - digitalWrite(_DataPin, HIGH);3 U3 I; c) Y4 G3 _
- 0 G9 Z1 K* [: d8 D( n# v
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
/ k2 a$ D9 L' F& A. M: w- N - {& L* }1 n! |( Y1 \ ?& y' C- R
- old_data = Sensor_Data[0];
; l1 X6 l$ s% a9 s* W - return Sensor_Data[0];
0 p- o Z2 J: S* o; [ - }
8 @ C2 Y7 E, H" ~# [ - else7 R7 c. O6 L0 V, r8 r
- {* r* U3 k& m4 S5 h, W
- return old_data;9 h* H7 \1 O$ f8 g/ H& W
- }
- z+ n1 M9 K9 n" n1 [ - }
/ G$ J a7 t3 L, G
複製代碼
, v* z; ]9 Y k# ]MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
- s$ J" u# n i4 O
9 b- @1 F2 G6 h! |# p, e( W
|
|