|
|
1 @" y/ U) j1 N' g) s# Y/ n
- #include <Arduino.h>
8 D' c9 \( ~1 s( ]+ g0 ` - #include <MeAuriga.h>
( \4 D! N7 m; s. @ - #include "MeLineFollowerArray.h"
3 w/ [. Y& P. ?9 Z4 o ?$ V' J! o - + v# s8 R8 }0 F) ]2 U
- MeEncoderOnBoard Encoder_1(SLOT1);
* B! D' S: j7 Y! n - MeEncoderOnBoard Encoder_2(SLOT2);
( o' U0 A* ? z9 P - MeLightSensor lightsensor_1(12);
1 K; F) u1 J; H9 k - MeLightSensor lightsensor_2(11);! e: F* \- Q' C
- MeBuzzer buzzer;* r( g# n4 J0 l" j
- MeLineFollowerArray linefollower(PORT_6);
3 n ]. I" T" { y - & K. |' e9 t0 ]7 z
- #define Error1 16 W' T2 H/ x: O% k
- #define Error2 2
' H' d/ Z8 v0 S3 \9 f# L5 S- g - #define Error3 3
" F7 G3 D% Y d; l& T
8 i9 {* p! A6 M6 V- #define Kp 15
T3 w! p; J X( X) N5 K; s6 Y - #define Ki 0.15- [7 g4 h Q5 D5 {( h$ v0 [0 q# c- y
- #define Kd 0.036 Q3 z8 N* q% Z5 l0 G7 `6 T( j0 s9 G4 s
- : K! S7 G. A" T# I
- uint8_t sData;
! p/ Z/ V, a8 Z, ] - uint8_t D1;+ b, K5 P9 k2 L
- uint8_t D2;
# d3 F0 s) o5 l0 D, \$ H1 Y+ V1 k - uint8_t D3;3 s- t' y( w/ r H+ U
- uint8_t D4;
\+ F( ?2 P- u - uint8_t D5;
$ J4 ?% h4 a$ D; w4 N* F3 y- N# Q/ h - uint8_t D6; H; J4 W/ ~% ]9 `- z- T: O
2 c9 Z6 E% n" Z- s P- float previous_error = 0;7 L. M4 N q8 P
- float integral = 0;- A$ ~" ` o- z" m m( V m) [
- float derivative = 0;1 c* B$ b0 ]. ^' W+ }3 ]
- int Speed = 160;5 ^1 r6 o1 a) U0 o2 E( R" ?( o) }
- float output;
( y/ q( ^. c: @0 N6 |5 e
% O$ w$ d" U2 e( s- byte Left;5 M7 r$ e) u0 s
- " N" h- G5 s1 M* a
- void setup() , {& k- q! w* t: S
- {
5 j, g! N* A5 q8 ] - //Set PWM 8KHz4 I/ [6 _+ E: s
- TCCR1A = _BV(WGM10); L' d2 T2 i( B- k0 }; L/ M( I
- TCCR1B = _BV(CS11) | _BV(WGM12);
: `4 u) I& S* `- ^- ?: s% T - TCCR2A = _BV(WGM21) | _BV(WGM20);
! f$ T" Q: \( M9 d - TCCR2B = _BV(CS21);! G$ d) N; q: V
- Serial.begin(9600);! B* u. ?. e5 X6 g
- buzzer.setpin(45);" g+ O/ a6 N8 X0 O
- }
; k$ t$ N7 [4 d7 P, L& i# p- V - ! q8 O8 M, a2 h0 ~9 E& o" t
- void loop()* z% `$ Q; g4 r( c' p# r% v- u" _
- {
) I+ a$ S3 f1 x+ K) U$ a - while(!((lightsensor_1.read()) < (10)));
. t2 [. P w2 W4 y6 n6 S - buzzer.tone(1047, 500);. v: H6 I1 l4 | F0 e7 h5 S
- delay(500);( V4 B/ z: S( x& @
- do
# k5 a3 F4 S* b2 W5 H6 A - {
$ p/ ^0 ~& s0 \ - int Err = getErr();
/ E3 z; \) m e% t: N1 N: i' e - if(D1 == 1)6 {" O4 o* y+ T* P/ y$ a
- {
1 B c G' c- e - Left = 1;
' d& r$ L, U. D/ j2 f. G# p2 F: ?) P" { - }1 p5 i" i3 B/ r) G8 R& N& r
- if(D6 == 1)/ H6 Q2 t4 b* r$ }3 z% j8 G
- {; h' Z" z" L3 E( Y5 x4 D
- Left = 0;8 e$ m) i N+ i3 `# C" i
- }. Z. ^6 x& [5 k8 `# ]! r
- if(Err == 99)
; F& ^% g% m2 G. D8 }' S - {
+ l$ U- t9 e& b% D - if(Left == 1)
/ u+ k6 L$ \0 i6 K - {3 z4 R) d2 H8 p* e4 X9 s w5 N
- Speed -= 5;
- z0 A: }, e/ Z* r, W- |1 f - moto(0,Speed);
( Z, [( m7 s9 h" c, [9 j - do
0 m/ f, x% n: I' m7 ~* ]; o2 I% ^ - {
* S4 H9 M# i* R& ^! d8 l - Err = getErr();& E8 b1 Q" O6 p' G/ d
- }while((D1+D6) == 0);
4 K* t2 _2 h- D# H - }/ V G( w G% C. W4 {
- else# g* i! S4 y+ W6 g! r" t* ~7 I
- {
( t- W: G% j/ A! v - Speed -= 5;
# N* Z8 A1 [, V5 e }( a - moto(Speed,0);
9 B3 S G% E7 V7 N2 d& q# i" C - do2 `1 ?. B- R+ ^
- {
! f- T; `) P1 K) P - Err = getErr();$ r) _4 N; K N8 U6 }/ X/ v6 f
- }while((D1+D6) == 0);. U, q& t) [3 c: M
- }& v" g0 u) \7 S# G
- }
' N# J- q6 E- M8 {6 R - else
' }+ P( G) X2 ^7 {" T- _ - {
9 i) e: a3 p5 |" D" R - if((Speed < 160) & (Err < 2)) Speed+=1;
& U. ~/ _8 H6 F& s/ ~ - if((Speed > 100) & (Err > 2)) Speed-=2; 1 m: d) V/ n4 T/ Q- k; V
- integral = integral + Err;
$ `- f1 A& g: K - derivative = Err - previous_error;
2 g; q8 u, b# ^ {6 e - output = Kp*Err + Ki*integral + Kd*derivative;
9 \# s8 a) v: E5 v& [8 J - moto(int(Speed-output),int(Speed+output));
4 T* ]1 R$ m9 T: l: s* T- X1 `- H - previous_error = Err;
4 n- B5 _! T( ?, A z1 n* g8 g - }& H( g! y4 L7 U" q" x7 }% R. D- u/ _
- }while(!((lightsensor_2.read()) < (10)));2 C, @5 D/ \; ~9 T! u
- moto(0,0);
. {4 n- n+ n3 s# y: a) c' z& \ - delay(500);% v& P5 ^" m( [* N- R" R3 ^1 ^
- buzzer.tone(262, 500);( L# f* |) N) L2 s" i# Y
- }; ?2 }% ^# X c
- + }- {+ `$ h' d& v. ?; t
- int getErr()
* ]1 w+ j: l) M2 \+ R - { 2 ~) f6 D, c+ a7 k* k- Q% {1 [" J% M
- sData = linefollower.getValue();
# u' {6 y1 p# \+ x$ S - D1 = ~(sData>>0)&1;
- E# s8 k5 W& q - D2 = ~(sData>>1)&1;' w. Q& q6 I9 H
- D3 = ~(sData>>2)&1;0 W- d1 V+ j, g+ r% q$ ?
- D4 = ~(sData>>3)&1;1 y/ ^2 H' k& P: b) }* T; ~7 A
- D5 = ~(sData>>4)&1;7 R1 e! e$ N; Q9 \6 ?7 f
- D6 = ~(sData>>5)&1;
9 M2 Y$ N y- x( f$ X) R. L9 q - int downD = D1+D2+D3+D4+D5+D6;% ]5 J3 l* f: H4 b( k& T' w
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);: m/ i- K1 {( c$ h3 J: Z
- if(downD == 0)
0 v0 i' x, @' e1 p2 k& z8 }2 E1 ?) a - {" L! s O& g, Q! a, k
- return 99;
4 _! b0 N3 g1 {+ s6 \4 l - }1 A( A# [& r# G; }( X4 O
- else; G* l' a. {: Y+ f
- {6 @" s5 `" Q3 R3 V+ }
- return upD/downD;: m9 x2 v3 T7 ?! g
- }# \$ h) V0 \5 f1 N" V; j5 A
- }& r' m2 }# A" j3 n
- 4 t; ~4 U' p% T- ]+ b! z
- void moto(int Speed_L,int Speed_R)0 f7 I6 h6 `8 i( E, @) |; W
- {
% i2 m! X o0 X _0 M - Encoder_2.setMotorPwm(Speed_L);! T6 l+ V2 G1 }* _
- Encoder_1.setMotorPwm(-Speed_R);
) A6 k/ }# W: s6 ? Q& [6 | - }
複製代碼 2 g/ b5 E2 z3 I+ Y, Y* D8 ]
MeLineFollowerArray.cpp* F+ R, ~7 _. }& ~# P0 w
- #include "MeLineFollowerArray.h"
# x6 e. w% a; v1 h: E3 [$ l
0 k9 ~4 }9 z. \! x. p. c- #ifdef ME_PORT_DEFINED
9 |& k0 C3 w! t4 Q& }/ X' J3 y - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
# L# V& B. v! I4 p - {& I* P% e4 n# ^% c
- % o5 d2 X+ M% T: d3 u0 c( m
- } c+ U ]% }3 C9 @! l: K, [; d
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)8 ]- t2 u1 C% }% p
- {
! v; V7 r, W' R, Q - _DataPin = mePort[port].s2;
6 S: T; q2 a0 }8 \! Q - pinMode(_DataPin, OUTPUT);
% N" K: s6 J4 M! e( y: |8 p7 e3 B - digitalWrite(_DataPin, HIGH);
0 p: a J5 n2 j0 J& ~3 x - }
8 ^) H1 R; v# o( \: Z) ] - #else // ME_PORT_DEFINED
0 b" Q; a" j j; @' |- N+ v - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
: Q5 r: L" o' p2 ^% M& `" U8 w+ X - {
) n& X4 w7 \' p! [. r3 ^ - _DataPin = pin;
) X* N/ b5 \ z. T - pinMode(_DataPin, OUTPUT); ; x. p, k+ R3 m, I/ l
- digitalWrite(_DataPin, HIGH);
( t2 m8 |/ U* w - }
( j( T. x w2 ^; B* M& D - #endif // ME_PORT_DEFINED
( r9 a4 {# a' H$ N8 d/ S+ R o$ N - + r" Q0 W* V4 i$ `7 M! ?
- . K* }8 r! D3 p, y0 o6 y0 b! J
- void MeLineFollowerArray::setpin(uint8_t pin)
' m# ]; z) ~3 ~% h( v# z- J; ^ - {+ X. C2 T* F$ F+ p8 H
- _DataPin = pin;
0 O& V$ O0 S' Z$ S6 `" V: R% o/ z/ X - pinMode(_DataPin, OUTPUT);
2 x+ L* `# Y7 B - digitalWrite(_DataPin, HIGH);
* w: w+ m# L! Q' W3 z
5 l) |9 Q. g' {; ^- \" K3 l- #ifdef ME_PORT_DEFINED! }' p6 V& H1 E" m/ R
- s2 = pin;" f4 y: g- E2 N$ E) h X
- #endif
, t* b I3 R, X$ U: L: I - }
) K2 M8 u4 f4 J: D7 ?6 ^2 u
) `6 ~, z9 J' k+ j* b- uint8_t MeLineFollowerArray::getValue()
5 ^5 o6 _* A8 Q& ~ - {
/ S6 G# @# ^- O - uint32_t LOW_level_read_time; @0 A4 @( [& w8 w
- uint32_t HIGH_level_read_time;4 V2 Q- [ x% c9 j% V) Q: j' [
- uint32_t time_out_flag;
; n- k' X: s8 V$ G - uint8_t Sensor_Data[3];
' p8 d4 ?: P2 F( ?: `5 a - static uint8_t old_data = 0xff;# v7 f( k5 \6 o7 H6 G% g
# Y7 M1 @ G! M2 h# i+ q% J- pinMode(_DataPin, OUTPUT);
! V$ \7 v9 c0 w$ i# q6 Z - digitalWrite(_DataPin, LOW);
, l2 O, w, ?. q - delayMicroseconds(980);2 u, U/ Q$ |! K% K
- digitalWrite(_DataPin, HIGH);5 n3 v* y2 ^; l/ c9 }, L; Z" ~
- U# F- _0 r% o, E6 I
- pinMode(_DataPin, INPUT_PULLUP);
/ H8 s u% p/ ]8 V4 L2 [ - delayMicroseconds(10);# P$ S1 u7 z9 d( h3 }8 [
' `) W# X4 s e: O t- time_out_flag = millis();2 X( o; b9 m5 t) x
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% g4 f$ F8 R) G" G+ h - ) }! o3 d7 c0 y. M
- LOW_level_read_time = micros();. S% h3 W, s9 n! @0 m
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out% G( d- L& a+ }" O6 P) s1 t
- {
8 C+ E5 v$ [: M - return 0xff;
3 {$ `& k- C2 b" e0 z - }/ s/ t3 p" U* p5 k/ `% k: a" C
' X2 r# t9 P4 \0 C" |& r+ h, _- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 ~% {$ k+ b/ ^/ [' m8 X5 b
$ L8 M" c1 e/ I- A5 X. V( j% X- HIGH_level_read_time = micros();
K) { \: w- J* i$ [% p: g( i - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level9 A$ f# K0 X4 G
- ) @# Z) k9 p: B6 g A' w
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 ^) K/ ]0 |/ A: I: O5 o, k | - {8 o; T. w, v5 `. X; d6 i( J, b
- return 0xff;5 ^# r* M0 v/ [- _8 z
- }7 q5 p( n4 g, B5 c) |8 U' [
7 F) H( {8 d& R9 } W- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
. L3 }" W8 t4 Y - {
! }: g' C7 d( g- e - return 0xff;
% {$ k8 j. t: k- Q8 q2 H - }: H6 I5 M1 D x* R/ G4 T
- ( ~: U) s5 a# J- U& a1 J0 F
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
2 e* t3 d/ d% H; U - LOW_level_read_time = micros();
# e) H3 m6 n( k( R y - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level" o8 R; u- N, Q; ~8 Z( y
- - Q. ~5 s1 s' W: f
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 m9 e4 ~9 n4 B! h8 y# P1 ` - {( }% d! J. Q% O/ A1 K
- return 0xff;
# A# w0 B2 w! B6 }# e - }) {" o9 Q. K6 ^+ J& Q& R
0 F0 n9 g# j( C! }! M- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))! d/ t5 X! I1 a( n! R- D; Y7 }! k
- {
+ L& V8 I% u# d) s/ A3 \/ ] - return 0xff;. {- b+ I2 O4 ~, c1 G
- }' C* U4 S* W1 f; I; N
6 L# C$ _& p @/ D7 D) P- for(uint8_t k=0; k<3; k++)4 S* W, A) {% r( _% `: I3 Z `5 [
- {7 e! X* m/ a% r& L6 Q
- Sensor_Data[k] = 0x00;
! i6 D4 l! w7 z2 `. T/ d - / K) j2 ?4 |9 I9 g) a8 ~
- for(uint8_t i=0;i<8;i++)
1 {$ ]- ]1 t/ @* ?& h7 m0 m0 O1 c - {' ?) ?9 z% P, v. M
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
4 e" W2 c. R( @' P9 s - HIGH_level_read_time = micros();1 B5 `9 |6 `8 h, S
- LOW_level_read_time = micros() - LOW_level_read_time;! B8 O4 Z: q6 ~3 Q0 {- Y m- d
; W7 g. ^! m# i: g5 Y S- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
# e% R2 M4 q1 q* q5 t - {
: Y3 X& Z: W0 j# N( z - return 0xff;1 B. {' c7 G& u" [) F
- }3 J$ S0 t. g; j
' ^0 f( d* i$ S- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! s# w8 C$ t& v7 a" a$ S - LOW_level_read_time = micros();
& k* X# c1 I8 D/ n1 u) u* P - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level* B+ B- U: Y! o; y* v$ {+ N
9 k1 q Y' {2 b* v4 R* k- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
/ Y3 Q$ g* M! B - {- I( Z3 S$ F9 T/ Y7 V. W$ q7 K* \
- Sensor_Data[k] |= (0x80 >> i);% g! M/ c+ J* I" _5 {% q& l
- }
' _4 J/ n( i [. b - else if(HIGH_level_read_time >= 100)
1 h% q2 \! R1 T2 i; c$ Q - {! _& _: G) | p" z7 X7 @9 p7 V
- return 0xff;; R; X2 ~7 P2 q6 b
- }; F6 U3 t) I# |" t, [
- - L+ g4 z- M' U C. ^
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)8 C1 d* h9 v/ f0 x. v( B0 l
- {
% m7 S( ?" j# t2 B - return 0xff;# C4 t9 Q k1 ~/ n
- }
) ^& I% X: T; r0 K+ F - }: V9 X* @5 Q. y
- }5 Q a Y- Y3 Y# t/ R
- 3 ~3 H% S2 |5 ~6 y3 v* I
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level% r9 Y( O' ^( W4 _4 `
- HIGH_level_read_time = micros();& ?+ N; O" p! q- D& Z" y3 `1 @
- LOW_level_read_time = micros() - LOW_level_read_time;
9 P/ b9 e0 F9 M, ?) u
% W6 B0 M. l: o9 b* S- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
' m2 } m4 V7 n - {
* V. R8 {& B# ?8 C' S% ^ - return 0xff;1 {9 h, ` |* e6 }: ]
- }
+ A& [7 e. ^: y; g* z5 T
, f Q+ u, e6 X+ r9 Y- pinMode(_DataPin, OUTPUT);
$ p* ^* q5 e, d2 \7 F - digitalWrite(_DataPin, HIGH);2 c% M2 k% z; e0 G% j, {
9 F7 f- {7 `3 N; Y/ l- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
, U6 X' K8 g" v$ l2 [. R' c - {: B$ _9 l6 v1 i* f( O$ f
- old_data = Sensor_Data[0];$ ]3 ]4 B& s' m& \1 W
- return Sensor_Data[0];. Z" w7 a. P, \
- }- i* v8 k- j" o" Y
- else- J2 }4 q& D# [/ z0 O2 ^* Z
- {$ O4 w3 c1 D8 K+ G6 c2 h
- return old_data;9 W8 Q4 A1 F* s1 K9 s3 J" N3 P
- }
$ |9 w( I' Z3 L7 r - }
# N& r: n! K9 c: G: D
複製代碼
5 A) @3 H* X9 f* A+ |) OMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
" k& V6 H0 u2 Q% R3 R; X- \6 L+ p3 R0 v- z
|
|