|
|
. E, Q! a% T4 M
- #include <Arduino.h>; ^' Y* [3 }6 J' g1 R) t
- #include <MeAuriga.h>( R* m3 L7 e5 z' z, h3 D
- #include "MeLineFollowerArray.h"
5 {6 R4 Q4 {. [! w/ N, ?; y - 9 c" f5 I0 w% f( a
- MeEncoderOnBoard Encoder_1(SLOT1);# Z& s& Z# `7 @. x% F0 F
- MeEncoderOnBoard Encoder_2(SLOT2);( D+ q( a& V- u, E0 w
- MeLightSensor lightsensor_1(12);
1 S$ B! m: H. F) r! N7 S$ ?8 L - MeLightSensor lightsensor_2(11);% _4 }+ l: Z# y& V: Z
- MeBuzzer buzzer;' X% n% b% W& n5 j9 Q
- MeLineFollowerArray linefollower(PORT_6);; j m4 O! u. c$ m
, w: G( V( u: _7 a- #define Error1 1! X* ~' S @2 F( S6 p: h
- #define Error2 2
6 p5 s* p! V" l F - #define Error3 3
; t6 q! Q7 d& f& u
0 b( E+ B+ }& N) X; Y- #define Kp 15
3 n; ?2 |% |* ?- [ - #define Ki 0.15
5 Z: [+ x S. @2 ?+ Y9 f. Y3 u7 y9 O - #define Kd 0.030 r/ r2 T' K( l
3 S; f4 x x, n. V6 m/ ?2 ~- uint8_t sData;
- d+ e5 w, G+ c$ R - uint8_t D1;; x$ x n5 h( Y5 O6 z" I* j( |; L
- uint8_t D2;
7 m+ i% L, e9 [" R7 a4 f+ z6 p - uint8_t D3;; e% k+ z" Q9 B: a1 ?6 ]9 y
- uint8_t D4;
: [# W1 K2 E: n6 E/ Z# t3 B - uint8_t D5;+ C2 P) _! P% W: _
- uint8_t D6;
) {. h6 I3 b7 I8 T
6 b; v; X) ` s: V) g" Q- float previous_error = 0;
7 F! y- h, b4 t- Y" x- [/ m - float integral = 0;; o J7 l0 s2 O' c
- float derivative = 0;
- s2 D; n- \5 w - int Speed = 160;/ B7 S- C# n" v) Z% e& ]
- float output;
7 \6 W. b0 m7 _8 T9 r* Q - g2 s6 m- a3 [: k8 @: k: x
- byte Left;
5 X' j7 r z. O$ Q5 o - + L+ }# i) w) t7 k }$ D' x$ ?2 O
- void setup() 8 N; C, x( o3 N. ^. z* W
- {
1 @4 `; k" [1 E* [+ [ - //Set PWM 8KHz
2 {+ v$ Z* |1 L - TCCR1A = _BV(WGM10);2 D6 e! @/ o( t5 Y1 X
- TCCR1B = _BV(CS11) | _BV(WGM12);: u: O& I% t* X8 e, u
- TCCR2A = _BV(WGM21) | _BV(WGM20);
e$ M6 P0 P3 A$ H - TCCR2B = _BV(CS21);
, u# _6 s8 m( j& U: { - Serial.begin(9600);
7 {" @- d3 J* @; U - buzzer.setpin(45);. x% ~& R+ ^+ g6 O3 E
- }$ G8 K7 Y) K; T/ n! b3 o" l
- t" D2 R2 t1 I- x# n; |7 s- void loop() ^0 y( u- g2 O$ G5 i
- {' \) \: ~7 k( ~. N1 p
- while(!((lightsensor_1.read()) < (10)));% A3 z% M3 E4 u2 x- c' b* X( G
- buzzer.tone(1047, 500);7 D6 P$ Q- `1 T. @' G
- delay(500);
! ^0 V, t" `& N% N - do9 |1 K% G7 o2 ?. V
- {8 H' j0 G* L2 z& w: ^2 N: n
- int Err = getErr();
* J, ?. }! A9 H4 ]' I - if(D1 == 1)4 t L) s+ }4 {) n8 V! e F
- {7 R: t4 `* G8 x( \$ G4 H
- Left = 1;- i) `6 N V P i4 z- H0 T! {
- }
. j. |8 G4 G+ W! M& ] - if(D6 == 1)
* g! |4 T3 X, j6 H$ Y7 m0 u2 g- V/ } - {
2 _$ @4 l2 h4 l2 `& c - Left = 0;- u* q f, L7 F* F9 C
- }
/ p3 Z; M; }3 @ @7 Y. z - if(Err == 99)
) M! k1 U2 @8 d3 R) V: {9 B/ } - {" m0 ^4 r9 N+ D! W
- if(Left == 1)6 j7 H3 j* y* o: Q% ^
- {
/ ]" v: w, B6 d" D" x' S - Speed -= 5;. e3 p" f) W+ @* L
- moto(0,Speed);
+ S8 b! ` h! F0 U; x# ] - do
6 T) h" L2 }7 N. U8 A - {' r5 G9 k) D8 F
- Err = getErr();% ?; z3 ?5 Z) I% r
- }while((D1+D6) == 0);
; J: S, o0 E4 e# N7 r - }
% q8 n1 d6 [# h* z+ M - else: _0 a: y0 m+ L* r8 H
- {/ ?) p& @) E3 M& i0 @
- Speed -= 5;
* n6 b7 _0 H; i1 l" X% ^+ n - moto(Speed,0);
& t P" w# q- I) U* a |+ b+ n) F - do
% v! y8 t3 c0 d - {
, b$ |! U! h8 \0 M$ u6 u7 d! } - Err = getErr();
, j3 o1 |1 v! ]' {5 V- J - }while((D1+D6) == 0);; {4 e0 w9 V9 L# R2 m1 o0 F
- }, H, ~5 V# b# g j
- }
; i) ?4 X- a* |* N5 ^% C5 D3 s k% Z - else+ k5 w, [: T8 {0 o' l K9 ^5 ], P7 E
- {
% b/ F" u; O g8 f. n4 o - if((Speed < 160) & (Err < 2)) Speed+=1;
% R- X, f& Q' J t$ |/ v - if((Speed > 100) & (Err > 2)) Speed-=2; 6 L) m4 V3 Y6 v. A4 `. W% \; E
- integral = integral + Err;
( y/ Y" e; ]( w+ y- D q& c z - derivative = Err - previous_error;/ |8 u# E, F a2 h& [5 h" r8 r' ]# z
- output = Kp*Err + Ki*integral + Kd*derivative;$ Q D% z: ~+ `+ o+ Q: k
- moto(int(Speed-output),int(Speed+output));
1 U0 i) f; p" T( J/ l5 A3 ] - previous_error = Err;
* C7 X1 m+ `/ E; Z4 B2 s - }
1 j4 B0 Q2 Z/ Q5 I - }while(!((lightsensor_2.read()) < (10)));0 E$ j2 C% ?1 M$ @0 s2 y9 M) F
- moto(0,0);
# v& p% f5 ^# d$ x l/ O - delay(500);
' ~* M( V% k, Z( U; Z$ [ - buzzer.tone(262, 500);, {1 n1 \1 h# Y! N* _& |" h
- }
+ D/ M9 |9 ^6 U8 F1 y9 t" m - r1 |- n- b4 J& I3 V+ y
- int getErr()
" T9 A3 @7 h8 D( o8 D W - {
" d( T6 A5 E0 j( R: h' t6 \$ O - sData = linefollower.getValue();. T* `, _1 A! c4 H/ f
- D1 = ~(sData>>0)&1;0 _2 S" x3 F E8 w: n- F/ V4 h% \) d
- D2 = ~(sData>>1)&1;5 Q* ?- L) @2 _9 N' a' h2 |2 ?' X
- D3 = ~(sData>>2)&1;$ p! c; }# y$ l$ ^% @
- D4 = ~(sData>>3)&1;7 t& k6 v) s2 ]/ _" O$ {/ _
- D5 = ~(sData>>4)&1;
- w! R6 ~5 x8 V, Y- R% ~) P - D6 = ~(sData>>5)&1;7 s1 z4 k# q" G6 t' `( f" ~
- int downD = D1+D2+D3+D4+D5+D6;
3 Z f0 I) b+ A. ^; [ v - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);% s5 Z) d8 c! ]6 E; y
- if(downD == 0)
: b8 f4 ^ p n6 J% G- N - {
, J# c3 s- \/ ] - return 99;8 X; E- u8 N; O z6 I4 |
- }! E3 l3 D0 ]1 V. S
- else$ o4 `* u: F6 H5 j/ j% n, D8 k
- {
6 a8 t" Z" j/ y i - return upD/downD;
. e, n% [2 H9 t. a- F - }
4 A8 a, e g7 v% s* R* B& W+ a+ w" ? - }2 C3 L Y8 n6 Y2 p" R, f6 h
# F, @8 f& y1 G Q9 g; `7 n- void moto(int Speed_L,int Speed_R)
5 W9 {' g- h) }# m, n2 K2 m - {
% `7 s" j P0 L9 `) r - Encoder_2.setMotorPwm(Speed_L);
. N) d) r& W) u1 }6 x! ]6 B - Encoder_1.setMotorPwm(-Speed_R);0 u/ p u+ M3 r* k6 h
- }
複製代碼
7 _: c/ W) e* o( EMeLineFollowerArray.cpp; @* @ v4 G4 C m: z8 [+ W
- #include "MeLineFollowerArray.h": p# F6 b2 L: [
2 K. x1 t% |, D c* e* a, a& f0 r- #ifdef ME_PORT_DEFINED5 @7 a j) F# N0 @! ?& p* t; l
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
$ H2 O2 t# n. ]5 v: i) \ - {
* K' {+ m' u$ B/ g9 `$ C1 W4 o# k - 9 j" u- B! v* j% u% e
- }
; H8 B; z0 ?5 s, k M% t - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
$ m# C: i) J3 i( M' h/ H" g% ] - {
5 P J0 a6 L6 ?( u - _DataPin = mePort[port].s2;/ E O1 G \% P
- pinMode(_DataPin, OUTPUT);
$ i1 [" y4 M& ~; ^& e& |' X& } - digitalWrite(_DataPin, HIGH);
- A! s q! _0 X' A2 w$ c - }
+ x- o3 j- J! R0 }# Z9 L% L - #else // ME_PORT_DEFINED
! S# x g6 r6 A! ?6 s# O" \ - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
% [4 l% i2 Y+ }! c& e+ ^+ D/ ~ - {: A# L l0 D9 e2 s3 v; X
- _DataPin = pin;
- ?8 F+ _ f* V* [ - pinMode(_DataPin, OUTPUT); % A5 k# P1 P; ]- Q6 T0 I
- digitalWrite(_DataPin, HIGH);) ^8 `9 H3 W! j1 h. [8 n0 t* Q
- }7 p& |2 c: G% ^* M; k8 }1 |2 v, E z4 J4 t
- #endif // ME_PORT_DEFINED
5 E0 F' L! e/ J4 ^# t - |$ Q( X; e$ J+ `
7 p/ L- ^0 O4 o3 u% a- void MeLineFollowerArray::setpin(uint8_t pin)/ x0 A0 r& T. N% `7 n" C# u! \5 r9 |
- {' l* p( w+ b4 C. M: V
- _DataPin = pin;
0 o9 A! G5 m5 S1 p% P; J - pinMode(_DataPin, OUTPUT); * A; s8 _! Q/ _
- digitalWrite(_DataPin, HIGH);" L/ x# J6 m8 G% K
! W: Y1 L3 t/ _- #ifdef ME_PORT_DEFINED
, K# R' _" [ t8 L* | - s2 = pin;7 s9 ]* m0 L) S; j8 s0 ~" t% u
- #endif
' t3 @' ~6 L. K* ~ - }
" G4 @/ }! f% ?1 ]& s2 @0 L1 N - 9 c0 P6 o. l3 u# j, @
- uint8_t MeLineFollowerArray::getValue()! U6 |" @9 ^1 Q7 Z' O5 ?
- {6 q4 l& w- s3 r; \4 ?( T+ q$ _4 b
- uint32_t LOW_level_read_time;& u: ~0 G2 A. E' x2 C4 }& p$ c
- uint32_t HIGH_level_read_time;
% T1 B7 R' [5 i2 `# W/ M# V5 i7 T - uint32_t time_out_flag;! z1 {. H( F1 u0 W# L! A/ a
- uint8_t Sensor_Data[3];
. _! R/ w6 V. v5 s( q/ j - static uint8_t old_data = 0xff;7 O; j( S' ?& X2 O6 v
- 5 D9 B, |4 m9 }4 G
- pinMode(_DataPin, OUTPUT);3 q' e5 i: N7 Q7 v/ c
- digitalWrite(_DataPin, LOW);! I K% z* D3 ]! U5 S) v% Y' ]
- delayMicroseconds(980);( Q9 z$ u( v2 ]2 m7 i1 }
- digitalWrite(_DataPin, HIGH);6 Q; t/ E# T W, b" g* r3 j& y( F
- ; u$ s- F% [5 o8 u
- pinMode(_DataPin, INPUT_PULLUP);
( S6 X9 z# o. a+ E* P1 u! u - delayMicroseconds(10);+ }4 c! t( f3 H8 e! P' ^$ O/ h0 g/ J/ ~
- 7 S& A9 p3 M1 ]: @: o5 `
- time_out_flag = millis();
7 ^3 `+ N4 Y+ V; J8 X2 f - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 ~1 S6 C4 h% | - 3 ]" y9 Q1 B: g( W) g- X. a
- LOW_level_read_time = micros();
9 N3 S9 _/ d7 ~, x' g6 t! D7 ^ - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
4 J2 \) L7 Z. y: b% R6 c1 P - {8 z3 ?) x: M6 l* m: @4 M7 n9 |1 o
- return 0xff;; |4 R5 T0 x/ y; @/ L
- }6 r8 c6 k4 x, u( r3 u" P
# l+ J w2 t2 u' f! X( f1 v9 Q& }- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 {' B1 y1 c8 p- Q1 E- ^ - ) q4 c. ~& V7 c7 A4 K# K7 q1 i- k
- HIGH_level_read_time = micros();
+ i& H* S5 J1 n Z - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
p. G5 U8 @( _) W3 U
- i3 P/ ?8 [8 _, C$ X- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
. B( ]7 ?/ B/ R - {, e- p" W) \! J4 t6 u
- return 0xff;% S+ Q7 s0 x2 x) A
- }% W& n2 {; ]6 b' d$ b
6 e! g: k7 `4 h4 o2 q" [2 R% m- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))8 N; \: T' j% y2 _& n" B
- {# D1 n- A/ |( t/ G# ` J
- return 0xff;
. H: T0 o: O1 }- d - }
@& U/ N" R! \% O+ [8 c/ t: O - ; ~! v0 j* p- K2 F2 I; V
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
2 d. X# d1 a+ G" X. s2 `' F - LOW_level_read_time = micros();
+ T4 c6 ]% s* G/ r - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
9 B3 @1 Q' R$ \2 i% ^1 _ - , J ]6 W9 ^6 F; B& i/ G
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 c) h: P; j# \ - {
: l3 |. x$ J8 c) i/ V" X - return 0xff;9 G* A9 A N4 ]* A* `5 Y! R$ l
- }
1 O3 l( |2 f ~5 z
{2 z9 Q* _, _9 ]- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)): i' X+ L; A7 a2 a2 _! ?
- {
) W* P% J. I0 { - return 0xff;8 k6 u' z) W: ^
- }; N* g Z: n9 ?2 V5 C8 m* |
- 3 V7 z" v9 l5 `+ _
- for(uint8_t k=0; k<3; k++)
9 G! f) R. F# \1 C - {5 v S0 a; }8 O, F7 K
- Sensor_Data[k] = 0x00;
6 X+ r4 p9 K, {+ U
) e A4 X. z* n- N% C- for(uint8_t i=0;i<8;i++)' s' |7 c$ q1 k) y
- {
2 e ^+ o' N# j9 Z" h8 O - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level( |/ w/ I4 S2 A
- HIGH_level_read_time = micros();
6 @. G' c' W$ m5 Q - LOW_level_read_time = micros() - LOW_level_read_time;9 e( _; ^6 S$ s
- 3 w1 P% N$ N( K0 ?, n Z7 L! q% m6 c
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )/ \8 Z3 ?* ?$ k8 O6 r4 N: m7 ]7 K
- {# t7 h X+ z& S" e
- return 0xff;
+ L0 D! B! p* d" x8 L0 @2 h - }
9 j5 d( j# \/ h) m8 Z2 K - ( F3 o. G& W9 y1 T9 z; b* s
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ ]4 @# P- K; i7 W8 A8 z) d - LOW_level_read_time = micros();
0 o8 ]$ M/ @/ ~/ \1 q( ] - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
6 s! t6 e% ?# |- s9 V
/ F) i( `+ U! U- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1! h7 @ a6 T. `' T/ c1 p9 O K/ R
- {8 l6 s. s! a+ _! A' F5 L
- Sensor_Data[k] |= (0x80 >> i);# c- M! C/ Z$ V, Q5 }
- }
e$ L" t0 T! h- C* J - else if(HIGH_level_read_time >= 100)
: E2 w, _( T+ ?% D - {! r& [7 ~4 e, Y; i; x% X- X
- return 0xff;
: ]: M+ ?& d" ^8 z3 V - }
& J8 v- N$ {- I+ O - / a( b/ Y+ U$ R+ M; T: N
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)- ~/ w \7 ~& z7 A! ~& ?& I# ^2 O
- {* g8 U L0 Z s; ^8 r
- return 0xff;9 V( p7 T+ J! F* I- G$ T
- }+ \2 ]& `& v4 b+ }& c
- }
$ l c! f4 U& r8 C* B4 I - }
1 D1 P% v8 ?/ J9 l( v! H
6 x1 b' H. N$ L( Y5 a- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level$ H2 M, m3 `$ G: A, q/ c: ?; K
- HIGH_level_read_time = micros();
7 Z7 X. o B4 e - LOW_level_read_time = micros() - LOW_level_read_time;
4 V- f6 x: I4 ^0 ~" D" H7 x1 p
m3 A7 o8 O5 m; D9 S3 t- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ), I6 Z" m& p$ P
- {
- `, P" Q p( I: g! o7 y( @; o - return 0xff;
5 k0 ]# j/ _. U" Q0 s3 B - }9 y) u: }! `& o3 L J
- 1 W9 d3 p5 `+ }4 F
- pinMode(_DataPin, OUTPUT);' P: g) W% y- n, e
- digitalWrite(_DataPin, HIGH);# b/ K+ H' P N
( f1 } }1 _$ ]) l$ N* C- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))8 L5 q/ H5 N; R
- {
6 A, l8 i! [5 D - old_data = Sensor_Data[0];
: B+ d3 Z& F1 I1 \; j0 g - return Sensor_Data[0];
w- M# t0 x9 P9 q - }
. @( @4 z/ \' d; A( O) Z - else
" L! K( j: Q/ O9 q* B q - {( ~4 j9 D0 T$ w0 H/ [
- return old_data;' A2 U8 P' c! ?
- }8 V2 M0 G9 d0 @, N
- }% i/ X7 y/ B C- G6 D4 B
複製代碼 * R9 A6 f4 k4 B7 v, O
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
) v Q) z$ i* A0 K t/ k
. j" {& N4 m2 Y1 @7 L |
|