|
|
: s7 h. ~6 H" y3 \+ k6 Z
- #include <Arduino.h>
2 t' l' [. I4 v, k& t5 c' \. L - #include <MeAuriga.h>
# Q+ Q* w* R5 j0 q - #include "MeLineFollowerArray.h"
0 @7 W+ Z G( Z - # j$ } ~) c1 H4 \7 L
- MeEncoderOnBoard Encoder_1(SLOT1);
y* a6 {8 B6 R3 Y - MeEncoderOnBoard Encoder_2(SLOT2);' m+ \0 j7 w5 p
- MeLightSensor lightsensor_1(12);
6 W# m2 u) s0 z0 a9 j# a( P% n; B - MeLightSensor lightsensor_2(11);
- j8 e; H+ v( a" I - MeBuzzer buzzer;
, c g! @$ @6 P$ B0 t, {5 {, Y: i - MeLineFollowerArray linefollower(PORT_6);7 }% t! K; {( I Y% S
. P6 m/ m3 Z. i7 v/ l7 U- #define Error1 11 m# u4 k/ q% m" ]+ G% w: \ S
- #define Error2 2
! C* C6 a; _7 g% ~/ U0 P2 }6 f( X! {+ r - #define Error3 3
: I3 {/ l" {* [+ P
8 b' h+ G! G; M6 M t; `. W1 ^- #define Kp 155 A: Y; G' f0 g+ c; |. l( Q
- #define Ki 0.15/ {; Y: E: b# q7 \( K5 I" O/ O
- #define Kd 0.03
7 b! {& I1 O) K - 5 C# t/ z# ~3 b& J6 h4 T, ~
- uint8_t sData;
& d7 C5 r# K# F4 F - uint8_t D1;2 W: E2 q+ _, f" l0 G
- uint8_t D2;
. x) Q4 l0 ]) E# r# J; r7 C - uint8_t D3;
! ], e! G- i8 [+ E - uint8_t D4;% U. K5 _; ?- r% q- N' U
- uint8_t D5;
' w' V8 l. Z- L - uint8_t D6;6 h* C- N- U: @6 J& m+ U8 s5 ?; K
. u5 a& l1 L/ O" s7 }) ?/ d% O- float previous_error = 0;0 k* l( k7 O9 _ w, x8 d% ^5 u# E
- float integral = 0;3 k5 r+ K$ H, @- Y
- float derivative = 0;
: m; |: O; S) H - int Speed = 160;/ E4 L3 s1 R1 i
- float output;+ G2 n0 R) E; s3 F2 L* g9 I' W( R
- 7 j2 u* _+ I# f1 N- m
- byte Left;
/ J- H/ f' h2 L. P8 W+ b8 y - ; C& t3 n7 D! J7 x, w6 s# Y
- void setup() # F. z* d. c ^; Q
- {
; N4 \' w( z4 Z - //Set PWM 8KHz* `; S0 z7 E2 B. L/ L0 D
- TCCR1A = _BV(WGM10);% ?/ q, u U) x& S( \- `) J7 w
- TCCR1B = _BV(CS11) | _BV(WGM12);
# ?: e& @7 x; Z* p& w. ~ - TCCR2A = _BV(WGM21) | _BV(WGM20);* M" `0 r! t Y: G
- TCCR2B = _BV(CS21);
1 O' J8 G, c1 S. F8 i - Serial.begin(9600);
4 ?' ?4 B9 K3 K( M/ b9 a - buzzer.setpin(45);
+ {$ {" B& m0 x* }! B5 y% r- n - }
' W2 T' q; u }4 D1 @# t4 ? - ' L1 `3 Q' {, o" m2 h2 L
- void loop()8 j8 a8 N! ]2 @+ A
- {
2 f2 ^* T6 c+ O1 v1 J - while(!((lightsensor_1.read()) < (10)));! H) o4 K, M: n: D9 k+ G8 a3 q
- buzzer.tone(1047, 500);. p( a. y3 T6 w# q L
- delay(500);2 y. r- q* U2 G- ~- `
- do( T+ w1 }* v' \5 _/ N# w5 i
- {/ h& M1 y" P, @8 Y" g7 l. x+ x: E
- int Err = getErr();# P. v! ^; i+ }* h7 ^6 p1 \
- if(D1 == 1)
, J( k3 m0 v1 S( o) j: S* u - {7 g# k5 R2 @+ k$ ~
- Left = 1;4 i4 n( O( @. X0 Z9 g
- }* L1 u; h, E3 A* r
- if(D6 == 1)' C0 h1 `* |" m9 d3 K
- {
% k- o4 D$ W- I/ n( \ - Left = 0;( h3 \2 D/ v* H
- }* v" X! {' k( q; w) L% f
- if(Err == 99)* Q$ Z6 @5 ]. }( O- u% V- G1 ?) A
- {# F7 n9 U4 E4 T/ M
- if(Left == 1)5 M! l; c( o1 v0 p% X- w
- {" K4 o9 y9 ?0 x! S0 |1 S/ V9 m
- Speed -= 5;
5 I6 o3 M6 a/ K/ x. s2 ? - moto(0,Speed);+ v \) e) P: Z: @0 Q) D
- do* \ s, |1 `# {) R
- {9 k3 o2 ?. T# ~( z! R6 c
- Err = getErr();6 N7 d. G/ q( X: }
- }while((D1+D6) == 0);' P0 {. X/ |4 d0 x
- }$ \0 O% T' _6 Z4 z+ c ]" x+ W
- else
5 v" ^4 e+ B% ]8 M# A1 J - {( R# U9 i/ {1 m9 s
- Speed -= 5;) T, L- J& N, t) _7 Z" c
- moto(Speed,0);
. L% s, Q1 Z! R9 I% E/ _$ y! N$ b - do% w/ k) [4 [% G
- {" ?( q+ \" ^! f& |
- Err = getErr(); c$ ~. u, M0 M
- }while((D1+D6) == 0);. E _. c( H8 c
- }
& A8 }$ a; E4 }1 p - }% V$ }3 N- @% ], u$ Y: j/ y0 y
- else
+ y) a7 O. {' F1 S0 `8 [" u - {( R- R' W; q; Q4 P
- if((Speed < 160) & (Err < 2)) Speed+=1; - m+ R m; k {0 w# y' x; ]% n
- if((Speed > 100) & (Err > 2)) Speed-=2; 4 J0 G/ u$ @/ e( X H
- integral = integral + Err;
, s/ C6 N0 [0 ` ]7 d4 r3 X7 C) E - derivative = Err - previous_error;
8 H: \7 n# z9 d. v9 X! l - output = Kp*Err + Ki*integral + Kd*derivative;
; V0 i/ s3 M% y' q1 d - moto(int(Speed-output),int(Speed+output));
1 b. h' y0 o8 P9 J- u - previous_error = Err;4 e' {8 o; N( D. j! y0 E8 L( O
- }
& |6 g7 i$ f. e% ~* t1 h; x - }while(!((lightsensor_2.read()) < (10)));
0 M) [! `+ Y! P - moto(0,0); W" q& }8 b3 Q* ?
- delay(500);0 a" j7 i- ?$ W4 G' K
- buzzer.tone(262, 500);: N1 g' O; ]4 }& f6 \
- }
; O& `0 _1 }1 o0 z
0 B2 r" `! y m# ~: C7 t- int getErr()
) L* b4 }+ a# N$ q - { 7 y- W9 I; M( _! Y5 A+ l
- sData = linefollower.getValue();7 c. J5 |& I% M( o, [- F9 b
- D1 = ~(sData>>0)&1;
Q( L1 g9 E6 y5 E8 l9 c - D2 = ~(sData>>1)&1;
. M8 m: x2 U9 f: G6 s R5 ~ - D3 = ~(sData>>2)&1;' j8 D! j: |; e/ ?: U, a
- D4 = ~(sData>>3)&1;+ i) D( o+ j' X; y8 S# k
- D5 = ~(sData>>4)&1;% u' u( X+ X* M" r. A& k g/ \( S
- D6 = ~(sData>>5)&1;6 U" P' w5 t3 f* L: f* l" @9 J
- int downD = D1+D2+D3+D4+D5+D6;2 G/ p( W# y. p" p
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);6 O+ E5 L. E- l2 O
- if(downD == 0)& c% t; ]$ q& R5 k* M$ b! L9 O% i
- {
( x1 N1 {3 f! R6 u3 A! Q - return 99;9 ?3 ?: H- l( l( ` f- ]
- }
3 l% `- V, w% @( [ - else% k5 p' h1 m# N+ U/ ~; {
- {
0 Y0 C; |% L9 _2 X0 Z - return upD/downD;1 C8 g, ^ F7 X8 H# d
- }* w' p( u, e' O) P( u7 b
- }
* X, }) R0 v4 w) i, b - ) c; w; i+ B0 x b
- void moto(int Speed_L,int Speed_R)3 C8 |& {7 ^. @! O! m
- {
3 _. j1 p) `' r3 V* f5 l% z2 N - Encoder_2.setMotorPwm(Speed_L);* K/ n0 ]& S, o) U$ @/ v" s0 y
- Encoder_1.setMotorPwm(-Speed_R);
5 l) o- \: W' Q, v' I' J - }
複製代碼 3 z W* L8 j% R4 a9 d. }
MeLineFollowerArray.cpp# J4 a: l; r Z1 g: i7 T0 i# ^
- #include "MeLineFollowerArray.h"
! ^- B5 f. d. ]/ U3 a e
7 Q2 U4 }6 {$ o7 P- I; i% x8 p- #ifdef ME_PORT_DEFINED
& ?9 I5 e6 v4 A7 P3 R2 S- o - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
1 Q0 v" a# K4 a, ~2 k - {
8 S! L" {2 `/ }+ m; Q3 [5 Q - 2 a/ M' x Z9 F1 P. H6 x
- }+ _1 R0 t& v- n2 j/ K+ s9 i0 \
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
! n2 W5 W3 s x' }4 y' h) @& C, j - {
: c& r1 b# D. R! `3 ~/ p - _DataPin = mePort[port].s2;
1 ~2 F( M, Y) I - pinMode(_DataPin, OUTPUT);
' w5 M) A2 Z0 b" I+ G6 C - digitalWrite(_DataPin, HIGH);
8 Q/ @; Z( u! ~% m( G+ S& N - }
" M0 b+ n: k. \& [ - #else // ME_PORT_DEFINED+ z, Q B) [" u# W; S7 c
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)5 R l/ R) J. T0 K R
- {
1 J+ c1 Q7 V' n$ T) S: ^9 L$ D# W, b; v$ | - _DataPin = pin;
. V! B) U2 O0 G- p - pinMode(_DataPin, OUTPUT);
9 s. b9 j5 I1 i. e. ~3 U; X9 X - digitalWrite(_DataPin, HIGH);
4 w) G% ]3 x1 L( `: m - }- y3 Z8 ~7 [& G2 a: ^0 a
- #endif // ME_PORT_DEFINED: J9 T' ^& G: @5 w% b+ o
- 9 V/ Z6 P) @1 h2 D1 `: `
4 q4 ~, Y. Q" B! ?- void MeLineFollowerArray::setpin(uint8_t pin)3 e7 x' Z. U" ?3 }
- {
, X) ]* G1 k; r' o. G - _DataPin = pin;
- E4 V5 U) n: c - pinMode(_DataPin, OUTPUT); % F* p- p$ C$ P, \0 b" L+ N) a/ W
- digitalWrite(_DataPin, HIGH);$ D7 d7 \5 c7 b& n, u1 ?/ }
- ( X8 K1 f. e3 V" ^/ M6 y% L
- #ifdef ME_PORT_DEFINED% |/ O( |& j; ]& @: r7 Z' U5 R
- s2 = pin;
9 L0 q9 B! R& ^4 y% z/ [" g - #endif& l, k9 d; m6 e8 s% }7 T
- }
, C# l, _& L+ U! N - 5 s) Y7 j6 ]; }
- uint8_t MeLineFollowerArray::getValue()
3 o, z S9 {- T - {
0 T$ Q7 j# Y, c' B$ z6 G - uint32_t LOW_level_read_time;- Q/ X+ P6 i* w: z. W
- uint32_t HIGH_level_read_time;; e6 ?% U( `" p6 t: c
- uint32_t time_out_flag;
( M. x0 E: o7 h) Y3 E4 d - uint8_t Sensor_Data[3];# l2 m6 O4 L! t, x, t
- static uint8_t old_data = 0xff;
! F% \7 @. z6 `8 t/ `& p
5 P" z, F' i$ {; d I' Q& N1 d+ z8 S- pinMode(_DataPin, OUTPUT);
% N8 V% d/ r1 a8 T - digitalWrite(_DataPin, LOW);
% \% ]$ C- G9 b% c) P - delayMicroseconds(980);: E4 `7 j7 @7 ?4 _# m* b3 v9 |
- digitalWrite(_DataPin, HIGH);) A% J( r, ^/ M$ r
- % v* d/ _( T' b+ m
- pinMode(_DataPin, INPUT_PULLUP);
+ `4 H$ N( l( S- T* F# m: L - delayMicroseconds(10);
' G# Z0 r# \0 V# T
" F5 ^4 w# u9 w- time_out_flag = millis();% B" T- n) L) r# e$ X
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
* b# A' y/ q6 v" |
4 t5 _/ z ^2 L8 h+ ^0 @4 m9 f- LOW_level_read_time = micros();
2 L! L9 S) y! _ - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out7 W) y! H1 |3 r3 T
- {" B3 N- D, t0 L7 k
- return 0xff;
# j- j5 e$ H& q$ }4 F0 O. s - }- W/ M, C6 a. A
- & j+ u# a; t8 V, |( }' Y9 @: l
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' R, L* ~# }4 w m( `# j
+ ]& p4 u( {7 r* ], K- HIGH_level_read_time = micros();/ Y* I7 ], T4 s$ N0 i. d
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level% x+ s& H) v( r8 K
E$ l. |: w) l) p$ V- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out0 c% ]+ t/ H% C$ i3 o2 ~
- {& g0 O0 L( o! _( Y
- return 0xff;
+ t) y' T3 r& v9 i. V7 x1 k- { - }
2 h/ q: ^3 s& i7 s& M s
* j4 P* B& O2 [! ^- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))4 |# w0 b& x3 Q9 T+ \
- {
$ w, Z# E, p$ y - return 0xff;
) R5 r6 u' I0 Q; P( u" ^ - }' q8 z7 H( W' q, |0 O
3 n/ x$ N( q7 C" a) _- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 x; K& {1 D: ?; h% c9 y
- LOW_level_read_time = micros();6 R" ^3 t( [3 q. V! a- }( m
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level6 I3 ~0 z8 J/ I' A
- - j, j* U$ Y1 r
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
. O' p2 | h2 S8 w! Z2 b7 m - { ?+ A, H7 v7 ~) m' e7 g5 }7 z
- return 0xff;
! U2 l- w" R! K8 D - }
" a; b# `% K) @. o9 y - & `) ]9 |- B! \
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
8 B$ y/ o& x+ U) ]' U - {: q5 F {2 y% I. }- s. ~ u' g
- return 0xff;
}& m0 {1 X$ n% T* f - }2 ~. C9 G9 K0 ]- f, a* b
6 ?' b% p* e( r/ G: J' p- for(uint8_t k=0; k<3; k++)
1 K6 |0 g4 R, [- A4 J - {
4 z$ ^4 }3 Z. `3 [* d - Sensor_Data[k] = 0x00;
4 x# {% J' {- d: r* k" i
2 @) j; Y* g, f. i( G' F2 k- for(uint8_t i=0;i<8;i++)6 {0 }8 ^ `0 z# r
- {; a# q5 m" D5 Q% ^7 w' i
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level/ d* H+ i# d8 Z, ?6 Y- J
- HIGH_level_read_time = micros();' k0 e. |# C+ j- n \3 _ ?$ U
- LOW_level_read_time = micros() - LOW_level_read_time;
+ a9 I6 {8 P5 r" d - ' E% ]0 b4 C. S4 G# N
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ): x! u% O& S7 S! x
- {
5 K, f' ^# N9 c1 ]. r9 A4 B - return 0xff;
) T; M$ J& Z. d6 i: N6 v - }
6 w* k. f1 B1 D$ \; v5 ?3 L6 P
/ H$ d9 T) W) w& z) g1 f# ]6 A- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
) \& r1 A# V) C0 U d9 a, E' Z; @ - LOW_level_read_time = micros();; I2 G& t! C" t6 ~0 G# U
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level; I) \' z0 _( C3 U' ^
5 R7 z H/ s! |# F9 V9 Z- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1 J' S' x$ h* e
- {! F' }& Q& i4 @7 e) }% G
- Sensor_Data[k] |= (0x80 >> i);; L" k1 U/ D. C# k7 R
- }* i$ q% R. y/ l3 M" m# z, X
- else if(HIGH_level_read_time >= 100)! V) l) e, O, N$ w
- {7 T2 n4 C; x. ~( _
- return 0xff;
f+ n4 s) J @7 g - }
$ d. _: D g% E
/ c2 h& d$ }$ h- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
% L: @1 |& q; K8 p. v - {! Y @5 [3 C* R1 d/ y) K1 x
- return 0xff;
1 M: @* Q8 i! }$ h1 `0 c - }$ b4 R5 ] C: y# A
- }0 M2 H( N' v: o. W7 h1 j
- }
* K Q: Y( P7 B! b2 L! _/ k - : B% C0 N5 J* b1 z7 k7 d
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level v6 w* W- M( W. q8 L7 g
- HIGH_level_read_time = micros();6 U0 g3 u! I5 T2 l. a+ |
- LOW_level_read_time = micros() - LOW_level_read_time;
5 f+ E% }3 V O( D - " x; K1 D/ _ y4 y
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
* o. g8 b; ]* { - {
$ R; {0 U: ?2 P$ z6 m4 ]/ Z: [ - return 0xff;
7 z, L# l5 z4 Z# w - }, E* t! ?9 d0 F& p( p, w) G
- . q7 Q( M" A! x% d: M/ b3 ]) B
- pinMode(_DataPin, OUTPUT);
& b8 s, {1 P8 N/ H - digitalWrite(_DataPin, HIGH);( e+ j& b; a, {" s: G
9 r& d% H+ M( X& e) L- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
$ I4 M3 }: y+ J- Q3 p0 Q' m8 [ - {8 @1 o8 A3 r! M1 y5 v
- old_data = Sensor_Data[0];
( T1 ?+ E+ C) s% D - return Sensor_Data[0];$ A- w2 {- Z3 ^! K
- }
% Y' e, e! x$ l6 C1 `$ w; n2 {0 i. Z8 ^ - else2 N n% L) k2 x0 p' {
- {( d' M9 P1 x$ D2 d6 M0 h: H: T
- return old_data;: O& X" E) _) L2 J, {+ U8 g- W7 |
- }! G" h. e* f _0 e0 S
- }. X' W1 O0 Q+ ]! p
複製代碼 $ d( q, ~: }' M I/ F% y
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
* T; k {6 y4 u4 T/ \- _
: L ~1 n+ e3 B9 q# ^ z
|
|