|
|
1 A; H' W9 s: h8 f {% }4 l- G. X- #include <Arduino.h>1 s2 x" d4 V+ H* ~+ K Q
- #include <MeAuriga.h>
/ C% ?& |% @* u( Y5 t s - #include "MeLineFollowerArray.h"
$ p {- k, |! `" h3 h1 v2 d7 E - * m/ _7 T1 E: x; J$ e
- MeEncoderOnBoard Encoder_1(SLOT1);
& R7 W" C7 }: R0 G - MeEncoderOnBoard Encoder_2(SLOT2);
5 {& @! _ }$ h) e6 |- p - MeLightSensor lightsensor_1(12); ]6 ~- L# e; c, G: ?, G
- MeLightSensor lightsensor_2(11);
4 I0 S( H# Y9 {4 j - MeBuzzer buzzer;
; b2 }- T# [1 Y8 y - MeLineFollowerArray linefollower(PORT_6);
* N G$ T: A/ z( Q' S1 L
O9 f7 M: Y- _/ |8 L( z- #define Error1 1
2 u) n; N7 a& v8 L. [2 U: R' }$ b - #define Error2 24 u8 F) |7 A: ^
- #define Error3 3
V# o- c) B: ^! I) P: B/ V
- f# Z6 z4 P1 a0 n- #define Kp 15
+ Y. O# H. w7 a& u - #define Ki 0.15, K" M& T$ _) G2 P
- #define Kd 0.03 F: c7 T" w N* [
% O( z# u9 {: g: q- uint8_t sData;
9 I$ S+ m0 A! n7 ]* ?8 M - uint8_t D1;6 J, m6 ^! f9 ~% C1 w* p& h8 r! k
- uint8_t D2;& d$ U& \& i: [7 F5 g; u9 N
- uint8_t D3;
) y# o) G5 }+ J - uint8_t D4;
+ N7 R3 l6 K" B0 V. I4 x! M - uint8_t D5; D0 b6 g1 q! ]0 e# h0 [
- uint8_t D6;
R& S: V* X8 Y& ~* L
) h+ S. j0 E* e2 q0 ~7 D2 \6 N; b- float previous_error = 0;
% ]( m$ I" Q( P d - float integral = 0;
0 E7 O5 I. B$ n' x% g; z - float derivative = 0;+ y2 ]! T( a! X
- int Speed = 160;
/ l" a3 A' H9 j" U - float output;0 b d6 A5 D: w) f; D4 m
- 5 I7 d5 f) d: `. W) f7 f% t' \2 v
- byte Left;5 V( w# S1 G/ x# _8 T& Y' }3 K
- $ O% O) W3 Q: U3 Z6 a
- void setup() 8 n5 T/ R: Z( [: ~. z
- {
1 f- |% V$ u' T! V+ O( v) y - //Set PWM 8KHz2 @) w% M8 D. c+ {& ^
- TCCR1A = _BV(WGM10);( P9 q6 L9 Y$ A* q/ s: i2 _; C4 G4 u
- TCCR1B = _BV(CS11) | _BV(WGM12);
0 v; B& a& {( e' F* P - TCCR2A = _BV(WGM21) | _BV(WGM20);: J0 p+ w# i. W8 @' M+ R
- TCCR2B = _BV(CS21);
( F7 }6 P( T. k5 @- e - Serial.begin(9600); E( a2 S4 G3 C1 T1 i& n" ? a
- buzzer.setpin(45);
" L; J8 i3 o( K" Y4 @; v: a2 g4 l1 s - }/ B% R) V1 z( \& J9 j3 p
6 D% h a* B2 \0 C- void loop()& t+ U( j6 a8 l" x( R
- {6 H: e' f) P A
- while(!((lightsensor_1.read()) < (10)));* R ~; F7 z' N7 {1 v
- buzzer.tone(1047, 500);
" e- O I' b& T$ H3 a+ j% q0 u) l - delay(500);& D3 @. n2 Y" A% |9 E/ E
- do
0 L7 D! U3 r V3 M! ?" l' a W6 B$ S - {; ]8 p0 }1 G/ V! Y* o* g
- int Err = getErr();
/ s$ G" P+ J" ?6 [: x - if(D1 == 1)
2 s3 g m7 t) G7 b - {- c# g! k, f9 u- R3 P9 D! \+ G
- Left = 1;' ?, m F/ h: l) @; g t7 J2 |
- }, v& s' m" D$ j1 ]
- if(D6 == 1)
% L- g7 k- N3 B0 z$ l \) o/ B - {+ ]2 @8 H6 \9 R8 X1 ?" Q
- Left = 0;
( |" I% Z+ S! w3 X - }
& c0 x2 j4 a" t7 u' S, R1 ` - if(Err == 99)
2 r3 U1 c3 |' W& C% Z8 v - {# }# d: O- v. \" l) b* X+ j
- if(Left == 1)
! w5 Y& M5 L9 C- z% Q) s - {
# y, M& Z" j- P - Speed -= 5;# S) B8 Y5 J q% F0 `1 s
- moto(0,Speed);, L! d6 V2 e# o1 E0 k; q
- do
( U* \# f* ?8 |, b; _ - {! P2 u8 n& Y7 P; ^! \- h
- Err = getErr();: H* M. {- R @2 o/ W
- }while((D1+D6) == 0);0 \5 f5 B# N0 J% u
- }
3 ^ y1 w7 F4 H, f2 L9 n7 D - else5 D5 {: s9 \3 B: B* ]
- {
3 M- X8 b( q+ w7 U7 H/ X - Speed -= 5;
/ p" q8 T& W: n3 } - moto(Speed,0); - K% O0 |* d8 Y) p, E8 K# N7 s
- do; B5 O* k& J. X0 f( H
- {, r+ H& A8 R! F6 T" `2 S) C$ k
- Err = getErr();4 `- D) [! M0 G: u0 Q6 H2 d# N$ |, @
- }while((D1+D6) == 0); a) m9 ]( m" f2 r( k7 X% |$ k
- }
% x, W& z- Q2 q3 a# t- j4 @ - }/ i% J) w, T @( }
- else0 A: _5 n% b. [. A- M" m' W) e
- {: u1 g5 ~6 c, W5 J6 G
- if((Speed < 160) & (Err < 2)) Speed+=1;
% P, [2 ]3 D5 W* }3 S- L - if((Speed > 100) & (Err > 2)) Speed-=2; 6 L' T6 f% }& H9 O) v7 Z* V
- integral = integral + Err;9 h- o! ?; |% r( A, u3 K2 E
- derivative = Err - previous_error;
6 t, ?/ V! i% n8 ]2 C3 Q - output = Kp*Err + Ki*integral + Kd*derivative;' c$ ^2 @& F5 e' y& F# f; G
- moto(int(Speed-output),int(Speed+output));
( W( h3 _ I6 g0 W - previous_error = Err;1 E# \. ~( b9 i* {, J- _5 Z
- }
; P9 V- c. M# N4 X - }while(!((lightsensor_2.read()) < (10)));% x2 Z4 L7 D! e/ R% p
- moto(0,0);
6 k0 t, H- V# U# m" _; p - delay(500);0 b/ h+ C0 ]! U( v/ w* J& O0 E
- buzzer.tone(262, 500);
2 B# B+ ?* P: j$ ~6 h - }* F- \0 { f" R4 v, p; J6 E
- ( Q% @" J! R, M) w
- int getErr()
$ H* C* [8 C. u+ Q8 f$ R- B z - { # m& W+ j2 |; N1 D
- sData = linefollower.getValue();
q" H. q6 d u2 I2 S- h3 z: { - D1 = ~(sData>>0)&1;
6 l( d/ N( l7 N8 x# s - D2 = ~(sData>>1)&1;& c4 u! U6 z' t( W2 Y3 ~
- D3 = ~(sData>>2)&1;
# E/ {1 S# l0 s. a - D4 = ~(sData>>3)&1;
# W0 t6 ]" Q7 z- F: d - D5 = ~(sData>>4)&1;
! U& j" ^3 L/ ?: i* ^2 s$ ^ - D6 = ~(sData>>5)&1;
L+ X# `( P8 L6 Q" c1 t - int downD = D1+D2+D3+D4+D5+D6;
% Q; u( t$ g+ o/ L0 Q4 J6 W; ? - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);2 Q$ C' T* |. J! E
- if(downD == 0)# d% ^0 A8 ^ K5 ]0 z/ x# h; l
- {4 o/ K" o2 V/ O% |/ b: X9 i6 y! r! d
- return 99;1 D- N2 F' B+ r0 B
- }% H3 ~) `" A d9 w+ j j" m9 Y2 u3 s# t
- else" O5 j" O1 |2 `( `
- {7 s( b [! w! U: }+ k
- return upD/downD;
. C% H3 f$ A/ o' ^" t - }! q& z9 F1 }' ~# E* D& Y
- }
. ~" O4 Q. Z0 K: s+ Y: D
% ~5 u$ @8 X1 H6 J" ~/ a( m- void moto(int Speed_L,int Speed_R)
$ O) {+ J& Q% B% O. E - {" [6 I, Y3 v& d7 V% M; x
- Encoder_2.setMotorPwm(Speed_L);
$ G0 z/ H# I' p3 ]9 } - Encoder_1.setMotorPwm(-Speed_R);
' j# R6 ~ u) N( s( U7 d+ R& | - }
複製代碼 4 e6 J0 b. N+ t! f) [6 M+ }
MeLineFollowerArray.cpp
! h i! P# Q: z L1 Z' i1 {7 X- #include "MeLineFollowerArray.h"
2 s0 |. r' P0 l
1 W1 ?# b+ }: E! z- #ifdef ME_PORT_DEFINED
4 Z/ e$ v) t! ~; N- F7 w; J% j - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
* i; M% G5 G/ v6 Z8 s7 o2 s6 m: D3 | - {
; {3 T- ^* J9 ]3 `; h9 U - : }, [' _# j9 j5 m9 c
- }
+ c, R) e5 ^$ \: ] - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)! L/ b& I8 C% Q% r( Z0 E
- {
) q1 d# [0 h6 w! J1 m7 O+ t! s - _DataPin = mePort[port].s2;: h7 n! }& A6 x0 o" _
- pinMode(_DataPin, OUTPUT); 3 n) ?3 u( Z) ^! [$ r' w
- digitalWrite(_DataPin, HIGH);
9 M8 P( G* F, h% _ - }
6 ^( y7 \; O% L - #else // ME_PORT_DEFINED( N( `9 n R! J( }
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
- w% P; n: X7 w% C$ J! t0 ? - {
, R$ L+ [+ X, Z2 v- O8 F - _DataPin = pin;& r2 X- Q* |2 C6 U
- pinMode(_DataPin, OUTPUT);
Q- U& T# M" r4 z& N0 R1 P - digitalWrite(_DataPin, HIGH);1 ~; ?7 g0 Q# h6 G8 B
- }4 s4 \0 o2 E' L
- #endif // ME_PORT_DEFINED( ~' k, J) t5 U. |& D$ H0 k
/ K* t z! t- z
# \5 h, p$ i+ {' J: H- void MeLineFollowerArray::setpin(uint8_t pin)1 I2 J2 |, T. _& \- S# u4 n
- {# m$ q+ ]# t* K0 V
- _DataPin = pin;
! c/ `# }+ ?8 L2 R0 T/ K) O5 X- ~+ I - pinMode(_DataPin, OUTPUT);
+ w4 ~7 d! d! a - digitalWrite(_DataPin, HIGH);1 ] e% h X! i: G+ p# `! d4 h
, ~. q( Q* j v% E2 x- #ifdef ME_PORT_DEFINED" J" q8 h+ K! C
- s2 = pin;
) h1 i1 _' O, d+ x! s - #endif6 W5 _- u6 p$ c9 P7 a! ]
- }) F! P3 F3 L5 ^1 ?; F
& L6 `5 Y- ]( k, |9 Z. i- uint8_t MeLineFollowerArray::getValue()& m5 x3 D- K% I/ r9 N8 H" N( ]) `8 }
- {$ Z: D8 ]2 Y" h- a& ]
- uint32_t LOW_level_read_time;. [7 q8 y2 g# k
- uint32_t HIGH_level_read_time;
# r/ u' ~6 o D$ a7 w. A* d - uint32_t time_out_flag;
' I# A+ u/ r8 O+ j! @ - uint8_t Sensor_Data[3];
' k4 G" @7 |, K1 D - static uint8_t old_data = 0xff;6 e" {$ J4 t, Z/ d+ h$ O3 F
v$ ? B/ y( E* b8 c. e$ Z; `- pinMode(_DataPin, OUTPUT);
% n4 q! |4 v U9 O - digitalWrite(_DataPin, LOW);
6 H' j/ o$ E" Q* x1 ^+ u - delayMicroseconds(980);
- {& ^- [1 Y2 [4 K" z - digitalWrite(_DataPin, HIGH);
9 M9 Q3 J# L! H5 z - 3 [5 a. u# r+ y2 q( L, _8 `$ j* E
- pinMode(_DataPin, INPUT_PULLUP);. h# g- U/ y. J
- delayMicroseconds(10);: U6 b2 ` Y2 g- {: F
$ q; ?' v- }& B- time_out_flag = millis();
. I% b0 }6 n0 @! j7 j' K, S( m - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* S1 ~6 \! X6 w2 w5 H
2 ?- S6 D$ s5 g7 I5 F- LOW_level_read_time = micros();
4 r2 j$ _! O/ @. e: Q% ? - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out. ?/ z- z5 s1 v3 t/ W# w
- {
8 P1 a0 [* o. Q- ]" E2 d& k - return 0xff;
9 b5 K# r9 b0 K, } - }5 D j* n1 M* O, r5 B3 I8 W
$ p7 e" N0 {, ~1 h$ P- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( M) c$ p* `, l8 L- [* @+ F
9 q) D( h9 B" U$ ~' X( ^- HIGH_level_read_time = micros();8 s6 t# p$ w) J7 o, m) }
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
1 a+ h) h1 V9 W5 W- C% t - ; b/ o- c9 {7 M' l- Y" L5 {
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 Y1 T6 }$ Q9 V) r8 i# b% f - {
5 o7 f# \% y N+ m* p5 s& ^2 k - return 0xff;2 s# q' x( k ]$ L) c, y
- }
# [" c+ G; }. [ Y8 Z - . J$ i" S7 W1 k; f5 r
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
; s# L/ n$ l; K - {
" L \3 R5 N5 V. v& M% [ - return 0xff;
2 k$ m2 z M3 |4 u3 R7 j8 P - }& V# y- M" e. d+ @* N2 v0 F
# ?. e7 g$ o. s/ G- Y! k s- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: O2 V& E6 ^4 T1 ? - LOW_level_read_time = micros();$ S3 z: Q8 [ H
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level: L/ c8 w; r: [! l
- 0 V8 Y2 v9 t) Y! H8 c, n) p9 K( S
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
A: J- \* k: |+ o& g - {
# [7 h, ^' V" K7 ] - return 0xff;& P6 v. K7 |( c
- }4 k1 O8 y7 C) E" U- {; I
5 g5 N& H6 d4 m, i( {! k- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
; z) O, D4 ~1 l% H$ O; X- e& S - {2 X4 V/ f2 v# e1 B% U6 o
- return 0xff;1 p& M$ C; N4 X! q, u6 s
- }7 h9 s$ O8 i0 @4 a# Q$ H; h7 G
( R& d2 `* C/ `- for(uint8_t k=0; k<3; k++)* G7 R4 d" r( H
- {+ g# D9 t5 ?+ f7 @( E& g
- Sensor_Data[k] = 0x00;" S! R; @" [5 k
- `" E; ^9 g# x; @& P% k3 }, T9 R' S3 }
- for(uint8_t i=0;i<8;i++)/ c* c2 W7 p# ?( E
- {; u( H9 r& f' t9 Q9 R, Z( v
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level6 Z1 D$ Y5 R _ ~6 \2 B
- HIGH_level_read_time = micros();
/ ? F7 i; R, t8 T, N# W - LOW_level_read_time = micros() - LOW_level_read_time;# g7 [, t* T, v/ O, `* Z/ ^& E
- 4 [; z1 j, B6 |) u" c- |$ ~# P
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
% S5 Z: g+ `! \5 v& x - {
# O( v4 D& d3 c$ Z* j' w - return 0xff;
# e3 w* F3 B6 @7 F - }+ B2 u( H, t. q! k; r6 {1 C1 v
- 4 V, ~6 Z; {) G9 S
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; N* c2 |' m0 B" f% [ - LOW_level_read_time = micros(); ]* \9 F2 l# D' d# w
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level8 `4 ?3 g( N" u9 A, Y
- # `/ C4 |5 t% x3 \! H) d
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1, @, H! D3 v! G4 @/ a
- {- y" e4 t0 Q- O3 s) w4 q0 _
- Sensor_Data[k] |= (0x80 >> i);0 z1 a* B* U f0 _5 W
- }( h7 v6 [2 Y: A& p- o) S
- else if(HIGH_level_read_time >= 100)" F% X+ a+ ], s1 I: V. |! [5 [% P4 l
- {
. Y7 N$ K0 o7 o# G7 F% U - return 0xff;( Z6 H2 |4 L( f+ \" y1 i2 v9 Y# R V" L
- }
B* L7 s. h: N
8 i( W" N4 d: ~ x- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)$ B+ ?1 j+ G7 r$ c# G$ ?. u; o
- {$ u! _1 a/ p) O: t" o# G
- return 0xff;
; W) |# Q. V! E1 ` - }
0 W: }; K* F2 j0 ~" r( o: c# x; S( P - }
) k F% q2 S p2 D5 q" |2 ~1 R- Q - }
# |7 I4 `2 s6 [% |( c- v0 Y9 E2 v
# ~1 Y/ q) B( Y5 O- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
5 r4 U5 R! f% t; Q2 Q" s - HIGH_level_read_time = micros();
! j- F4 |: Y) q' b* V! t- x - LOW_level_read_time = micros() - LOW_level_read_time;/ K! Y6 T8 W9 d5 B' Z5 ^5 }
5 S$ |8 v6 X9 [( @" |" p- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
- r. D d$ z' |2 m. \ - {
) X$ m- r/ |" M$ B. U4 ? - return 0xff;" b1 h7 K8 ]; |7 C& M5 e) f
- }, R" c' F- _ L, V4 m" D& y, i
- ' J: r& Q# _. n! w7 m
- pinMode(_DataPin, OUTPUT); P% `3 o$ G+ A- n* k5 a2 x1 M+ j
- digitalWrite(_DataPin, HIGH);; g$ B6 @2 W n3 T7 c; x0 ?
3 {1 C! c1 F6 H: T$ g4 m! b; z2 `* ]' T- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
3 _7 e) H' f [% V0 x - {" S! x; I& G- W/ Y" f% d
- old_data = Sensor_Data[0];
% M7 ?: }: n0 E* Z9 a2 K - return Sensor_Data[0];4 U9 H# h1 n" t Q1 A b
- }0 i# U7 @' K% H' H, Q! Y
- else
N) p- t3 x; X8 Q: Q$ R - {% T5 K" Q9 ~% o1 i9 }: X7 A
- return old_data;% @6 v7 {9 y* o( X! _) F. K
- }" x3 k$ n+ l6 a9 d
- }) P7 }1 u3 f0 _" S
複製代碼 0 V- W1 U1 ]4 V, z% ?
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
8 j/ d" z# h6 K3 {7 q O) P. |. l8 x) Z2 r
|
|