|
|
9 ]8 z, h; Q' E) `9 Z3 P, ^/ g9 I- #include <Arduino.h>' s" j3 n; w% L: i* c" l+ W
- #include <MeAuriga.h>
: f( v/ W5 U" L- G: w - #include "MeLineFollowerArray.h". j; @% m9 P6 {# C0 K
- ! ?& w D% P. {! d( w2 ^7 b& ^% w
- MeEncoderOnBoard Encoder_1(SLOT1);
( x' p9 `( ]3 F3 U% j' e - MeEncoderOnBoard Encoder_2(SLOT2);5 d+ k" H; h V
- MeLightSensor lightsensor_1(12);8 ?6 K8 D7 q; n( N
- MeLightSensor lightsensor_2(11);/ Y' p$ ]6 d, X& e( f* O
- MeBuzzer buzzer;
) B0 O. g$ Z+ b* F- l( d% M W - MeLineFollowerArray linefollower(PORT_6);- V# g F8 P' L* r' Z+ `" o @ h+ ~
- 9 \6 w0 K3 f% | Y* g+ F1 z
- #define Error1 1; ^6 f1 ^; v' N
- #define Error2 23 V( j% F4 T% i, _5 U& K0 `
- #define Error3 3/ `" D0 K* X) |
- * X6 J$ H% O' a# R' o* A* o
- #define Kp 15
' d/ T3 t9 y* S& \+ N - #define Ki 0.15
# U7 ~! V3 v! |& d! x& A/ R - #define Kd 0.03
5 ?6 o( Y% b/ R; T, X
( k f7 }9 r1 `- M- uint8_t sData;
/ g* `8 Y) M' z) C% G, |% d4 t( r, q - uint8_t D1;
. S& p' @9 T7 C$ t' H3 g - uint8_t D2;. W2 D& ]& W! b. K
- uint8_t D3;! `6 n, t) L* V+ l7 a
- uint8_t D4;! }. I8 a3 g7 o. w! B
- uint8_t D5;
) L! i) k4 ]) j - uint8_t D6;* G8 q$ X* J) U. Y4 a0 ?; e3 E) |
- 2 }5 W6 {, d6 X& N( q5 E L2 ]
- float previous_error = 0;' h" T- w; r2 ?* u; a4 M, \3 r
- float integral = 0;# @8 O& D# N1 t5 @% ~0 t. R
- float derivative = 0;5 I: q {) \8 L- k; B
- int Speed = 160;
0 X% D! r. q! X4 g( t# K# Z- a( P - float output;
' _3 \( X4 S: O1 Z, [" l: ^, q
& c2 b! N# A2 N" ~; y9 r- byte Left;& j% K3 V3 g D) r0 I- e/ z
* K4 j2 ~- c5 K5 u( F- void setup()
2 I5 u- C# J# u* A - {5 v! V# m6 Y( F+ Y" W% k$ _
- //Set PWM 8KHz
/ ]: t1 y# D6 Z- W - TCCR1A = _BV(WGM10);7 N6 Z. F2 B6 M$ X. o, P, W+ ~* B' q
- TCCR1B = _BV(CS11) | _BV(WGM12);
& K1 C/ [( w1 J: T) F - TCCR2A = _BV(WGM21) | _BV(WGM20);
# m. M& B1 j5 R - TCCR2B = _BV(CS21);6 f/ @+ g: e. A& f t
- Serial.begin(9600);
8 L3 w0 ^# @* }' j" r0 L7 T - buzzer.setpin(45);: i9 t) S9 c" `! i6 m; n
- }
3 f+ `# w; d$ J* |, x# [ - ! i. c/ W7 G5 L# }1 D% [# s. W
- void loop()# m- t& V- L6 }. t/ G& U) {4 B9 |
- {
9 t) @: x Y3 l K" j w3 t - while(!((lightsensor_1.read()) < (10)));
5 g+ ]- g) e0 G& f. F0 \. X - buzzer.tone(1047, 500);
w% k: E5 d+ B7 V( _& q - delay(500);2 d5 y6 p. D1 n: P7 C" S! n
- do b) ~4 P y( ^5 c9 j
- {0 Z n% U- V9 Q/ o( ]
- int Err = getErr();1 l1 d- ?( ?0 P
- if(D1 == 1)
; u4 l2 w( _+ s: f4 S+ ~" L - {
2 E. T2 W6 c" g- S+ F; r - Left = 1;: A7 s' w$ w6 N; e% Y9 y" _4 [
- }! F _, u( h6 ]. h4 a
- if(D6 == 1)6 f4 O0 }+ x$ A0 J1 r& B
- {# Q# y& p, Q1 {+ ? E$ k2 Z0 J/ D" G
- Left = 0;
3 W, I" b2 x% Y# ^5 a - }- A2 _: L( z E; K
- if(Err == 99)
/ J8 K+ C, Y; S% @) f6 Q+ w/ B5 c5 K: a - {* f! a( I3 _' s H/ k. s6 X7 P
- if(Left == 1)
7 L! D+ g. S- v+ U( i4 _ - {, ] s( L, Q& [4 {
- Speed -= 5;
2 g' N" k4 u, N* v0 _ - moto(0,Speed);5 V& p* u' q+ V& U3 s, `+ r
- do* i) |, i) U5 s6 X
- {/ @1 }, j. [1 j# C$ F
- Err = getErr();/ k; [& ^2 M( d, s w) |1 V
- }while((D1+D6) == 0);, g* h) S+ x- |- W5 l" `7 m
- }
: h0 O' c. s! O0 S, T( `, n8 | - else
% l6 ~% F) q" w! b. v: M - {
: F$ f# }* y6 }* t" ^$ D0 ? - Speed -= 5;
1 h" \4 \- @1 R+ _3 E9 s6 U - moto(Speed,0); $ q2 e/ d3 V. ^0 p3 w8 q6 h3 d
- do7 ]/ |) g3 {" Q6 J# u0 d8 T
- {& ` K; ?' z! F, B; l
- Err = getErr();% G; d* c- o/ u2 E* x' h+ |+ y Z
- }while((D1+D6) == 0);
R' d( E3 t! L - }$ _# ]& t$ l4 @# m( z
- }
' V) I" l" s4 t) ^1 p - else
/ P C1 y( r7 \& X$ l - {
. H% l8 Q8 y) W - if((Speed < 160) & (Err < 2)) Speed+=1;
3 ^- [4 V$ i. i! m6 g - if((Speed > 100) & (Err > 2)) Speed-=2; & M' ^( \) T4 w( ?& D. A, i1 |
- integral = integral + Err;
% J2 j5 i: _7 k - derivative = Err - previous_error;
- p6 f2 S+ W$ L ?7 @7 @# ^, r. s - output = Kp*Err + Ki*integral + Kd*derivative;
8 l+ l' q$ H$ ~2 }7 a: h - moto(int(Speed-output),int(Speed+output));* X; \' z& }9 A, W1 W
- previous_error = Err;
: z# G% S! L$ t; D5 N - }, I. R) M# l0 \1 p+ c
- }while(!((lightsensor_2.read()) < (10)));! u1 l' u& k5 q% s7 H
- moto(0,0);
& U5 o/ A& t! W( q/ }) c& f8 Y - delay(500);5 U# @, V5 h6 J- V$ x0 Z& s
- buzzer.tone(262, 500);
, {1 @9 V/ ~ U# S2 ^7 ] v/ j4 _ - }7 C8 H: E3 D! W3 B$ q
* m# L# h0 f) |; R1 \, X3 x- T- int getErr()" r2 X: V# ?, W: z- y! k
- { & |$ a0 c# P6 l% r" q5 x
- sData = linefollower.getValue();
: U, I8 |* p& c6 u; k2 j' g5 ` - D1 = ~(sData>>0)&1;. E* ~( S/ N9 ]3 j
- D2 = ~(sData>>1)&1;" n; z% V; r- z
- D3 = ~(sData>>2)&1;2 h5 ]6 o: Q$ N* }3 d+ Z
- D4 = ~(sData>>3)&1;! L+ a7 U9 E) O2 ~
- D5 = ~(sData>>4)&1;
; Z6 _0 k: T4 r- I( U+ y; g - D6 = ~(sData>>5)&1;
p/ \/ j& I- R! Y - int downD = D1+D2+D3+D4+D5+D6;! N& j' f u9 t8 E! ^
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);& _4 c7 h# R- E0 G; Z* k
- if(downD == 0) @6 ~2 \$ s* s4 z" Y" C
- {9 ]/ I# f4 }1 ^! t. e
- return 99;; G; @! S9 p/ r1 @( B
- }
- {* w, Y$ y( \$ E: E* u - else9 x5 _% t5 Z8 {. h* b' R
- {
) \& V2 i% {; n" Y - return upD/downD;
9 w) S+ A9 V7 E% d; I' v - }! L2 q* a2 @4 F9 y; ^
- }7 y, M# v2 a# f* Y: v5 E
- $ q& ?% m7 q/ d2 Z, _% L0 Z
- void moto(int Speed_L,int Speed_R)4 S5 @% x" \ i: }) }# x
- {* m, }" o- D; {- @
- Encoder_2.setMotorPwm(Speed_L);
* o5 T$ K* d. `- l! e - Encoder_1.setMotorPwm(-Speed_R);' o7 c( f- e. d5 u. f2 d5 R
- }
複製代碼
( j$ j2 b8 H" W1 m% m1 N3 qMeLineFollowerArray.cpp# S U' }- s' J1 x5 \- M5 M
- #include "MeLineFollowerArray.h"
" v) w6 T! ]; s D0 }
+ U2 g! i/ `9 m8 K; a; [5 t- #ifdef ME_PORT_DEFINED
1 u& Y; a$ ?* `# o - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)! M$ w7 E% @7 c) F" U$ V$ B% F
- {0 O' c5 W* f5 p% V
- 6 S6 R: I3 l# \2 }/ v
- }! V9 }; L; l. \# O- A9 t* l2 S+ m
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
) X/ b' h$ C4 Z1 I) N; \ - {
* L/ P5 ^ L5 | - _DataPin = mePort[port].s2;* \7 E1 Z- Q5 A/ G' I
- pinMode(_DataPin, OUTPUT);
, K& n. K6 E( }' S - digitalWrite(_DataPin, HIGH); u( c0 s' b: M0 h% Z; \9 V/ j% Q- z
- }1 y% z# N% [5 b& a8 d
- #else // ME_PORT_DEFINED
0 { g5 K) }; B% @, b - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
+ C* y9 D2 Z! h3 x# j+ t - {
: J1 g9 ?, A8 C, X6 {* i# x - _DataPin = pin;
s, v- E% x1 r) _( ~ - pinMode(_DataPin, OUTPUT); # g% t& |% l" B5 P' u
- digitalWrite(_DataPin, HIGH);0 n3 [9 S5 l5 Y1 }- H. z
- }
3 ^1 K% _- u$ a& ^7 L- E, O4 j Q c - #endif // ME_PORT_DEFINED
. ~& b3 q9 ?. n7 u" w( B- S
% J* e( Q3 k% P; V9 X& i- : O6 b0 i. ]0 S3 O+ L+ h
- void MeLineFollowerArray::setpin(uint8_t pin)
* l1 \+ D+ A( d2 A( T - {
+ b+ S/ X z1 @/ a; y$ t - _DataPin = pin;
" s. ?8 w" i% c1 f - pinMode(_DataPin, OUTPUT);
; c4 q s0 a. h9 p: E# o - digitalWrite(_DataPin, HIGH);
2 v: I+ w- n" x( e, D5 M
6 t" D7 N' B+ s* [- #ifdef ME_PORT_DEFINED
) r) e ?' }, Y. x0 b% q - s2 = pin;
/ R5 X- i5 s4 n# }" L* X - #endif# B4 h. F4 V. J
- }
: W# C0 P2 y: t) A* q- {: m5 [$ Q
9 b0 ^* L& F* n' } m& `# p1 l' A- uint8_t MeLineFollowerArray::getValue()5 A6 H# U) K/ {6 D1 e4 ]; h( n2 G# P
- {
7 N; N- f3 e( i2 r" m& P - uint32_t LOW_level_read_time;# d* C( {2 g/ }9 Q; l& c3 @6 n
- uint32_t HIGH_level_read_time;- Y# a( o, m4 C. l
- uint32_t time_out_flag;, v( w: F' ~, \% f0 G
- uint8_t Sensor_Data[3];
% a4 U1 O' s, a& E9 U1 S - static uint8_t old_data = 0xff;( U0 j f. J5 F% l$ X
- $ e. ~" l: i' z. I; \
- pinMode(_DataPin, OUTPUT);
4 x% R. ^* U/ B. O7 V - digitalWrite(_DataPin, LOW);
# ?* j& x' G' E, A - delayMicroseconds(980);3 C$ H& _ I+ u; |$ V+ q
- digitalWrite(_DataPin, HIGH);8 I Z f/ u, }. B/ E/ K
' l$ D$ ^6 E- ^0 {( z% r' J, |- pinMode(_DataPin, INPUT_PULLUP);
- v7 Y# h- x9 e( L4 d - delayMicroseconds(10);
% @/ [+ B1 i& j, ^5 A C - + F A: v2 C! r' I6 f7 S* _
- time_out_flag = millis();
' l2 ?" q o3 b+ {5 X l- } - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
Q+ c C, \- F: ]
, l% m; d' f' n, b- LOW_level_read_time = micros();/ V% F( V" a5 G3 v0 u
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* v5 x% N( ^5 h/ j* X
- {
$ {# ?4 `1 z( M2 ^' c" x - return 0xff;
$ ]: [$ ~* Y% S4 b. p - }
) {$ Z7 P# |* ]1 P5 H - ! D" H7 N+ S2 S: |0 r
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# i( T" _4 N) @4 w7 K+ ]; S0 O - ' K* F, N7 `/ Z" N. x# R
- HIGH_level_read_time = micros();
" d/ K; M4 P/ `* a6 D; J" f* E - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
, U( j/ M, \- i: O h1 w {
0 ?) R# u3 n& A* }7 w- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
# A5 L- q2 Z8 ]1 H1 z0 N9 c! N - {
3 b! h3 O# u" n7 |. g% s) g. W - return 0xff;, p8 R D8 n9 B/ o* {/ D' u
- } ^# M$ @8 R! J7 s( i% x) \
' M8 m/ s' Q/ ~# i1 V- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
9 [8 Z; L% v* I - {/ K( |$ i7 V, o
- return 0xff;% }- F4 O4 T g' h, {) I
- }
! Z0 `. J M8 r9 z+ c( ^ - ( t8 v' {% \4 i5 c" A0 W
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
0 d: A+ z- l2 k E7 I - LOW_level_read_time = micros();' R3 M: s, Z! {4 e& Z' `
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
& d/ B1 @( a( I) F+ t9 p - 1 I6 V. B6 r1 ^
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out4 ~" ~5 `- v- u7 C! J
- {6 [( @- I$ l1 t1 Q& ]2 x; [
- return 0xff;
1 i$ a3 f2 p9 u2 R# S - }
0 B; t0 a4 u+ f% }0 W+ u; _ [ - . Q5 d! z6 r. M$ d* K# b2 m' _
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
5 U) C3 e0 B& V - {
7 v1 S: H% f# m: W3 A! G: T - return 0xff;
0 @6 D1 p6 U2 \5 p" t9 B% O - }1 W# R3 \& I: j/ @' b! B9 H
- 6 r, k m' ]5 w4 E5 P
- for(uint8_t k=0; k<3; k++)
/ |! b$ W ^1 O0 r; ^1 Y& g - {
. f9 O) C! A+ q1 h - Sensor_Data[k] = 0x00;" W0 \3 Q/ B: B
- 1 r2 G0 {: N/ Q. e% M$ v
- for(uint8_t i=0;i<8;i++)
2 x6 {. |+ [1 A' o4 j$ a: t - { T2 }- |5 G7 o( S
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level; p/ p# a" W5 u% }
- HIGH_level_read_time = micros();" L( O) {5 v' s: R. c9 e
- LOW_level_read_time = micros() - LOW_level_read_time;
' \0 h. i7 D% j- o - ) s2 s1 z$ V9 `9 A5 K/ ^' Q
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )" E% [) w* g/ Z5 t# x$ }, h
- {0 X0 W% @' \8 }) l
- return 0xff;8 t) K. u1 Y4 c" ?- M% T' D
- }, e1 l+ x: S' u1 L$ g- V
- 9 h3 g/ _/ k6 W4 U
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- C+ n- D( `4 V3 w E( S
- LOW_level_read_time = micros();' i5 m) u% H, B) \, w5 @
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level8 \! O# h3 F$ a4 |; j
+ q: i4 t7 t" \4 p6 l6 h- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
2 t3 d$ a! M$ i5 z1 j% G - {
) V1 W/ D5 j' Z1 c8 n) z. \ - Sensor_Data[k] |= (0x80 >> i);
- I# u! p9 i* X" ` - }
) _! F. [9 J8 r1 a - else if(HIGH_level_read_time >= 100). X& J+ [# E1 |9 t) ?4 P
- {
# X, h1 f" w q0 a- ^) b) j' U - return 0xff;
+ s! _/ N0 ~, s2 X- z" D; `0 z - }
% T9 ]' r' p" \1 |; Z0 j N3 M
1 v, S9 }' D) J5 N! _- Z( X! M- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
2 w2 p, ^9 p+ e. l - {
4 {* ]9 i6 Y8 o, H - return 0xff;9 w1 L, S5 l8 c; ?; e9 j( {3 j
- }0 h5 w9 Y4 M: ]3 }3 T( @1 {! i
- }/ b" u! l3 s# l) k) |" U
- }; j3 \4 X, x6 M, G
- $ p! V- M" B6 s+ D1 K
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level( J2 p: b1 E3 @
- HIGH_level_read_time = micros();7 Y# |6 b5 i: _! t- n. y& A( P
- LOW_level_read_time = micros() - LOW_level_read_time;5 S, F+ ]% `6 d1 T" ]- S) C
. ~5 v& X6 u3 x1 k8 q9 g- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
5 o% D# a% @% z \; ]4 W - {7 v" L5 t8 @0 t* K
- return 0xff; w, a3 U9 j/ }. B: E$ n# V$ g
- }
" w$ `/ h/ F9 g- C - 6 n, J5 T# _9 a. G" j% `, Z9 @
- pinMode(_DataPin, OUTPUT);6 j+ d8 G* C( o
- digitalWrite(_DataPin, HIGH);
$ q. v2 c# ^/ v3 h: b9 n
1 H" ?$ a# L5 |6 k- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))" D3 f* _+ n* j; i. L
- {2 r/ w0 F, b0 N+ y$ v1 }
- old_data = Sensor_Data[0];+ r4 E: k: S. Z) M) V9 a0 c. M
- return Sensor_Data[0];
& x, C- k% A# I# }0 [ - }
0 u' O4 y4 |* { - else8 W' k- ?2 z% r
- {
& x/ @. N* t1 q$ m/ V$ b( z. y$ w - return old_data;! s- L7 D1 K; U# c M! y
- }% K' Y1 N9 N3 G$ P3 E4 T
- }
' f3 ]- Q6 A- Q/ G; Q& s
複製代碼
! u3 S3 n$ F( k7 t% c& nMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
. S( I& b+ J w8 D; G. M' p; {" Y
|
|