|
|
6 L( o; Z- h% w) ^
- #include <Arduino.h>8 Q* H Q4 m3 @7 R: p& a0 O1 {
- #include <MeAuriga.h>4 [; x4 K& {5 f9 x' f3 s
- #include "MeLineFollowerArray.h"
, J, m' o \) Y* Y2 o q
) v V" w# }; i3 _, Z, g O- MeEncoderOnBoard Encoder_1(SLOT1);" ~0 E9 H: {8 T. d, z, ^
- MeEncoderOnBoard Encoder_2(SLOT2);
6 S4 k/ a1 o2 @! A3 V - MeLightSensor lightsensor_1(12);2 H. E: V. \6 p0 l" v
- MeLightSensor lightsensor_2(11);) A1 }) m4 R+ y/ \; F
- MeBuzzer buzzer;$ u/ A N+ n7 f6 _ K, r
- MeLineFollowerArray linefollower(PORT_6);" w4 `5 R/ M; {- f# f
- ( _8 o1 C) a0 Z. ^5 ^
- #define Error1 1
+ r: }" u" _' ~. n+ l - #define Error2 2# K& t' e' A* L# y: B9 I! m
- #define Error3 3
, u! `; L W' m! _& y# S6 O
& ^( v: Y9 J) i6 C( G4 s- #define Kp 15
. } s7 O" w; \4 X# ?2 j2 N - #define Ki 0.15
3 ~2 v0 G$ E& I+ |6 q4 r - #define Kd 0.03. L4 @! `9 p8 d% M. F
0 H& x- `* I9 @4 j% `! k1 D+ u- uint8_t sData;
7 Q. O9 h* g: B! o+ o1 ^: H% L - uint8_t D1;) D( H/ i0 Y4 U3 o( z6 ^# b9 [- }
- uint8_t D2;9 T5 m) f# T0 R: T. @
- uint8_t D3;
8 h+ w9 v. [8 ]6 C: f9 I - uint8_t D4;
) {7 N8 Q5 [! j# t) {( P - uint8_t D5;$ u2 k3 p! w0 V7 [/ p" J
- uint8_t D6;, R* g$ G& O9 ?5 M6 B
- 8 p6 q: y9 n: r. y$ Q* R# {3 d& y
- float previous_error = 0;" @7 V% \ l1 O, _9 ?
- float integral = 0;' O4 a2 w: _* k, p/ i8 o7 `1 X) f
- float derivative = 0;
8 q! v0 L" `$ y: Y4 J6 S& t - int Speed = 160;/ O- s( A9 `& F* q& t& |+ A
- float output;
. W `( x G! v9 F+ m' q- m - # G; K5 w4 O; B# d" d
- byte Left;
Y& Z! X. E* y) c
" O( U7 u% ?# r" S1 n0 Q$ W, A. v1 E) h: ?- void setup()
3 [% j/ g, H9 A1 b - {( F8 Y7 V1 r$ B$ B+ R; ^/ B0 S6 M
- //Set PWM 8KHz* c3 J w2 J6 H
- TCCR1A = _BV(WGM10);
% x* ]8 a: e: D! q# @, U# Y - TCCR1B = _BV(CS11) | _BV(WGM12);7 E& x4 k/ v3 G
- TCCR2A = _BV(WGM21) | _BV(WGM20);
6 t0 Y) Y# c# q4 x - TCCR2B = _BV(CS21); U/ u8 t% z( |1 Z# ?2 H& u5 U7 J! b
- Serial.begin(9600);
% f) o: D, j6 r, x9 E. Q* z - buzzer.setpin(45);- L9 a' e/ \5 D6 e
- }
" V* d6 i" n& |6 S* `0 m6 J; s# f
8 p3 P1 c. U9 N4 k- void loop()
3 @. E3 Y5 C1 C0 t - {
: Q4 F. \- ?& Q5 ^+ o3 \ - while(!((lightsensor_1.read()) < (10)));
! z( p* z* S9 E, B* O* C% u% Z - buzzer.tone(1047, 500);( N) G5 U+ p& |0 |, g: w
- delay(500);
l8 j/ \- B8 o5 j' W - do* u: `6 o: R; N. F4 \+ _, \# f
- {7 b4 T, @; N& M& \) n& c
- int Err = getErr();
( U# V4 C! W: M* ]* ~ - if(D1 == 1)
" |( P" ^* V( z: ` - {
" F# u" D8 P0 C' A \. ^ - Left = 1;
3 T# }/ m& ` e8 @$ } - }4 {8 I1 L$ W3 F% t
- if(D6 == 1)5 Z1 U# ~+ G4 `4 p- @8 F
- {
3 v1 p. ~& F8 I! s( U) x) F6 t2 s - Left = 0;
# f+ ~3 v9 L- _ - }
- ~, M# @& \! ~9 Y4 O2 Y3 P - if(Err == 99)
7 n* ^. g7 J! j/ j% q+ |! U - {2 }6 Z+ a. J% N$ I
- if(Left == 1)' R) I9 o* m# B' Z
- {0 Q: J& W O, W: V
- Speed -= 5;
1 h1 [8 T* T& V$ j" E: p# b! v: H - moto(0,Speed);! u& z0 F, h5 n( }2 l$ m) W* j; F/ U
- do
+ |4 ^, O. U, n2 s5 u, a8 H - {0 N* ]2 |( y. n$ E* y2 Y0 A: t- v3 F
- Err = getErr();
, n5 J6 l5 [- `; q5 b - }while((D1+D6) == 0);; @! G2 R4 R* U
- }
9 ^( j% y6 N8 R5 A& s( t7 u4 L - else
" v3 A5 ~. H! f2 i5 @ - {
$ Z- e5 Z* X1 v* K% m$ [) A' o - Speed -= 5;, Q- z! H( \7 N& M' Q
- moto(Speed,0);
* t% H7 R& a8 G* b: C - do
( `- ~) u+ Q1 T0 k" R7 D - {: w3 Q) S7 b; F8 s6 s$ C
- Err = getErr();
. B/ g) ]7 T% p# a( |! p- O - }while((D1+D6) == 0);
9 ?- Q+ c3 Q; g4 G G6 _ - }
. [7 X" ?* ?* p - }- q I& U3 N/ l" \( G, j7 U
- else( s0 W: {" [" e$ \8 h$ H7 y, h$ q
- {
% K6 w0 U! _0 w - if((Speed < 160) & (Err < 2)) Speed+=1;
: ^% Z; m9 m" |( m - if((Speed > 100) & (Err > 2)) Speed-=2;
# O x& K! |# W) P( h5 i/ S - integral = integral + Err;
) ?1 j+ D/ L& o2 Z% n7 R - derivative = Err - previous_error;
, j, v* ^1 d p, [# Q - output = Kp*Err + Ki*integral + Kd*derivative;
; n! A1 K3 k- I' i5 }) C - moto(int(Speed-output),int(Speed+output));& b8 `5 Y$ m; s( Q1 C
- previous_error = Err;
& |2 d# p* B' b: G0 R - }
: M. Q# V O. O# J+ ?# S1 s1 R7 W; g - }while(!((lightsensor_2.read()) < (10)));0 i/ o! G e" i" _
- moto(0,0);& n. e( e' C* y9 x. [3 Q
- delay(500);2 @. N H: S2 U4 X; H5 A
- buzzer.tone(262, 500);" r- X7 R' T/ @, w2 [6 G3 N+ T
- }
; x% g1 T5 G" _9 K6 g% x/ G4 g - ) V3 v3 x( M' h" a0 v% {
- int getErr()6 b. A) z" Q9 q- }/ }+ N# @( O
- { 2 P7 ?2 `7 |$ I1 x/ n! B
- sData = linefollower.getValue();) X& ^* E6 y5 U$ M' c
- D1 = ~(sData>>0)&1;) V3 D+ z' M0 @* f
- D2 = ~(sData>>1)&1;7 q' Q6 D* q! r! t- \/ k6 f* B
- D3 = ~(sData>>2)&1;
9 f. p6 y" ]* V' @ L! X - D4 = ~(sData>>3)&1;
$ F4 Q' H9 q( ] - D5 = ~(sData>>4)&1;' t6 O# I+ e) N! F3 ~# J' z4 y# k, I
- D6 = ~(sData>>5)&1;' v+ e8 f$ x4 D+ {( c6 o
- int downD = D1+D2+D3+D4+D5+D6;
1 [: I9 r2 c& S - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
' n+ Z8 P7 D# J0 ] - if(downD == 0)
1 y) b+ q2 `7 h% m5 Q - {
- a4 ]" c7 M, _5 R$ @! f2 b - return 99;
; G6 \9 v# e4 U. X$ c7 g - }2 h' y2 a. T- L/ @" n: ]6 @; D
- else
/ n* U: u) E! @4 ~- N! g Y" D - {. ]# U$ B+ N, z' ^/ U( G
- return upD/downD;
& D6 v$ \0 F4 }0 s - }( o2 J6 z0 a5 q! h& c Q
- }9 J# j; W7 t; ]+ K5 C9 E1 ?
- , C. ~. A2 ]6 y. U9 U5 K
- void moto(int Speed_L,int Speed_R)) b: n7 a, {( s% ]+ w6 H1 G
- {
& t, }6 g7 D" u. n0 V9 d - Encoder_2.setMotorPwm(Speed_L);4 V, e% i% p. J/ Y+ j
- Encoder_1.setMotorPwm(-Speed_R);
7 f9 A3 Z0 L' V. U# \9 e - }
複製代碼 8 r2 F- ^* ]1 r6 T+ d# M2 A. C. B
MeLineFollowerArray.cpp' x( P: L' c" b3 Q& B8 z
- #include "MeLineFollowerArray.h"5 S5 Y/ Y* A( g' T$ X. l
+ r6 t4 `: ^' _- #ifdef ME_PORT_DEFINED2 O! @. t# J" c8 M
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
U" T, U1 W% x - {
- ?+ j, ?4 G+ }0 L - / H& _( x8 \& }7 z. ^: \! c
- }7 S4 S* r; c6 L% j* r
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
8 x3 M4 L) \. d. S& ?4 H - {
# {2 f+ `1 ?) O; T - _DataPin = mePort[port].s2;
E7 t$ c, J) R1 _5 p - pinMode(_DataPin, OUTPUT); ' ]/ u" c' ~0 h+ \
- digitalWrite(_DataPin, HIGH);
8 @1 r+ d/ v" h7 ?8 N# \ - }
+ t3 I& ?6 A; L$ P- @& H - #else // ME_PORT_DEFINED
: M' v# Y) P$ C3 q/ |8 f - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
1 d) j' e" i2 q# z2 |9 |( S- D - {2 ]8 k- i" |/ ^6 w/ S
- _DataPin = pin;
0 N9 X! x4 ~8 M! t - pinMode(_DataPin, OUTPUT);
9 `9 \4 B" _/ {% O! f) k - digitalWrite(_DataPin, HIGH);# n7 Y' H* C G. |/ Z) t5 V
- }
6 F g2 H/ K( m - #endif // ME_PORT_DEFINED' \! l/ V1 M3 a
- 7 C G- ^' _3 r% Q4 T0 @. a
- 7 }) w; P+ `; B- K$ P; Y+ R7 [
- void MeLineFollowerArray::setpin(uint8_t pin)
- `4 i1 Q' S- E( J2 U' Z- a e - {
1 p; [' H. ]6 _+ [* T9 N - _DataPin = pin;
4 W" s1 ~9 u+ K- A - pinMode(_DataPin, OUTPUT); 6 H/ m4 u3 o, s9 o3 [
- digitalWrite(_DataPin, HIGH);
2 Q( p8 w( \2 ]& O - h& A, v# h: Y+ q
- #ifdef ME_PORT_DEFINED; P( X; d* P' t+ A" l
- s2 = pin;
6 y2 `+ t5 g; u# f. `2 O5 D - #endif7 M& C% ~5 f+ P, o' C# w
- }
6 v) q1 x6 U: f5 {
; @( q4 r7 i# U: L* Q @0 S- uint8_t MeLineFollowerArray::getValue()6 D( {' z/ C0 f/ E4 v
- {
3 z8 I1 [9 p4 n - uint32_t LOW_level_read_time;
, P; X; E$ k7 d+ C4 n: U: C - uint32_t HIGH_level_read_time;
0 i b P6 r0 J' O - uint32_t time_out_flag;3 H& P/ C0 m5 _" Q
- uint8_t Sensor_Data[3];6 u. r. W% z# ~/ U+ a
- static uint8_t old_data = 0xff;
" }6 Y4 N3 ~( v' D. e1 [8 ]/ l% J
2 ~! M! g! G/ ^* _% m6 E- pinMode(_DataPin, OUTPUT);
, b& X3 ?+ W- ^" h - digitalWrite(_DataPin, LOW);
2 T0 R, d2 O4 J/ n5 c - delayMicroseconds(980);1 H) v t5 ^* B. j/ a
- digitalWrite(_DataPin, HIGH);6 @# E$ F0 h$ k; V6 ?7 k
- A1 R+ s* L+ ~/ t; K/ Z/ {- pinMode(_DataPin, INPUT_PULLUP);
. Y4 B' G, W, U! n; y4 p - delayMicroseconds(10);
: D- u" {% [8 v* F+ v! ~
3 F r; T6 X$ q. E- time_out_flag = millis();
# {5 x' ~- w( A - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
/ t5 z+ Y5 O2 o) {0 X7 a2 C
8 Q! d ?# L5 q" X- LOW_level_read_time = micros();
) h) }+ {; I/ _( p - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out- c, n" Q% U$ w$ c1 x, ]
- {
* Z6 h$ U( ~& E1 ~. a' U0 k5 c - return 0xff;
) p8 x* O+ r9 L - }
9 a. a; b. H; B$ W+ g4 U4 R - 3 D- R, ` y* i/ k; Q. d/ q, V) s
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );5 Q8 C3 f8 P5 V0 m/ @
% h; J- J: e" f0 C7 U- HIGH_level_read_time = micros();
" q& Q; x6 N4 d6 y! z# f9 U - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
( K$ J2 Z g6 T5 B - ) X6 R. g7 c8 U9 y; w
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out C# B3 @& `0 s+ r" l; E3 o
- {7 j6 c' W ?. y# ]; T
- return 0xff;
5 o3 O, _- g+ Z7 N/ ~7 _6 E1 g% p - }
- M# L! d' Q' U" X6 }* M - ' x4 g5 l6 Y( D2 o
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
% P( R% t7 e# Y. `/ Q - {# Q" o. T- k9 u, U$ n
- return 0xff;( E! H6 u1 s8 ?. q3 v0 c
- }
! J; p- D2 v8 m/ c2 e0 \9 ?1 g( ]4 d - 9 {+ N8 S9 t8 P9 ]9 h
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );% I, j% {& {( B
- LOW_level_read_time = micros();7 b2 d2 T, R5 I% t* ?
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level5 y+ U5 T" C8 X0 W
5 P0 z1 j) t/ A3 _5 M1 u/ C. C; u- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
: B* Z6 ^- @8 a3 v0 t) q - {* ]( l3 K, v0 k. w2 {
- return 0xff;
4 V0 x, } C# g8 Q+ l8 Y% b - }
, L7 W) Y3 J4 d - ! C% k, ?* t% ~9 ^5 i& W, W6 B
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))4 B: |% Q) ]. F" O& }
- {
Q4 ^- [: l1 K+ v9 G - return 0xff; K- z0 A: z, g& Q9 ` K- S( W
- }
% e& d- G ?5 K7 l8 e: w b - - E& ]" \: I" s; a* S
- for(uint8_t k=0; k<3; k++)
1 }0 O; J5 e" q7 C5 i# \1 I% [ - {
8 F/ o1 M. p Y! D7 z+ n$ V - Sensor_Data[k] = 0x00;. R+ K* }- s( G
1 e, ~: G, ]/ [- for(uint8_t i=0;i<8;i++)2 o. X& a0 \) I- U' j
- {
" V/ P0 p0 c. y" e! @3 m" S3 D - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level& X2 h6 K- [7 k1 b' x9 H/ ]
- HIGH_level_read_time = micros();
6 o' W" s8 v! C( x2 Z - LOW_level_read_time = micros() - LOW_level_read_time;
6 j* U# Z* b( C7 ?$ T7 H
7 d: \2 }3 \1 d8 F$ A- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
- |# C& `; | d3 b4 K& h - {
% X6 z' k. L6 o0 n2 Q2 K* r - return 0xff;
0 |% R& L: J1 p8 a8 f - }$ E7 t* |) P6 f! V6 Z3 D
) p. B$ ?; l, k+ G* d0 w" A3 j2 @- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) U1 v3 i3 ]& R5 V- S, X) j" | ^
- LOW_level_read_time = micros();
4 n9 {4 ~( o! X7 W6 V - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
+ n; X7 u+ w# c - 3 x8 ^ a* [9 S! R9 v3 [
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
+ K/ F3 r* a5 K& n; |% d0 i - {: l8 Y6 i k7 `# A( y. r% j
- Sensor_Data[k] |= (0x80 >> i);
# C% Y% i' P, e' q - }; K" i4 w. i7 X" |
- else if(HIGH_level_read_time >= 100)
1 ?( t$ n( `) v, y& C" I - {
2 L3 a6 U* A i E; W" m7 I - return 0xff;. G5 p; f0 J# c, O$ C3 Q
- }
, U" {" Q! K! v/ V" n s" \ - + I; y5 j# f* Q6 C! ~: Z$ Z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)& K" H6 x* P0 W- e, T3 E, e6 c
- {. M& D$ T& g) q2 Q
- return 0xff;
8 m" T. ?" K. q" k, o - }# p5 B5 v' J( l0 `7 W
- }( R5 q, P) |! A" [( ?3 E5 ?, O
- }
- e7 x0 U+ J2 O9 I! R
! L% j6 f) L( K- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level6 }8 V9 P0 _2 l u4 h8 z- }
- HIGH_level_read_time = micros();
. v9 ]1 K/ Y2 n - LOW_level_read_time = micros() - LOW_level_read_time;
1 d' S$ G0 }; B - . _! J7 ^3 `7 T9 s: E9 r
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
1 I0 u' V& }& e/ O* u6 _: S5 @ - {, |0 i4 g3 f8 q; n! R( V9 A
- return 0xff;3 S3 I& ]9 d0 F K. p8 i
- }
0 N* @5 S8 m* @ ^4 E6 ~0 H5 I
& H/ w$ Z& g9 }/ y: l- pinMode(_DataPin, OUTPUT);' d3 w8 ~$ f3 ]/ ]
- digitalWrite(_DataPin, HIGH);
) {, p5 l6 Z( j* Q C) W - 9 |( V7 Y/ y) w: l( O, ]
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
/ K1 X' f6 }, G1 e - {5 U8 ]- X2 q. U3 H
- old_data = Sensor_Data[0];
) E( {: @0 g+ d - return Sensor_Data[0];
# p4 l+ K0 ]! _2 j; ~ - }
1 y7 v! _& b7 c - else
! M( w" g4 j' D# s g4 P - {
+ ~5 M4 k3 F1 K; | - return old_data;
1 K+ x) O, q6 g$ Z. d! [4 C - }% |* u) w2 u7 J% ^% Z
- }
/ }* V% n0 |9 v: u
複製代碼
1 p* ^8 [! K/ T# z. L% n( H% S8 @% jMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
A8 K) R4 s# e
# z4 e4 V m9 {. X5 M
|
|