|
|
, z& v5 t4 R1 b5 `- #include <Arduino.h>
. c$ b( \1 u m+ i# \: E - #include <MeAuriga.h>5 u9 H" U# @6 @
- #include "MeLineFollowerArray.h"
6 x' \& t3 f7 g" x _/ N% Q& Q
1 K& n. K( t! s+ x, P# k- MeEncoderOnBoard Encoder_1(SLOT1);
6 v* N7 g% `8 E, A. O3 ^ - MeEncoderOnBoard Encoder_2(SLOT2);$ p8 @0 P! s" @ ^& K
- MeLightSensor lightsensor_1(12);3 B9 s' w6 Y) ] G6 P- u
- MeLightSensor lightsensor_2(11);
4 q# X5 a) Z: d# p - MeBuzzer buzzer;
4 X% N! b/ u9 G3 z9 \' I2 E8 {; K - MeLineFollowerArray linefollower(PORT_6);
1 c n6 \! S, h8 R
7 w6 U: g7 |4 G N, W- #define Error1 1
8 r! l3 @6 B* ?) H3 B - #define Error2 2/ z) W. _- S$ ?1 E* K9 z
- #define Error3 3
& c3 f3 \. T2 a0 b1 e! E
7 s, R6 T4 X. P9 \( w- #define Kp 15
0 c* ^9 t- p$ d' i: C) I( q - #define Ki 0.15% W0 _: k$ X, b6 a" q* |7 C- t+ K( p
- #define Kd 0.03
% c. x. f0 A. j) Z1 g
; b6 K, X' e$ T; C, L0 W& @- uint8_t sData;: U: w5 m& A0 K& P- d5 T- Z
- uint8_t D1;/ {0 M$ Z; P9 V6 z3 t0 T7 d/ V
- uint8_t D2;4 k) S) v, Y1 ?/ ]8 a" j5 g+ ^
- uint8_t D3;" D; A! {8 T7 d8 Z# b: _
- uint8_t D4;' M$ O1 I3 f; K1 L6 z8 w _% a4 u
- uint8_t D5;
+ A) l0 J0 X& l% Q3 q - uint8_t D6;
2 U5 Z: G c$ t- k
, _, Z" s `/ O. B2 d- float previous_error = 0;
' d# h" r; A7 d2 @: C+ Y+ ? - float integral = 0;! w4 p+ [8 `' a; P# h
- float derivative = 0;
0 c* g% W/ @5 t" t - int Speed = 160;
% [, E' T, X8 ]/ K- [ - float output;! W# l8 G R3 [) ]
8 U( |. ~/ ~7 b1 B) @- byte Left;& W. t! M5 ~. p2 C! L# H6 `
- 6 Q" ~, y; K. z, W6 t! d
- void setup()
* s) q( H' ]4 g, Q) o: n6 h( g( L2 c - {, N( Q! i I) ]7 q
- //Set PWM 8KHz! r' y* m6 n* t# M' X- v& J
- TCCR1A = _BV(WGM10);
; _$ w; z! t3 X' \: E. K* u( _! K - TCCR1B = _BV(CS11) | _BV(WGM12);
& F2 Q8 M/ x- E* i9 O! G3 b - TCCR2A = _BV(WGM21) | _BV(WGM20);
2 D3 I9 l6 W# W; \6 X) T% F, m- J - TCCR2B = _BV(CS21);2 X/ D4 J# m/ T+ h" f
- Serial.begin(9600);- A) O! o/ Q5 r2 c! O# J# a; j* C
- buzzer.setpin(45);: I' W! b" c' _9 H
- }
1 s# x0 }2 m B) k' j3 o7 Z
2 b3 W" V0 S( r( Z+ r- H( G- void loop()9 ?" I2 i7 }* Q$ R
- {# x2 c& @' T$ H; \, B u
- while(!((lightsensor_1.read()) < (10)));
: {$ Z2 z. N0 A: \ - buzzer.tone(1047, 500);" S3 V0 ~3 c' Y6 g) F
- delay(500);1 |, N4 Q' g4 I# ^; f% c, o0 P" U
- do
2 E) e1 }& P9 n d2 M - {; \& ^: z' x) K: T5 w
- int Err = getErr();) J& a$ T$ Z. r$ C
- if(D1 == 1)
) X( h2 f' `1 `# }. B - {
a- g# Z6 D: R* \+ U3 K' K" L - Left = 1;
/ G/ _0 ?6 T1 [5 A( p, X - }1 I* Q @& L. Z8 _! L# E& b
- if(D6 == 1)
9 I3 E+ F ]5 y+ Z - {0 ]( |" [' k% _# z% C' K6 ?, ^) d
- Left = 0;6 B8 i- l. e5 S8 v7 K; K% H/ Q
- }7 l$ W/ l0 Q _3 {! O5 K
- if(Err == 99)- r; K& j3 b5 v# O l" A# h
- {
# r1 L8 V# W; m - if(Left == 1); W5 U" M- Q$ V5 s1 k$ J
- {) t# h |, v c, p# ?: h2 W
- Speed -= 5;
/ q# M- F2 g! q, T5 A3 ~7 A& L - moto(0,Speed);; d' Y [' J- s
- do
! D9 I/ }2 z6 o& m' K) S" V - {
% `4 k m+ L( ?% x7 R6 s+ I - Err = getErr();
' Z, J8 o% P0 y' r. Z - }while((D1+D6) == 0);; f( M( O8 b5 }6 g* d
- }6 K8 n V+ ?% m. E. Z8 T
- else9 D7 ^* T$ ?& B4 W) ^2 e, n/ c
- {
& N9 `) d5 f2 P' @ - Speed -= 5;7 }; Y9 o2 g) O
- moto(Speed,0);
+ G# g/ {( ?% S' n! @. a9 Q+ | - do, r7 H- C' C+ K
- {6 i7 ~9 B. ~) D1 U: Q% t& I
- Err = getErr();
" l( [6 u6 X: x. A. q - }while((D1+D6) == 0);) q# H' x8 D" Z. M7 G* j
- }6 E$ s3 Y& {5 Z( ?# Z2 y
- }
! C0 u1 I V5 D' q - else
+ |, Y8 U; j+ |; F8 ~ - {& @; p* e: r* P9 j- v' R+ W+ ]$ h
- if((Speed < 160) & (Err < 2)) Speed+=1;
" ?3 H' A3 S; N8 C - if((Speed > 100) & (Err > 2)) Speed-=2;
4 I5 N7 j, k' {: ?0 B/ r3 F - integral = integral + Err;
a8 d! T( U. O% l; Z; F+ Q) g - derivative = Err - previous_error;' @; P" L/ ]) m; ?
- output = Kp*Err + Ki*integral + Kd*derivative;
& E6 ^2 ^) ]7 Y1 k. f7 O - moto(int(Speed-output),int(Speed+output));
' U o& y5 h+ v- w, M* D: B - previous_error = Err;: b- k: c4 z, G$ q
- }8 L+ u6 O+ A7 a! c2 p- G( l
- }while(!((lightsensor_2.read()) < (10)));# o8 ~ m4 ]/ ^$ d' C- _, h
- moto(0,0);. \( r0 }6 a) q$ ]! U( B2 l, ]
- delay(500);3 w7 O. @! }+ n0 m
- buzzer.tone(262, 500);0 F. m- S; r/ P; Z* p+ y5 u, T
- }+ Y9 k' b, W8 E) j( e
6 s/ j# m$ z6 S; G4 D3 o- int getErr()! y# ^7 M# l) i) _
- {
* S$ U |' ^+ H2 E' ~0 z9 F* L4 O - sData = linefollower.getValue();" f: `) X+ o' {) w+ C d0 T
- D1 = ~(sData>>0)&1;' H- t: d/ S# ?$ [4 B4 s# B! C1 P
- D2 = ~(sData>>1)&1;
* S1 Q& R- b7 E2 V. n - D3 = ~(sData>>2)&1;) g+ D3 k! f' Q" a7 u6 C
- D4 = ~(sData>>3)&1;
' s6 ]: V0 u+ h4 h; k - D5 = ~(sData>>4)&1;! W) T8 X- J0 o3 _
- D6 = ~(sData>>5)&1;
~) C! R( N8 B* E - int downD = D1+D2+D3+D4+D5+D6;( J6 f2 K! M$ T2 z
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
. l" N9 P9 C: ]: t7 I3 s6 d' Z5 w( W - if(downD == 0)
2 k9 e- f3 q0 u+ S- B8 ~$ L# y - {
+ j+ h. o& v( N% \, d - return 99;- a' j; L$ D- J# {% V6 |' _& V
- }2 P4 o4 \! u2 z X/ b
- else, c0 u0 Z2 m4 P) P5 `2 f+ {
- {
9 X7 W* K! q. e - return upD/downD;
" B& R I# G- u. t. j% M& ^ J - }, r* R. f7 J9 x' _
- }: B7 U% Z' J; f/ ?+ l v
- + l* @( @( m1 F6 Z- x
- void moto(int Speed_L,int Speed_R)
" {' x8 m' b y! I ~' G$ H - {/ m) |4 W8 V5 T" \' \+ L
- Encoder_2.setMotorPwm(Speed_L);% R0 W! ~* [& Y0 o* f8 u2 T# y
- Encoder_1.setMotorPwm(-Speed_R);5 [$ h% ?) @7 U1 m% P8 M4 m! D4 }
- }
複製代碼
' \4 t( g% t: K7 |) o" v V$ |/ \MeLineFollowerArray.cpp) C' t+ x" |( K0 v% |
- #include "MeLineFollowerArray.h"2 a$ f* }5 G( p- }7 K
1 z: Z I5 p0 k7 Q' I$ ~: A- #ifdef ME_PORT_DEFINED
; _$ @5 k4 n/ x3 c- m$ W - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)" H3 i% W( s) x _' J- r" E# v
- {
2 E% Z! ~6 O+ a4 c: J0 w5 P6 W. r; ^9 e - + p& e8 c# _( |9 c8 n U
- }: V) l" ]* H; Z @/ b
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
$ h" b0 {9 D3 Y2 O/ w, f) V" A7 d - {
$ @3 c6 |) s7 p5 u1 s - _DataPin = mePort[port].s2;. a/ ]; [/ _; }0 n: D+ F" n
- pinMode(_DataPin, OUTPUT);
9 U c! H' i& ]9 b* B, c8 @2 B - digitalWrite(_DataPin, HIGH);0 Z' P* B5 S" Y+ Y- @3 A. k
- }
7 d) G/ o/ @5 s! K: V$ K- f2 I - #else // ME_PORT_DEFINED
6 A$ p3 d( W; n2 L3 w3 V! [ - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
0 H+ v6 a$ D! l/ o: y' v - {
' B6 y% k" |+ g7 W' ]! C# ` - _DataPin = pin;
& t) ? E: n+ B - pinMode(_DataPin, OUTPUT); 6 L6 Y+ z6 B# G: s( M0 \9 d2 B1 }& J
- digitalWrite(_DataPin, HIGH);
4 P* @' a; Q% x6 w* T - }
3 H& ]3 N- v8 {2 r3 c - #endif // ME_PORT_DEFINED
6 F2 f5 ?# F" r
" x f8 x3 H4 r) Q7 a. a. ]
- [# |0 u2 b! {+ O4 f8 Y- void MeLineFollowerArray::setpin(uint8_t pin)
- g' f; u0 G" c; ]7 V - {
9 i8 _3 Z" }% V# F - _DataPin = pin;
; L" M- h5 s. U6 f3 @8 D$ p - pinMode(_DataPin, OUTPUT); o6 {1 ], t4 ?$ ?
- digitalWrite(_DataPin, HIGH);
5 z& ?$ d; I9 Z# H$ A. Y
) z- ^. X( v8 g6 T- #ifdef ME_PORT_DEFINED7 o6 f- J. [( {0 K! ]( |
- s2 = pin;
7 S9 M2 ?' f( R: J( Y) G1 S5 g - #endif' r# o! Y; |1 E7 o7 H
- }7 @! n$ E: b3 z4 F$ d+ G h
3 E% c6 M! Y$ z% [- uint8_t MeLineFollowerArray::getValue()) J) X4 c8 Z) _& |6 s
- {
0 K" J( p, ]# p+ X2 @; w - uint32_t LOW_level_read_time;
8 P* a# W& K( u7 T - uint32_t HIGH_level_read_time;
7 b" r3 T1 W# `) M - uint32_t time_out_flag;
/ z1 L! h6 D. x$ I1 c, b - uint8_t Sensor_Data[3];! x/ u% V3 Y A% k$ `6 ^( h
- static uint8_t old_data = 0xff;
. C4 Z: {1 H" i) P4 \
" M" T/ g( M/ Z$ v) j( M5 L4 C- pinMode(_DataPin, OUTPUT); x. \: U' x4 `: b) s- ]: g8 i
- digitalWrite(_DataPin, LOW);2 b# X4 N+ g& O
- delayMicroseconds(980);
3 ]- \+ [ g$ d: H& g - digitalWrite(_DataPin, HIGH);, n& R9 o7 {. K$ ^
- 3 m' b8 t5 l4 J
- pinMode(_DataPin, INPUT_PULLUP);7 A; T8 j7 _: [' @5 Q
- delayMicroseconds(10);
% [) R0 I% ~( i - 4 V4 _' f; N" {; F
- time_out_flag = millis();! i# ?) S/ f2 ~# T& ]3 _/ V
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 ~7 b3 _6 f1 R2 o! c7 H2 P
- . P- J$ n: K+ F" F) N& X+ `1 |
- LOW_level_read_time = micros();& v: N5 F2 `6 e8 w/ Z; a. l
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
; z s. K9 X* L2 p" | - {9 n4 J N: [& ?- {( h% C) u
- return 0xff;5 t: a. G: j, R
- }
S" D0 w. S- r4 ?3 y
# s1 x/ ` t9 C8 Q2 M. L4 F- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
* @) L5 \, [4 C. s* O - ' v& e( Z+ j, _
- HIGH_level_read_time = micros();! F6 A3 Y1 x3 o4 C# ` v8 J
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
, A& Z8 s, H3 e1 ?* n9 f! n - - [% N2 N% w2 F4 L& o, D, e6 F
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
8 |; h. u( Q$ W* S t9 w; J - {
, Z0 U+ H6 H4 @ - return 0xff;9 u9 M5 J; K9 H3 O
- }
2 @9 }& l9 V' N' P$ q5 G - & N: a6 L2 a f& ~; T, B: f6 y
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))- ^* Z6 }: x% C2 A
- { h& a' ?' Z) J+ n
- return 0xff;$ ^$ E9 L8 X$ _; L" G/ h5 j
- }/ o5 p3 Y6 [7 V) {+ q( ~
- " p# A1 G- B" i/ g( X" Z+ v
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
' s4 u+ |- R: k5 U( E, y - LOW_level_read_time = micros(); D& ?* t5 n' O( h1 L O
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
% f' I' Z/ ^) p4 F' u
, g2 u) p" K3 B+ e. t- y( d- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out( u# m5 Y8 Z2 k2 P7 P5 D7 K
- {
3 W( |1 `/ q' A) ~ - return 0xff;
0 K) [- X" j9 X - }
, [9 u/ z% j! M1 q/ Q. r/ Y+ ^
2 v7 F$ K: O( K- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))6 D! A! ^8 I/ g3 y2 o" E: X) Q
- {4 k5 a1 _! D/ _8 G6 y W
- return 0xff;+ o- I t" I% b f5 C% {0 U
- }
) R7 z$ ^, u" H; C
; [5 N2 b: o9 Y& {4 d+ n7 t1 E. g- for(uint8_t k=0; k<3; k++)( h/ ^0 p: N C& T
- {" |2 [& B" x5 {- }" p4 v
- Sensor_Data[k] = 0x00;
0 ^1 {9 H' z) p" k$ L. E/ Q# F - & W5 d1 u( t9 _3 ~2 J; U4 C# A
- for(uint8_t i=0;i<8;i++)8 n) V* N7 ^$ a
- {8 l$ Z& }+ @5 x5 d9 `6 Y
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
0 Z8 y3 h6 x# b8 S3 X - HIGH_level_read_time = micros();
! o0 u9 X, ^ k$ D0 f5 R6 w - LOW_level_read_time = micros() - LOW_level_read_time;
3 [) h: {* w% n* Z5 S; k, c" B
$ j. x2 J8 S5 {( }! Y- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
) }( p" |0 K0 ?& d* f7 N1 x" R - {0 f: o2 e% V$ |$ ]+ r
- return 0xff;, K9 \3 b/ i, q0 ]
- }9 o4 y& o) E8 @% Z+ N/ d
4 G4 q; O2 w& a) u7 c/ G' j0 k- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );6 o6 j' N' Z: T" e3 S7 h
- LOW_level_read_time = micros();
$ S0 j0 H$ q% }( c+ N; Q, a3 o - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level z, Z- v6 U0 n [- n: u( _
6 x2 p2 ^5 O8 X$ l. w( a- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1( W$ @' b1 g6 T+ j4 `+ D" W
- {% @( E7 ?" x: [" d
- Sensor_Data[k] |= (0x80 >> i);
! I9 @: |' V* G, p+ }( ~5 c - }8 S$ o& _% d7 k7 ^5 ]; ^
- else if(HIGH_level_read_time >= 100)
, f* M$ t( q2 i: u - {. D, r' Y& v) ]
- return 0xff;
2 P# G0 L* b. i7 D - }" L9 }# q8 D: ` ?
! p+ P- p& C$ A) b R- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)9 b; ~/ F! X5 \1 p
- {
7 y7 a- D# Z4 q( e5 ^- g - return 0xff;
! S- F6 f& E( m" d! @ - }
5 |* h% B9 V; q! U; {( l8 }( n - }8 H. F. d0 t+ G; Q0 E5 q# `! |2 T. C
- }; e8 R$ v" k) G! `( V
- / L& t- o2 T I
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level4 E! S0 {" F5 M7 X! a& s/ D2 L5 E, E5 M
- HIGH_level_read_time = micros();- i* h6 y! @; u
- LOW_level_read_time = micros() - LOW_level_read_time;
+ v$ [7 C1 A1 ^0 d/ f; M/ _& P - ) s; U3 ?- x+ {1 e/ \5 O% S
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )7 y% S. r. U2 |1 U% b+ ?
- {
& A$ P# d( [" E L+ ` - return 0xff;6 F" q9 W+ U5 X! t( P
- }2 g5 D0 t5 ` z+ h- L
- 8 ?; v: y+ G7 n$ B9 L7 d5 r# x: j
- pinMode(_DataPin, OUTPUT);
4 S7 D$ g; ]3 E - digitalWrite(_DataPin, HIGH);! ] x! ]8 o( K. y: l
. i d4 m+ B; g0 f O- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))2 b0 x- C4 a6 d+ }$ c
- {
7 r- p" X5 O, J1 m1 s - old_data = Sensor_Data[0];, o; _) M$ B% ]) c* F( K$ E
- return Sensor_Data[0];
6 ?3 @- |) f, W5 b: n& v G% v - }
- X. j! G. V5 Y/ e- O5 X - else+ h8 _: _. S- _7 p
- {
4 X {( U1 [( i - return old_data;5 V# U4 C9 V& `0 W* _! B
- }
) c. x2 p0 S; |: Z - }: s9 x% D5 Q& W* Q4 l/ M) }8 K
複製代碼 ; R1 ~/ K4 f* | O
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
( h6 }1 `! G8 K8 L8 f
8 Y Z, t/ k3 L" h# T* ]3 u9 \
|
|