|
|
+ z, s2 E; N# d4 {( ^/ r
- #include <Arduino.h>$ m+ K7 Z: m2 R W& @
- #include <MeAuriga.h>. o( W# O4 ^9 ~# {* A2 f* m
- #include "MeLineFollowerArray.h"
4 `1 E9 j9 D5 e4 o
1 A6 ~% @9 } g- MeEncoderOnBoard Encoder_1(SLOT1); x# _ G, Y* R
- MeEncoderOnBoard Encoder_2(SLOT2);6 i7 L' T% S# L7 k
- MeLightSensor lightsensor_1(12);
! I+ G6 ?( |5 v4 G3 x' b6 p3 C! m - MeLightSensor lightsensor_2(11);4 u# m/ J' z/ h& \6 `: v( D
- MeBuzzer buzzer;+ V# }. K( m7 Y3 W: y4 Y
- MeLineFollowerArray linefollower(PORT_6);
* Q+ D4 v5 i; O. M9 Z1 h - * w- v+ N$ r& e; R- ?8 F
- #define Error1 1
9 T8 n2 o- f# r* i - #define Error2 2% P) C) Q! X6 e1 \3 Q( o
- #define Error3 3
% M3 x+ u3 Q! W- K. w - & f! k$ v& D& L; ?
- #define Kp 15( @0 h3 A& g' c L. }, T6 O+ S6 L
- #define Ki 0.15
& g j9 {5 {8 n, f - #define Kd 0.03! Z+ d. L3 X/ E( u8 X
- 9 O' b) J( E4 u; ^8 N8 G
- uint8_t sData;
# K8 l1 s4 ]0 X; P- ]7 C; W - uint8_t D1;
3 B# \% V8 w% M% w5 [( L3 f' q - uint8_t D2;
" I. j; f* n, {1 J: a& @+ Z/ _8 L- @ - uint8_t D3;3 A1 \1 L6 Y5 ~! j
- uint8_t D4;
# N: }4 `1 m7 x1 [: |% p5 r+ d - uint8_t D5;
4 O u0 [5 V+ {# w9 H r0 @- i - uint8_t D6;* ?+ A, K# T& {
. c1 T' E" G$ k: w2 R! b3 b* F$ b- float previous_error = 0;- f. M1 @+ b/ m; ^ y
- float integral = 0;7 M4 p; S* C6 t% N$ N! Z
- float derivative = 0;
a3 Z% _9 j8 ]- A" ~6 X - int Speed = 160;: z3 W; Q$ c1 [7 g* E3 O( R
- float output;/ S1 J ^) U5 k7 U( I! @0 b
- 9 I% g6 Z7 f0 ?& y' N$ {
- byte Left;
V: r D/ R. |7 |$ ]
7 ~# b3 x8 P, }: Y' c- void setup() % I6 H; a. }# f/ s) ?
- {
P: c, ~& y6 T& _ - //Set PWM 8KHz
( d1 ^7 h! R- j# V% r( _( ? - TCCR1A = _BV(WGM10);
0 N- X& _( K- K, W+ W5 m. i - TCCR1B = _BV(CS11) | _BV(WGM12);
9 l* R* N3 c$ z( h - TCCR2A = _BV(WGM21) | _BV(WGM20);6 @1 t9 H4 p( R# F- g) G
- TCCR2B = _BV(CS21);; }- V. O$ C& U) K( B3 j
- Serial.begin(9600); h: M8 i. ^) k; o( c. c Q9 M) c3 W
- buzzer.setpin(45);
8 i& K" g! C1 l) h: |( a' r - }
" Z, g6 d7 E: F/ O
4 ]- x5 R0 e% j) r1 C" I- void loop() n3 l; \, F: b! _3 i. y% P. s3 {
- {
: z9 I( Q; O( Q- x Z, P - while(!((lightsensor_1.read()) < (10)));* Y8 V, x! L8 a1 o: t6 q: X9 W
- buzzer.tone(1047, 500);5 x% A0 p6 A4 \6 G, I$ N9 X
- delay(500);
1 P# \3 T7 E1 _/ }! q5 z4 r - do8 M7 m" g# }6 w
- {
5 l0 J" Y. d) l, k4 i - int Err = getErr();( G' Z5 N+ T: x
- if(D1 == 1)8 h6 s$ |/ N" R4 v k& ]
- {5 g6 z; _7 s8 g3 X2 g) {( g' E( G
- Left = 1;
9 ^& l3 L+ i, B3 q( g6 b) _ - }
6 a* m0 i6 r' i2 y - if(D6 == 1)
* z! U2 E- v) |* X/ `) M - {
5 l2 @7 T" y. Q- Z5 A7 g - Left = 0;5 C9 |' W# \/ A, D
- }$ j# K9 J: z( C- w: N! p
- if(Err == 99)6 ^. ]5 d, }3 \4 x* e" [( @
- {
( ~( b/ g- p( c& {. O5 | - if(Left == 1)
2 T2 U" [; H9 J - {
5 F3 h0 b9 I1 [/ \" G) z - Speed -= 5;) A4 ^2 a- x4 a, B- w( A3 n
- moto(0,Speed);
2 ?% W0 G1 ?$ c9 C# t6 E - do
5 K/ _! X; D$ I - {; L4 K- X' W% \8 Y% R
- Err = getErr();
9 m( o! U f( a - }while((D1+D6) == 0);
6 V2 z0 M8 p; N ^, t - }
1 [) T1 { n- U& |0 D7 D. G6 j - else5 k; ~- J* ~# f, \: y; r3 l
- {
' j' F5 C; p4 `0 U! a' n% b - Speed -= 5;
" ?! r" E8 O+ A7 a - moto(Speed,0);
( y5 F, {' S; @/ g+ [$ j - do
5 Q. N* O1 r6 L1 K+ E$ v/ j1 F - {/ f0 n: ?4 a2 \; @0 T
- Err = getErr();/ a* J$ H: P5 [- N! l( P4 t
- }while((D1+D6) == 0);
! ~! g4 U, b+ N* W! s! _ - }
O. `* `$ h. h$ `/ Z - }
6 y& I8 R9 b- _ - else3 H$ `1 i- h+ |3 D( i: w: R* ~6 g6 A
- {& @/ e" n, j& I( W0 ]1 x
- if((Speed < 160) & (Err < 2)) Speed+=1; - ^4 }8 x0 _. b& r/ X7 n
- if((Speed > 100) & (Err > 2)) Speed-=2;
( v; e$ J. |+ R) s. z+ a* S' ] - integral = integral + Err;
w0 _' u- r; S - derivative = Err - previous_error;
0 w# D. S$ r# @+ l; u; V) N! J - output = Kp*Err + Ki*integral + Kd*derivative;
9 |) F4 ]4 W0 {4 z1 u- v$ A2 l6 b - moto(int(Speed-output),int(Speed+output));% t, y& i6 O$ m7 h3 L9 V* R$ Q) L. Y
- previous_error = Err;/ }" b) p4 n1 j3 ]1 K! D# S
- }
; Y. ^5 z- c& L9 n) ] - }while(!((lightsensor_2.read()) < (10)));
& F9 `( Z: l' s) n5 s( }6 ? - moto(0,0);
4 H# l4 A7 \+ y) I b, G* } q - delay(500);
( |( Y9 L+ Y- P' O( @ - buzzer.tone(262, 500);% p- c6 T# p g2 d1 a
- }8 `. H, C; a/ L t0 ?
- ! F- K* v+ X w3 P6 \+ ]8 l
- int getErr()
, h! e/ w9 C7 g# _ R# m+ u - {
/ W7 p) P# r( A5 F) F' G - sData = linefollower.getValue();' h+ }! O5 V3 Z7 @! S: g
- D1 = ~(sData>>0)&1;
* k3 ^9 e* p3 m k - D2 = ~(sData>>1)&1;
/ ~$ w$ I3 g/ a9 Z" R( v - D3 = ~(sData>>2)&1;
( Z. e2 D; u: m# \- G - D4 = ~(sData>>3)&1;
! @0 f. k0 l' V/ ]# V' ]. f3 U/ r - D5 = ~(sData>>4)&1;3 H1 J% I2 s. L1 y/ l- K. }+ G
- D6 = ~(sData>>5)&1;+ v5 ^ q& U6 o
- int downD = D1+D2+D3+D4+D5+D6;& N& ^ _: @' |
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);/ ], H7 S/ p+ C. [
- if(downD == 0)
" {: p+ H5 y* ]5 \( i0 g2 ` - {
/ w; ^3 @# o0 A1 X# D/ S* f - return 99;+ J& m( l- x5 ?# c+ x
- }; Y6 q5 p* e7 O! P% ?; w
- else# P C F3 b1 @) u1 H7 q
- {
- x- \# h/ ~+ M, E" T4 i- U - return upD/downD;
C# n+ n: Y) Q# {; e0 z+ j - }
5 x; q6 E2 W- j( S3 p4 `: \ - }, E8 E' [( w0 K, \
- 1 B% X/ K7 ~) \: u6 Z6 ?
- void moto(int Speed_L,int Speed_R)
1 s6 n3 W5 e; K1 Z+ z0 o - {- h, Z& n4 _, L# `
- Encoder_2.setMotorPwm(Speed_L);# Z* y; h1 [! O1 L3 V
- Encoder_1.setMotorPwm(-Speed_R);
; t; K/ [4 g }9 m) X g+ i' |$ c - }
複製代碼
2 J2 P) w4 V/ n) \% ^MeLineFollowerArray.cpp
3 n# B% V" T! b- #include "MeLineFollowerArray.h"
P+ @2 W" ^- F. ?) c& i: U% m - 0 h; Q2 y+ t/ m$ {
- #ifdef ME_PORT_DEFINED" e: h4 J( ^- d
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
/ I4 D# x6 F' q# U7 L% F1 j - {% i) a* d3 l9 t( H
* B1 N/ J8 x- C" D- }
4 I+ b( y. _2 H+ J2 b - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)$ A$ V3 Q+ F# a$ ]
- {5 p1 g9 c, _7 M- \/ X ~9 q
- _DataPin = mePort[port].s2;5 p& z4 f: b. L% }( ~6 B. v
- pinMode(_DataPin, OUTPUT);
2 y2 N7 U8 @/ h; ]5 m* V - digitalWrite(_DataPin, HIGH);
n. S8 _: S; }0 T2 { - }
2 |* |. ^2 x) v8 M8 ? - #else // ME_PORT_DEFINED t6 m+ @: O0 X$ M
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin). F$ [7 S4 u/ t3 _
- {. Z' A/ }+ ^; a v
- _DataPin = pin;
& Q1 O l& t2 P) A6 ~$ T* k y - pinMode(_DataPin, OUTPUT); 5 G( n- b1 Z% b3 {- z
- digitalWrite(_DataPin, HIGH);
+ j- ~- t* W/ I/ `7 \* ~ - }
# N/ K$ B0 F, J! d - #endif // ME_PORT_DEFINED \$ [! A: T5 I9 ^5 G2 G9 y0 H! }
4 w* | \1 @' _& M2 G7 w, p
) F Y- a, X% J" c$ |) c- void MeLineFollowerArray::setpin(uint8_t pin). F& e* t2 W, k8 z9 s* G, o& [
- {( S% o3 V/ h$ G
- _DataPin = pin;( p# I8 N. c5 d& h8 T2 H
- pinMode(_DataPin, OUTPUT); ' K' B1 F' e# h' j) P% ~& n
- digitalWrite(_DataPin, HIGH);
( L- ^3 |2 |# f# F: `
/ Y6 k; R) B* d) G- #ifdef ME_PORT_DEFINED6 \8 `8 ~ U. i; _0 ?. D
- s2 = pin;- j* e0 Z( A7 ?$ j
- #endif
, H) T: |) j m0 t" G4 W! b - }
, Q( W9 @( B6 p) N- @ - : a+ N3 h' w* h# Y# t8 H e
- uint8_t MeLineFollowerArray::getValue()
0 D+ n" W9 E, o' k: I - {, f2 \) _6 F: l
- uint32_t LOW_level_read_time;) G: G2 v4 j0 M2 b
- uint32_t HIGH_level_read_time;" x( s9 _: Z7 S* Q+ a% O/ }
- uint32_t time_out_flag;
: H% K/ U1 K: z E) E2 u' _* N+ F P - uint8_t Sensor_Data[3];4 L! G3 H; `# F' f) P, z
- static uint8_t old_data = 0xff;
2 r% G5 V5 y( r; T/ G - 1 Z5 L3 b9 l1 N2 {6 A- p
- pinMode(_DataPin, OUTPUT);$ V% P- ~7 z! l8 \4 s# Y
- digitalWrite(_DataPin, LOW);# F6 y1 d9 L% R6 Y1 M
- delayMicroseconds(980);
3 u; `" }8 }- z# m3 _2 E! y, N+ E, ^/ l - digitalWrite(_DataPin, HIGH);* C4 t0 x3 H/ s$ |) @0 Y t/ D
- 6 w& x; A$ `) t$ _# \& L
- pinMode(_DataPin, INPUT_PULLUP);
5 e3 |* V1 B6 q; J+ ` - delayMicroseconds(10);
0 Y7 } h: p# X H0 T: ^. z7 v
7 d3 e& @8 W' O6 u: v, f# Z- time_out_flag = millis();5 z- j' e q4 o$ @, `
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" K2 `, T' R5 X8 y, a
- # L. ~' h* T5 F+ l* y
- LOW_level_read_time = micros();
& d1 h( Y# u2 |( D0 L2 R - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
. r$ F% _2 M8 G; Q1 ]7 Z% q - {' w2 T7 A2 x2 c9 U* [7 }( x
- return 0xff;9 Z. P, _ b# J) i0 f8 G, s
- }
, c, a- Q. p: S+ D7 f& b+ B) q
2 Z' d3 D) }) S6 _7 `& H- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* ~; c# Y: E. H1 b+ E! ? t: O% [
- " b2 c! m; t3 D+ e" a! t0 s
- HIGH_level_read_time = micros();
" b% |8 S& T" P# B" H3 _ Y; q - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
$ ]! v' _. {' o! `
, {( m5 ^; _4 ~9 W5 l; a- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
( \1 A9 J. U% l5 R: c6 F# W - { k# n, j& @' u
- return 0xff;) e$ j5 E/ ?) A# G( H- T i
- }
1 v" e' _' n( n& D3 ^2 Z& z - + d1 O4 h$ d( m, C( W# {% c
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
9 [# @7 z# b$ @# l" a8 t - {1 d6 e7 e- D0 z. [ B6 Z+ \
- return 0xff;
2 c. D& s& w* l; b - }
0 r3 l0 ~; a8 `6 R3 e0 y6 e
8 @6 |+ E3 ?, ]# s. [9 @- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 M" b- s: |$ r) Z) l9 o% {
- LOW_level_read_time = micros();. C8 S2 `6 Z( }3 g
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
2 A4 H% F- A+ p- N - ( O/ O: X+ N. k5 N- I) t
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out# h) n4 ]: Q% Z: t/ X8 @
- {
/ F9 C! ^: f9 ]# o - return 0xff;6 p0 s0 K* B" P
- }
0 p3 J0 K7 `8 a- Q
* X! \9 z3 w( O9 E; X- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))5 }; |0 h. H" S' w3 ]7 T
- {
# o3 B7 D3 W0 s5 `1 f. M - return 0xff;5 Y" G/ G8 d5 g9 K/ M# B" ]
- }' h2 \. R( `1 B6 U6 A
- " n/ i" H+ C/ Q; D
- for(uint8_t k=0; k<3; k++)
( z8 l! b7 w1 S7 |, o+ ~9 P5 H - {
1 h9 b: P$ o4 F( }, w8 B - Sensor_Data[k] = 0x00;* [4 V a# r& Q/ c7 }; T% q5 V3 ^
- ; R; k) r7 A. w! H$ q% s
- for(uint8_t i=0;i<8;i++)* O" ]; k" N* D) H1 o8 Z6 N% ^
- {- I0 G8 D$ q7 S/ Q4 Y
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level; L& ?. n' }& |" m5 j1 g, f) \
- HIGH_level_read_time = micros();- {+ A* M* s8 u( }& b. P/ s0 I0 t/ d
- LOW_level_read_time = micros() - LOW_level_read_time;
0 ^9 Y4 k4 G) u3 e- X8 g - / g% {- s3 K: E- p+ _2 C' a
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ): p8 `: o9 E- G# L
- {. l {7 q! v" g% M* u
- return 0xff;
; r" b7 L1 X+ P c" b* q3 K - }
+ e; { F2 J! b
& `$ R; H9 e1 f0 T5 G5 b. }: y- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 F% a& m9 k9 [) x+ p8 ~3 \( c' P - LOW_level_read_time = micros();
2 @7 u. n |! O& E' r* ~ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
' L& H& S: ~3 r7 x
9 _8 E, |( P" T1 K3 r( {- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
/ d! U. ^ k: ?9 { - {" n0 j- ~: {1 P3 Y$ K
- Sensor_Data[k] |= (0x80 >> i);- T0 a! ^# K. u: H; X: s/ ]
- }3 A, A6 |$ |! v9 X+ }
- else if(HIGH_level_read_time >= 100)
' `8 z0 D( p3 M+ B3 n- v - {7 a- j( f# T; u/ G1 q, ^: ~% E' v
- return 0xff;, a- k7 A( q1 N8 F* U4 y7 H+ g# M- O l
- }5 z9 z# B9 R+ c+ o# x
- 0 D9 Y3 r5 F$ U0 a' p
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)0 K0 ^& H9 \( l
- {
- `# O+ H. d. c7 Y - return 0xff;+ I2 |( c# y# z$ } }( `! W2 r
- }
4 _; |4 `% u) V5 k - }0 h* u) U2 @( Y5 \- H% R. {
- }
* f" B X$ S8 R$ d - 8 _( _/ `$ m4 a4 l5 M3 r, [
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level& [- b) |- V6 l/ T0 ?# [+ `' X
- HIGH_level_read_time = micros();: K5 i2 e: }8 [) ^
- LOW_level_read_time = micros() - LOW_level_read_time;
" `5 `4 l1 D/ h( T+ x/ E4 ?
! X0 y/ Y @* d- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
Z# D6 r, B+ E0 v - {
! t# ~" Y% j+ n: [# P - return 0xff;
4 j* O @; b* W$ {& f4 e - }. {& m. O) [# P8 h# Q; r
8 P, W H. P1 O- pinMode(_DataPin, OUTPUT);, m; R! R4 E0 S0 i" g: Z! Z
- digitalWrite(_DataPin, HIGH);4 W& ~% A( {( r) i
- 8 l. U, Y! ^+ l' a2 C5 G
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
, ]! A- n8 U$ \8 g* o5 _, A# r; U3 ~ - {" y) g' j9 B3 g$ I( Y+ w( q
- old_data = Sensor_Data[0];, R9 R3 ]' X- e0 `/ `! n7 Z" R
- return Sensor_Data[0];& n+ y. F2 O9 S X$ j8 ~+ u& k
- }
' H' [* i( w$ s* g$ j1 n - else: b! d2 |! s) M7 b3 V/ u7 G l+ d
- {
* P) c% P4 Q1 V, x7 Y' I$ @" G - return old_data;& I9 d: q. D+ t9 Z5 {0 ?4 v
- }( n" R9 \: p5 O' z
- }0 o% R. l, E/ g, I9 @1 `8 q
複製代碼
' A. U& u6 l. `8 H5 Z- v( @MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
8 W0 I+ H! ]: f: p' `+ V9 B. W) }
|
|