|
|
+ Z$ T$ N8 }& V' {) b! H
- #include <Arduino.h>
& K1 V, f/ X3 L - #include <MeAuriga.h>
9 o% P1 C& Q* [ - #include "MeLineFollowerArray.h"* \& k2 V7 T( V! N; E4 K9 n
- . x& O5 J' R# s8 F) a5 k8 p% @
- MeEncoderOnBoard Encoder_1(SLOT1);
6 _6 f: l' |& z; ]# x0 }: t - MeEncoderOnBoard Encoder_2(SLOT2);
# `* {) ^) v% u' J" {, p - MeLightSensor lightsensor_1(12);
1 z' G$ P5 C3 i+ e5 Q; H- O" D) F - MeLightSensor lightsensor_2(11);0 w: u4 F4 m7 n+ W
- MeBuzzer buzzer;) A+ T: g4 x( S* T0 d: p( ~1 v
- MeLineFollowerArray linefollower(PORT_6);5 Z, r) W4 g/ A: A
- 2 K; o* J" V. {- ?' I
- #define Error1 1
: n. Q" k+ W v# t1 i4 p - #define Error2 2
$ k2 w/ u- \8 O& z" f5 V. X - #define Error3 3+ ?* m) R$ m- n; U
- - P( R1 W/ `/ T c4 F/ ]
- #define Kp 15: |) o% l" d# v* d
- #define Ki 0.15( ?3 }, F4 A5 `
- #define Kd 0.03
; w) o7 f/ l( o. n P
* G% H$ x: j! x, {: w/ S8 m0 ^4 l( `- uint8_t sData;' Z% K& x7 O! }' K
- uint8_t D1;
8 W3 _: v$ \ P5 [: ^ - uint8_t D2;
6 d7 L; e W) S+ R: ] - uint8_t D3;
6 c" { c9 }5 j% a$ D, ?" { - uint8_t D4;
$ ?3 t" ]! ~$ H9 I. { ^ - uint8_t D5;& p d, R+ P o( a
- uint8_t D6;. q: ]! ^6 h& a, m7 O7 h2 u5 Q
- : d0 Y5 u5 D" f7 W7 l0 U7 N
- float previous_error = 0;
P/ I1 u1 q4 M: T - float integral = 0;
( v6 k% l0 [6 u2 _8 ^- D4 Q - float derivative = 0;5 x6 L/ k4 C# L9 Y
- int Speed = 160;
+ ]# M' z: R5 h5 s' G' t/ ?$ t. Q - float output;
7 h4 H& ~4 \# o0 N+ z. o! A8 q
' g# r: I7 i9 |/ b- byte Left;
1 K) W5 m7 l5 h7 ^4 H8 ~: u - 6 B5 q4 F+ x6 r& r C/ w
- void setup()
* _2 ?+ K8 _: X3 E" H - {
3 o+ T8 S( e" [$ S+ U: }1 s - //Set PWM 8KHz
# B' C3 s* u1 s. ?$ p- Y* Y; ?! Y$ S7 A - TCCR1A = _BV(WGM10);9 ]* i g( O7 i2 d8 d* }
- TCCR1B = _BV(CS11) | _BV(WGM12);6 K* Y: W1 S; B; r: V
- TCCR2A = _BV(WGM21) | _BV(WGM20);
* M9 U6 E' i% b/ O9 w0 } - TCCR2B = _BV(CS21);
, ], p. r. j, u0 j$ Z9 b - Serial.begin(9600);
$ O- V6 }, w4 E; W0 [/ I1 }' O - buzzer.setpin(45);
7 P- K' }8 E7 g0 t$ I" y0 x- K- j4 } - }
+ J. N7 O# W( b% l9 a1 J - $ A' Q8 ^# o$ |9 D$ o1 P' i+ x
- void loop()
& w8 d. D) H" K% W/ G - {$ h8 C. H6 F: j9 d/ z
- while(!((lightsensor_1.read()) < (10)));% Y) Q8 w' U& I2 b2 ]# E* C
- buzzer.tone(1047, 500);
2 X, a0 }7 E6 w' K, K - delay(500);* l7 I e( d, G3 C0 x
- do/ e2 M3 ^* a) i# x
- {0 u9 ^+ A2 m8 t2 v3 T3 ]
- int Err = getErr();
: g6 R0 f9 I% w* K - if(D1 == 1) q* \# w8 Z) }! h
- {
$ ?0 I6 z5 m2 F4 _0 B( k - Left = 1;
) W$ a7 z' |- z - }6 F7 C- q* n5 C5 _ L. Z! R$ a1 J
- if(D6 == 1)4 T- Y/ _/ V0 ^) ]
- {/ a% V# y) _/ }' S7 A( V" V, F
- Left = 0;2 h9 ?6 P4 P t- b2 u
- }+ R1 m; o! Q$ g
- if(Err == 99)
( R, B8 }, _, b! W' c - {
) U7 F4 n/ V3 o, I: ?9 | - if(Left == 1), ^1 y2 O$ `# z3 c6 s# Q" O9 N" m
- {
4 j M5 z: @5 |( g1 N' a - Speed -= 5;- ]7 S! ?) q% N/ e+ ~ ?
- moto(0,Speed);) F& ~0 y! Q4 y! q4 j9 J
- do4 M6 |. |4 W# [/ U$ Z5 l% [" R
- {
% L: z8 q- u8 |9 U - Err = getErr();- T, O( |7 e2 u' j+ p1 T
- }while((D1+D6) == 0);& Y# f/ f3 q' `' O) r4 `
- }# W7 p6 K9 t+ p$ y; c0 q
- else
, g; R' X7 t6 N4 m- r( J - {( y' u2 z* d6 @% s
- Speed -= 5;
% h, D& I, S: r8 L( _5 |+ r - moto(Speed,0);
% A! a; R" G7 z% ~1 T - do& V) s% B9 @' G. e0 h! _
- {
1 `* O! A! Q8 N. K2 ~ - Err = getErr();* q1 t$ O M" S- k2 }# D- C
- }while((D1+D6) == 0);
! s o# L0 c5 k4 V7 S - }& ^( j- U8 k3 Y& a
- }1 e1 G$ H9 d8 O9 q: @& X
- else; F3 w1 p9 h) W+ e7 a& v
- {
' \1 A* U2 B, y, g' G - if((Speed < 160) & (Err < 2)) Speed+=1; ; u# Q( D2 d* ]# ] J+ [3 T9 ?
- if((Speed > 100) & (Err > 2)) Speed-=2; * \6 y& z" d9 W' q
- integral = integral + Err;% C" d" V( T, T
- derivative = Err - previous_error;' t- C/ q4 X$ @2 G/ R* W
- output = Kp*Err + Ki*integral + Kd*derivative;
# |) r& n7 ^' m, m( v" T( i - moto(int(Speed-output),int(Speed+output));7 y2 i4 `2 w: M* ~1 R( F7 [
- previous_error = Err;! |9 z- r. P5 t$ U0 r
- }4 ^3 X% \/ ^4 p4 c% D+ x; e
- }while(!((lightsensor_2.read()) < (10)));/ I# T. g! ?* c6 N. B. M2 B
- moto(0,0);, A2 t. ` t+ r5 U
- delay(500);
8 M$ ^7 H7 B2 E$ A$ V, [ {0 P - buzzer.tone(262, 500);
/ `" E4 x/ @: H9 \$ {* G - }, ]) L' M5 ^' }5 t& L
0 p" y; L7 u/ F/ Z- int getErr() k: {: [1 F, {- m& ?
- {
2 i! P# \' C* w _ w - sData = linefollower.getValue();
& F. o& |/ J7 Z6 H% Y - D1 = ~(sData>>0)&1;! O) \; y; P5 k& Y% a0 s& W. L
- D2 = ~(sData>>1)&1;
" v S* I5 {( x - D3 = ~(sData>>2)&1;
% [/ Y9 F8 c) p+ e! P - D4 = ~(sData>>3)&1;4 c: l' M* }! ` u! S9 v% m
- D5 = ~(sData>>4)&1;
# ?" `: H. W1 [ - D6 = ~(sData>>5)&1;/ x, v! x6 i' j# w$ r; m4 i1 Z
- int downD = D1+D2+D3+D4+D5+D6;
0 d9 X8 ]- k; h2 X4 e - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);1 _4 b4 c5 N: I
- if(downD == 0)' y) e5 h) U% ]5 I
- {1 f% M9 ?% y$ _" S, a+ l
- return 99;/ F9 i9 P6 B$ g5 [ Q* b- Z
- }" Z6 X+ ~2 y& o1 E+ w& i
- else3 V- m q h* \) ~
- {7 [: e: U& A6 ~
- return upD/downD;; X% m. T0 p, L9 r4 S
- }5 J# k0 ]& X9 v* l
- }' W I. Q+ |4 z& {! W
- 5 c4 O, O4 C# y) O
- void moto(int Speed_L,int Speed_R)4 T, W9 B; ` a( X
- {
, `5 d$ s% ^! C* {" i& D - Encoder_2.setMotorPwm(Speed_L);
" t* H) n3 k U+ x' c* d0 } - Encoder_1.setMotorPwm(-Speed_R);" A" U5 p4 ^( c c \
- }
複製代碼
. c+ ~) a! t) b k$ TMeLineFollowerArray.cpp" U8 {7 C* U! \6 y2 C* M2 {* D5 Z ~0 l! X* ?
- #include "MeLineFollowerArray.h"
! O: _, Q! m- r$ ` T
4 q1 w3 ]6 v3 t- p: X& k1 m1 t" f- #ifdef ME_PORT_DEFINED
3 h+ p. R/ d- M - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)- p& V% o- f/ p5 g
- {
3 b1 M3 O5 x$ Q( X0 C/ Z& x
8 [0 Q! ~/ ^8 w$ t& y2 Y F6 X- }
5 y9 I$ Z6 q) s9 X; Q% [' O - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port). L7 ?$ T* b1 U2 k$ F" p
- {
3 u/ u: R" X! p- K! S# C# L - _DataPin = mePort[port].s2;
' \8 ~' }+ p G' F, {8 F6 U - pinMode(_DataPin, OUTPUT); . D- E x' w! C; F2 A% `( E G
- digitalWrite(_DataPin, HIGH);
& a! |8 }! M- ~ - }3 M& j7 A# ~2 I6 |) L% B/ G
- #else // ME_PORT_DEFINED$ h o9 o: ] W* l6 o- X4 Q
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)1 i/ X" y2 Z1 y5 F( n1 B# P
- {
1 n- s/ I" `$ b" \* R - _DataPin = pin;/ t4 C: O4 ?8 o4 X% |$ k i" _) y5 w
- pinMode(_DataPin, OUTPUT); 9 d* d3 b: L) r
- digitalWrite(_DataPin, HIGH);! N; y3 q5 D8 B$ O1 f& L8 y
- }3 L2 e. M/ ?, ~$ @& \
- #endif // ME_PORT_DEFINED
# w. {9 E& R& S# Q( a5 l @' [ - / E' S+ u2 Q& X& N @5 F" O
$ [9 q6 I9 Z+ c! ?. L n& q- void MeLineFollowerArray::setpin(uint8_t pin)
6 G: Z1 y' z3 i- q - {
, Q0 m8 D, w% A - _DataPin = pin;
; U) N! k; e9 V- C, ~4 S0 B - pinMode(_DataPin, OUTPUT);
2 q: n; ?! E) X R; }+ d - digitalWrite(_DataPin, HIGH);; E$ `. t/ T: V$ f' e
- 4 { ^/ f5 T8 k+ y. F! U d
- #ifdef ME_PORT_DEFINED7 `* P4 ]) O( P# q6 s
- s2 = pin;
/ Y. S" ?% i% U" B3 C9 e - #endif
: v" n$ W }7 _+ {+ ~/ [' j - }+ L$ H: e4 [ f2 Z% l8 Q3 _
- ! I* e, I/ f+ K
- uint8_t MeLineFollowerArray::getValue()
+ G$ } Q! x# m+ ^ - {
' c. d7 [# V- a0 M" m - uint32_t LOW_level_read_time;, V7 e! y8 u8 T7 d% l- T
- uint32_t HIGH_level_read_time;. G1 J% x1 ~! P: M+ X
- uint32_t time_out_flag;
: k% N6 D. l* _6 ~0 ^ - uint8_t Sensor_Data[3];' n, ]4 z. n; ^! T# ~% _
- static uint8_t old_data = 0xff;
, M* s( h: ^4 ]/ K: `3 U
( ?0 ?1 j' Z) D2 H& N' M& c4 ^- pinMode(_DataPin, OUTPUT);
: z2 r; |9 g8 x* v5 L' A, l - digitalWrite(_DataPin, LOW);8 j# I5 B. r& U5 Q! |
- delayMicroseconds(980);7 e( y& R7 w+ A# g v
- digitalWrite(_DataPin, HIGH);" X9 t$ V1 Y0 \- z; z
- ; J" b" [) ^& n" [3 J" m3 x
- pinMode(_DataPin, INPUT_PULLUP);
) y/ Q; k6 U6 s; B* m - delayMicroseconds(10);6 Q% k- D0 p! Y4 t6 L Z
3 |* y) X7 ^# G. u F- time_out_flag = millis();
% _* J, Q( C% E# R9 z0 \3 l& W - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 F- Y8 P$ f n' a7 w - 9 ?* D8 v& Q& a8 v. s6 @
- LOW_level_read_time = micros();
8 A( _" U# ^3 d7 \2 E. f - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out8 ]0 P: h1 R8 \" y+ a# h! c7 H
- {
$ V/ `# A; P3 X' k p( o - return 0xff;, y! ?- Y2 m% {' }( t! p: x z5 x+ @
- }8 N% {& Y% Q- N! k/ ?$ }9 j4 b! ^- z: i
5 j+ C8 G4 F8 M0 i$ s- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 q2 |/ C y7 @: b - : v% f9 t+ I; i# y* u) P l: T
- HIGH_level_read_time = micros();
: @. ?/ \+ J E) A# S9 j4 ^) z - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level# ] l+ Y; `4 m8 w+ Z/ e% h
8 n$ x% \6 ?+ Z6 `, s' e- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* E$ y. D$ \. [- O
- {
9 E6 U5 u8 Z" D' x# y4 D$ T - return 0xff;
. w% _6 T2 |6 i* U: q - }
4 o! O- k- U8 o5 x- ]6 j - * a; Z- e: P. Y# u$ o
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))/ L, {) f; D$ \" {- t9 Y- b
- {
; B" w9 g2 ^0 E6 }- } - return 0xff;+ w% P- e; E& u, p R8 z
- }1 |' y \% j1 S# G& {- O7 [7 S( a
e# V" ]' r, C* w+ J I- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 J2 z" s3 i+ N2 h& q - LOW_level_read_time = micros();4 t* k1 ]: G" @# v. h' G4 t1 ?
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level% j" d( g2 \, r) S
* e. q9 ]) @3 n! g$ X1 f( Y2 X+ _8 }- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out: L/ Q% C- ]% I* ]- a
- {
. J0 B0 N' a5 o) ?: g0 h - return 0xff;
& v% `9 b) X5 c5 t - }
) @& k9 n; w7 V0 V - % k# |. Q; T" N$ O
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)): J& V4 z- _2 p: H' k% o
- {
& j4 G$ R4 ]& a - return 0xff;
# ?- I- R6 z4 t9 l- B& u' Z/ t - }( `- @, i- |+ T7 T0 h& F2 D3 M$ t' w& j
- 5 ?. i6 ^% K% I" e
- for(uint8_t k=0; k<3; k++)
* y/ @/ l. o$ V" d' M$ ~ - {
+ O$ a0 y6 j" i2 ]1 `5 j) s1 d# c! q2 D$ E - Sensor_Data[k] = 0x00;* r* F# r! {9 H/ o' U
- " H1 o& R$ Y& d
- for(uint8_t i=0;i<8;i++)
/ O3 t0 i+ j& r8 v9 X - {
2 U! _$ ^* {9 j7 m5 } - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
8 p) r1 N, n& d1 Z6 F - HIGH_level_read_time = micros();
3 k0 a* v( U& @% R4 C- z6 ~2 q - LOW_level_read_time = micros() - LOW_level_read_time;7 c# |2 B: C( i1 q
4 @- |, b: j, A5 C- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
; A }$ c7 h$ r- V6 N - {. o( I4 J6 w9 F. \2 e
- return 0xff;
# c) ~. a3 V* } - }! c# x3 p: U6 h4 J8 u# W. @
) l e! r. E0 I/ j- H. J/ E5 C- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: K: Y; T# M! a( G: l, u1 b - LOW_level_read_time = micros();, w3 e! d6 E. k+ l p1 I
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
* Z0 I9 f5 a n
+ t" o$ V3 _ I6 {- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
; G' h3 N5 @: a# e! {0 ] - {
# N: a7 d$ e/ O) q) l+ | - Sensor_Data[k] |= (0x80 >> i);
7 X) y2 |9 ^. N6 z7 ^7 ~9 k, g - }, v* U* g7 w: z+ w* G
- else if(HIGH_level_read_time >= 100)/ o6 \# d) L2 T( B/ `& T
- {
y6 P2 d7 C2 L! w& ]( J7 z9 \ - return 0xff;8 G: y7 F3 r" o1 Q" f9 `# V: O* p
- }
$ f3 ?( S; I! V! o
0 B4 K- K# q0 W* z) J& I" z4 X' B8 C- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
' l2 X: R4 |1 S" z) t% ]' H5 ~. F - {' ?# x4 w$ I0 b. z
- return 0xff; L( F5 C; D& @
- }7 v$ j( V) {$ N* y2 K8 J
- }# B1 a+ m9 z+ g
- }8 f: a4 F) P b& D# R1 o& a
- / P& b: J9 e. L+ h# h
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
8 P8 k- Z' j- U - HIGH_level_read_time = micros();
6 R% ]; x; i2 j4 a - LOW_level_read_time = micros() - LOW_level_read_time;1 i( r4 v$ b' q" ]
- " R& W& i6 u' T3 `
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )% u$ W1 I0 h6 T7 J
- {" F+ @2 }1 \# V, ?7 f/ r
- return 0xff;
* o1 a; ]& }, [ [ - }
6 Q; D( h. K+ m$ Q# h( i
5 J j, N1 {& c2 u- K- pinMode(_DataPin, OUTPUT);' K a3 V! k* k3 c
- digitalWrite(_DataPin, HIGH);
7 h4 p- P T0 i2 g, k - : b4 {! h" j% l: ^) G" d# ]
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
0 [5 O/ P% z! ^, L/ f2 j - {
5 W c! r/ G) F# M! t - old_data = Sensor_Data[0];: c" {2 F# n3 A- S+ r6 t! U( }
- return Sensor_Data[0];
7 X- P8 v- k q, g( Y - }
! O6 z4 ] u1 f - else
) k3 _# f, I ^7 g1 y3 { - {% v% } ^* w5 v, K6 T1 I9 {
- return old_data;" E4 C& m. M1 _) `! E# f
- }
% u" B; m) [5 A" i. _, v( H2 j/ | - }
4 A- p" W, I& p4 s$ w+ |! z
複製代碼 / n/ [% A( W8 o! q0 V* K
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
" ]! |. V3 d$ g2 h
& \" P0 {/ \( g% U5 I |
|