|
|
- W, n/ _4 p, d! J1 ?! |' D
- #include <Arduino.h>. I& K* t/ L8 d! I% v) I/ Y
- #include <MeAuriga.h>6 O" d. g0 P) C3 R! r5 X
- #include "MeLineFollowerArray.h"
' J4 j7 m$ }* s - ' _& a# J5 J( w; d3 l0 I
- MeEncoderOnBoard Encoder_1(SLOT1);
! l; E) T9 a( o* r6 [% E4 M* x - MeEncoderOnBoard Encoder_2(SLOT2);
; S0 a6 c; `- ~4 P( \. z) R' J - MeLightSensor lightsensor_1(12);
2 t \6 @% `7 p3 y9 k - MeLightSensor lightsensor_2(11);, v( O( ]9 y; [$ ]: m2 o2 Z: n
- MeBuzzer buzzer;
. }* z K$ u/ ? - MeLineFollowerArray linefollower(PORT_6);
- U; y" C( h7 S: p7 Z+ W! L- e- ~, `' p, X - / g3 {8 X- X9 P- f2 |/ Y
- #define Error1 1
' i/ B+ }3 A. B4 L - #define Error2 24 @9 V7 }; Q' g3 s8 a$ K
- #define Error3 3( W% Z/ _( M! U4 l9 V1 w
- / ?: q. F( ]9 u1 L; s" M& N9 g( C g
- #define Kp 15
0 S9 O* k( b I5 c$ M* P - #define Ki 0.15% q5 S3 N7 ^% I+ G2 n; t A
- #define Kd 0.03& c1 I' p4 e; N8 O. y
- j4 ?& G. |0 x9 x* Q4 g
- uint8_t sData;
9 O) O$ k, X) g. r5 C/ k* n! n - uint8_t D1;
4 j1 J+ x+ {& q6 E - uint8_t D2;
, O0 `. G5 P8 T7 `1 J$ Q - uint8_t D3;8 Y$ E* ]9 `) c: Z$ ]! ~
- uint8_t D4;
1 Y: l. v3 X7 M3 Y+ g* E9 g - uint8_t D5;( E! v% d: `2 q) Z1 U3 N, N
- uint8_t D6;: d/ p+ m+ s+ W
- ! @ s7 N$ u' C: h( d8 f
- float previous_error = 0;
6 v% f3 T+ F0 O( p8 ~; @( W - float integral = 0;* N2 z7 l" q: _9 G
- float derivative = 0;) t1 y/ f `8 h
- int Speed = 160;
; E. j, _6 M, {* ^7 s2 z! r- y - float output;, c2 Y# G* w. B- j: _+ d
- 7 ^# v: }4 J; F- U% }4 k5 F: T
- byte Left;
! U! ?' }6 T1 S6 U( z - : q! p+ Y8 z: {4 e i. w, W0 B
- void setup()
/ {0 t; Q/ q4 d8 ^8 d - {
( U8 b- a6 M, d2 u - //Set PWM 8KHz
7 o: L3 Q! i; A - TCCR1A = _BV(WGM10);
0 |& C, K% c4 D. K6 f - TCCR1B = _BV(CS11) | _BV(WGM12);
* R" K' g9 z7 F4 L! x - TCCR2A = _BV(WGM21) | _BV(WGM20);
5 ]3 S6 w7 D3 m4 q9 |3 M, _: b - TCCR2B = _BV(CS21);
5 b3 L2 ~1 N5 R' U+ ? - Serial.begin(9600);
( M. Q+ o+ C; @6 x: |- J. r - buzzer.setpin(45);
! n; o. S' h2 @; d - }9 c4 i& @9 W: b/ p# T
- h! y( `: {% A" \6 c6 t
- void loop()
& Y4 n& n& ? U6 C - {
- q& T3 B8 H4 W/ Q7 |) { - while(!((lightsensor_1.read()) < (10)));
7 P% x( v7 m6 y7 `/ ^0 x8 q; t - buzzer.tone(1047, 500);% L q8 K* f0 b( e( x. k4 C3 x" A V
- delay(500);
+ G& A( A3 F% ]8 z4 s - do2 c: E" ~ V- ?
- {
" b( W4 I7 E+ s0 e5 @( v$ m: r. Z - int Err = getErr();( a) H4 j; H% s% K7 d t
- if(D1 == 1)- V" d# [8 h; C, G
- {
" J& @9 E$ z" [/ O/ E; k - Left = 1;
% [+ V1 b' K, F0 v3 Y - }
; i, `) r7 B, I7 V8 |1 {: S - if(D6 == 1)
+ a/ Y" _0 f1 S @ - {% b! a+ {4 t0 ]- h
- Left = 0;; y4 b3 @# ~# r# o% \- U
- }4 S# k/ m6 t* t6 |1 _9 l) f o
- if(Err == 99)$ s1 A: h' u9 O$ _; J
- {
9 R& x9 j" B* M - if(Left == 1)
4 I( Q; ]" _. ]% Z' ~* n1 s& y - {, d/ ~6 t1 _* Q' j x# @
- Speed -= 5;
- Y; {1 r: c$ S' l, C - moto(0,Speed);
$ y- [$ F k9 O3 F) U# c! _% D - do4 c t, m8 ~3 G0 _1 c5 ]* {4 K
- {, b9 S1 t6 z% J; i) M- e4 k
- Err = getErr();6 s0 m$ E, Y0 g: E
- }while((D1+D6) == 0);
; Q# I. `' `- l5 | - }, V# T6 p# r8 T# @8 E2 n' t6 N; q
- else/ i5 Q1 J5 o% _& u- J
- {/ X1 T" Y E7 b7 i& ?+ x% v& ]
- Speed -= 5;
2 V* G, a( d, u - moto(Speed,0);
. R. S5 _8 S6 N4 T0 O3 o - do& W, A3 ^5 K! \0 `9 G7 |
- {3 Z9 T/ a9 V' ^ J
- Err = getErr();0 ^+ J; w0 T8 ~3 \+ |
- }while((D1+D6) == 0);0 r& p3 h/ m4 U2 y
- }
1 c. V% W9 X% d5 x - }
1 h) T9 F! x% }9 x" O" j( p# ]$ m8 A - else
, T" r; i9 l1 t, R% [' D3 i. b - {
8 C* `" Z( `9 E( S - if((Speed < 160) & (Err < 2)) Speed+=1;
/ S- s( r2 ~7 y X; Z( H; F - if((Speed > 100) & (Err > 2)) Speed-=2;
: }! N5 \, z( u' \+ |- G; ]* n - integral = integral + Err;! w4 B# P& c% s6 ?$ T" }# s3 x
- derivative = Err - previous_error;
& U# n4 J3 [& K! `% @ - output = Kp*Err + Ki*integral + Kd*derivative;- W$ [. W4 u9 q+ ]6 i4 Z/ j# c
- moto(int(Speed-output),int(Speed+output));- C: x. q8 m9 z$ H, E2 B( m$ ^. T
- previous_error = Err;
/ `# u3 @5 u& v* Z( n3 F3 e - }
( E4 u$ O: v3 F - }while(!((lightsensor_2.read()) < (10)));
% y" Y- Z4 q1 S- s# b' G - moto(0,0);2 r$ T) i0 A% s3 f" O
- delay(500);: b; e+ R! Z1 }
- buzzer.tone(262, 500);
9 {& \, |! d. I- A4 Y - }* R! S6 Q7 j5 y' G+ {' L* V& l
- ( u8 X; ~4 O& m) q _1 v6 W
- int getErr()- o& G$ F, S3 q
- { 8 ~6 A5 X4 r9 G$ x% l4 Z$ W
- sData = linefollower.getValue(); i7 S4 m6 A% F+ D4 g, t+ r5 H* y
- D1 = ~(sData>>0)&1;
& d# I' v9 [+ X& u4 b9 H - D2 = ~(sData>>1)&1;
) U% }: L r8 x5 R- a: ? - D3 = ~(sData>>2)&1;
9 h& Y8 l+ w k8 L - D4 = ~(sData>>3)&1;: ?8 ]% h" O5 t8 x2 Y& a3 m
- D5 = ~(sData>>4)&1;" v: f$ z8 ~! U4 n: T- d; I
- D6 = ~(sData>>5)&1;5 j' N6 x6 x6 a5 l& ?/ q5 \
- int downD = D1+D2+D3+D4+D5+D6;
' }/ C: ~, e! T7 t - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);5 L) h- d9 c }
- if(downD == 0)( @6 Q9 e0 \5 L* F0 j
- {7 b& Q6 R' v8 Z: [8 t% ?
- return 99;
; V7 @3 c% Z8 \+ Y4 _ - }
% Z8 g2 n; x* F% R5 j9 K - else
8 U( U8 Y* q2 C; J4 w - {5 R! }" I# U, @; ~ q
- return upD/downD;7 Y6 c8 q' B. z/ [
- }& u4 l9 c7 v0 ], \1 B* c7 @" a
- }
2 T6 E, \# E( H# |8 w& I
4 e8 c p, Q2 F3 M0 U' k$ A5 q- void moto(int Speed_L,int Speed_R)
$ c8 Z, ]1 Q6 M. k2 W; t - {4 c: I" h/ W1 {3 L: i
- Encoder_2.setMotorPwm(Speed_L);
! c u: V- h! c0 \% i. a - Encoder_1.setMotorPwm(-Speed_R);! r' k3 r/ l/ S1 Z' `8 l
- }
複製代碼
7 ~5 P# {; v+ @; [& ZMeLineFollowerArray.cpp) v0 `; x9 C! w4 X- P( v: t6 @
- #include "MeLineFollowerArray.h"+ ?( {6 Y2 s0 i4 a& o, \& a5 _
3 P0 i1 j- ~1 j: f7 h% q- #ifdef ME_PORT_DEFINED
, m- c$ ~- i) D/ x - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
7 e; h Q' }0 @* L. A - {
0 L" G$ H4 s1 ? - 1 K- n7 t/ n9 Y$ A3 o* m
- }% j- B3 T2 s |& y( e
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
3 {9 P5 ~& K# Z5 k B/ h5 i - { |4 _8 _6 s3 v4 O
- _DataPin = mePort[port].s2;
: I+ Y0 n8 [+ p+ N - pinMode(_DataPin, OUTPUT); ; m1 F, U8 K0 k$ A( k' p8 W
- digitalWrite(_DataPin, HIGH);$ S; B/ @# l2 O
- }( g; \2 x1 y8 S, y
- #else // ME_PORT_DEFINED
& I4 W' B# a+ w' b) T& f6 i8 D - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)7 c2 {8 {2 m! g6 ?' M6 S
- {6 I' G# \0 G! E; J! {
- _DataPin = pin;
" ]7 [$ e7 ?- n/ J* e7 q {, X - pinMode(_DataPin, OUTPUT); & `) M9 j# r3 w, O! F
- digitalWrite(_DataPin, HIGH);( }3 b% t& X; q$ e; c
- }
3 @% w6 M8 M m$ _/ Q - #endif // ME_PORT_DEFINED
% O) m. P5 h3 P. C! V; Q - ' k3 t% n- F2 m2 N/ J9 b
- J1 a$ O& y5 D& `& ? y- void MeLineFollowerArray::setpin(uint8_t pin)
. i$ k) F! `7 o# u# C - {
# }) j9 C$ I1 T! o1 o - _DataPin = pin;
p) n% u# r* v2 I! B9 F: k - pinMode(_DataPin, OUTPUT);
) V; L: Y- I7 O' ]9 `) u+ j3 s - digitalWrite(_DataPin, HIGH);
) n; o" l/ ?. G _7 w7 V - 1 A: _. l# X$ b
- #ifdef ME_PORT_DEFINED0 C+ m, C+ P& ~4 M
- s2 = pin;' H, W6 d1 h. z8 n
- #endif$ J! R8 D$ l$ h- n6 Y
- }
' M5 P9 D3 Q4 q3 t9 M% @0 W
! b3 ^) m8 g! P$ F- uint8_t MeLineFollowerArray::getValue(), H$ e3 k# q! k k: Q! B/ r* ?
- {
8 i" O8 x4 o% x3 Y$ `. [, _2 b - uint32_t LOW_level_read_time;3 p3 o5 X# H+ h5 O/ o( z3 h2 [' X
- uint32_t HIGH_level_read_time;
+ ^& i( ^- b1 V2 I - uint32_t time_out_flag;
{5 i' W9 W- u: N" z: | - uint8_t Sensor_Data[3];
/ r j+ @, ?$ X! k# Y! h - static uint8_t old_data = 0xff;1 p9 I& m0 o J+ V' ]
9 N# Z5 L+ r$ y1 ]8 L4 r5 a- pinMode(_DataPin, OUTPUT);! o# M9 F+ Z: R1 W* e- d
- digitalWrite(_DataPin, LOW);
9 x1 |4 M2 @( _1 B - delayMicroseconds(980);0 ] S U% t8 j0 u5 ^& t* J$ V
- digitalWrite(_DataPin, HIGH);
+ E7 h: v& L- C/ V& }0 Y - # ]! `, X. N# n, _. T% l7 i
- pinMode(_DataPin, INPUT_PULLUP);& {( h6 U% v0 A
- delayMicroseconds(10);, x6 r4 I# u1 T, J
* W: l% y) U3 B. z" I) j& e- time_out_flag = millis();
7 @: T1 C; j7 A5 l' C - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 x( C7 V/ _; I
8 B }/ X( q1 S8 s U& d1 Q7 ?: K- LOW_level_read_time = micros();
" E0 h( Y1 R% x& \8 ~ - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
$ Z& P: _2 u6 C2 [ - {, J! m S7 h% R% ^1 ~. o
- return 0xff;
- D: i3 q8 z; X# N9 m, D - }. k; l% A, u9 I$ t9 w" N
- 7 k4 U- M' T$ a7 X7 t
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ f5 f* H: l' f# q4 V - ; @2 G# j) I0 Q, U8 _7 V9 z1 ]
- HIGH_level_read_time = micros();
! k* f7 y2 V- Y9 Z5 ]9 x1 t, S) { - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
# O A1 B/ w3 z' @! e
9 v. F" E& [0 J$ n' h- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
. B/ ?/ `6 {" z+ m( ^ - {; o. I" P7 O* d; y) Z! a$ f
- return 0xff;
7 |, E1 x6 Z1 `7 y+ R - }
( U% O$ N* K6 B# c
5 I5 E7 R4 L* T* j! p- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
. _* G7 c6 P5 k* D5 ]8 l; |0 w* a - {
: k( ~0 ]( f7 A# l. B' H - return 0xff;
! w7 A7 o1 [- v9 k/ |1 J - }! a& n8 g. D' g. ~! S( p. w
$ w- a0 E, P# I% z" q b- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 \3 d" a) g3 E" l/ o
- LOW_level_read_time = micros();% [9 b4 R' m$ F4 O; F
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
|4 J/ L" @3 } - % E/ O$ Y& [9 L& }" E! {
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out ~8 s, U+ F; C
- {/ w$ w' }& N% A0 B! y7 q
- return 0xff;
7 k* b0 Y% q# I- { - }6 k5 \/ l+ A9 i1 d: A3 w. F
- ( n% Q9 Q. w# G. q# ~2 Z- N
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)); B' J# z b8 U) o
- {+ }' c- t6 S7 N B+ r8 g
- return 0xff;
" Z7 R T+ d9 x+ G4 ?6 u - }8 j( s5 p. k6 J5 a
! l$ S4 ?9 l! H; p u& D- for(uint8_t k=0; k<3; k++)* V& }6 M& U) S
- {9 [; h" u2 b( F2 b5 ^* m0 ?9 Z
- Sensor_Data[k] = 0x00;0 D E- f. q0 a2 k
3 r$ ?* l+ U1 l4 Q+ J& n1 Y- for(uint8_t i=0;i<8;i++); \$ d* V: o- Z
- {5 r; r) N7 Q8 l) Q
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level* _2 C' K8 c2 K9 Z+ D: d9 q
- HIGH_level_read_time = micros();3 {! t0 f* q) f8 B( {8 A, B1 [) [/ L
- LOW_level_read_time = micros() - LOW_level_read_time;
* s$ v; @ P" o# p Z( s; r - 2 e* ?2 l: r, M0 z, |: f+ ^" _
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ), e- h5 c& R' H5 K9 Y- [( H
- {
) m7 a+ z4 m) L( E, z; r! w N/ e9 c - return 0xff;
0 Q$ [0 Q, t; Q2 j" r5 O: D - }
: X% ]" E7 `1 Q8 M* P - N3 u3 k7 Y2 W3 G( }& r$ z
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 _' K* j2 }% j! L, d - LOW_level_read_time = micros();
- U3 b r7 \4 @& a. ^# M0 q - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
6 ?6 ?7 f0 Q2 Z$ i! g
0 z, @ G Y, F# {2 s% M9 N4 a- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
8 q0 Y) y9 x6 u8 E Y) u* K; a - {! J8 Q, y, y% l3 F' z& u+ i
- Sensor_Data[k] |= (0x80 >> i);1 n4 Z! }- ^& I. I( h1 y; z
- }
7 \6 U$ V! q b+ @3 L x+ p" M - else if(HIGH_level_read_time >= 100)
7 G: }. Q% L8 x$ M1 x( ?0 i - {
; x! P" A/ L; p - return 0xff; K: V8 E$ c4 |2 D
- }
' l! ]4 ]1 {7 Q" W - 8 f7 W4 q" ~' J* T8 W
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)& Y; ~0 V q& ?4 n) S3 y9 J
- {
4 R7 y/ l* {; r/ `: N - return 0xff; |& `* @) H* g
- }
: k2 [3 ~1 N- J" I4 u) S: W( G h - }4 M) O9 ^" u6 W" M. f0 ^( l* m. O" b
- }
+ c' }3 g" t* y6 Z& h0 Q2 A' `7 i - # |8 x% S" @0 m& _8 f; Z+ [
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level2 U- A* f" q3 b+ L- v
- HIGH_level_read_time = micros();
) r# k# Y" ]2 a5 k - LOW_level_read_time = micros() - LOW_level_read_time;
7 d: T, z: I9 i/ u: Y - / X' \- I* v) C% w
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )" m& \% b8 _) r, W. g, M% }
- {& |# m' n& v/ m; n" s
- return 0xff;% N+ a4 M$ Z9 k# r
- }8 V- `9 P. C! h% B0 U/ u& Q9 E9 |
) {' ?7 A, I4 l% `# j5 E- pinMode(_DataPin, OUTPUT);6 l, W1 J: ] D$ I5 }' b7 _
- digitalWrite(_DataPin, HIGH);
! Y, o/ u E. {4 H" A4 S - 9 u9 I) g. R5 Z% g- f: h- `( d4 I
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
, R% Z2 z6 L' L5 P, v% @& i* U - {
- m1 ^9 t4 x: v4 y& d - old_data = Sensor_Data[0];
/ _# J! {% y4 |' D% `# x& H# g% x - return Sensor_Data[0];3 J# |. t! x' o8 Z& i* A- j. B1 ?
- } A/ J3 j$ L: u+ c+ y$ N0 i: d0 `
- else1 B' x, W9 V1 ~# y& r8 t5 D
- {; J5 f4 R) H2 ^2 C+ @% d1 q
- return old_data;3 w8 |; c" k1 y% d0 n+ f t
- }+ J* H. V$ N8 Y6 o# D
- }4 Y+ H1 K" X. O0 c$ \8 |) I
複製代碼
) [8 Y# B$ _! S0 c z' eMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
8 b/ j- S, I9 G' c2 `) Q
$ r: z: C, Q+ p7 q' j1 I0 K4 `0 S |
|