|
|
7 L. l( S, j3 i. Q8 D( h: C+ G, o- #include <Arduino.h># O" U' n' h4 V9 X
- #include <MeAuriga.h>9 v* g7 \/ H# Y, B6 N. Y
- #include "MeLineFollowerArray.h"$ S& s; R2 U/ x3 t5 X
1 t) Z" g( Y) Y- MeEncoderOnBoard Encoder_1(SLOT1);* L9 a' m- V% M; y7 r, t- R2 _% ]
- MeEncoderOnBoard Encoder_2(SLOT2);
* v3 U {6 X. E2 k - MeLightSensor lightsensor_1(12);6 T# B4 E6 z3 F0 z( r
- MeLightSensor lightsensor_2(11);
. M g& H8 Z2 e* `3 p8 g5 I - MeBuzzer buzzer;
, x" p3 h2 f6 u/ N - MeLineFollowerArray linefollower(PORT_6);# \+ E8 `& m9 j, |( `
- 2 ]$ p1 Z+ r1 E' C0 [; x) |
- #define Error1 1$ Y$ p- S* F: A% d
- #define Error2 2+ Z) K; u* a" l: E+ {) Y% j4 q. O
- #define Error3 30 P& ^# u' ?- X" G; E
- 9 m; _. k. e/ _6 U3 s
- #define Kp 15
: G1 O( N& [/ U: A4 k- {4 L) D - #define Ki 0.15. }+ _" r7 r$ i, e# i$ K
- #define Kd 0.030 p6 w. a0 r2 c& }' N
/ o1 `! q/ m, U8 p/ \* j- uint8_t sData;! K4 @( E$ V |. `
- uint8_t D1;
! [5 t0 j' ]9 {# i - uint8_t D2;7 w5 L/ b8 Y! L
- uint8_t D3;( `4 p8 P9 m* k* I
- uint8_t D4;
( ]- U; h& R; b6 r - uint8_t D5;
1 U# ^/ m4 l$ h; a5 x6 p - uint8_t D6;4 a' O' A+ E2 N
+ H/ B3 {5 V1 j6 y- float previous_error = 0;
. `. W/ N' p3 u, k7 A/ V - float integral = 0;
5 [# X# Y3 C: R8 y( S - float derivative = 0;
" ^; V$ O! C2 n) x - int Speed = 160;. p) |9 F$ V/ j0 H' }* y
- float output;3 A( M# Z, k" C! h' }( k, [, {
- 4 c; m& w# ]3 \$ q& o
- byte Left;+ Y' W1 j- H, H' E" v# Z6 w2 c
- ( }! [, A' |! s% t5 e
- void setup() ! T2 E5 K6 _1 n6 J4 j# ?/ D0 G: F
- {$ M# {7 {; ~. I! E K
- //Set PWM 8KHz! R& r1 k% m- o. r" ~3 h
- TCCR1A = _BV(WGM10);) c6 }1 {+ p2 X. _9 W0 O, {2 j
- TCCR1B = _BV(CS11) | _BV(WGM12);
$ X5 F0 y& ?: s% m - TCCR2A = _BV(WGM21) | _BV(WGM20);% r3 a- C" w3 B! t4 E
- TCCR2B = _BV(CS21);4 R) k: `, a/ Y
- Serial.begin(9600);$ l1 e- C; C1 l, M# C; e4 k
- buzzer.setpin(45);
3 A' n# r) [3 b" \; o) X - }8 l- b3 l: F- u$ l _' u
. B) k. k+ W9 x+ ?! g2 P) v' R- void loop(), v$ ] K" }5 ~( I3 Q- B6 q1 s
- {
4 E" ~; R& T( x7 X, f. Z - while(!((lightsensor_1.read()) < (10)));
* @9 T5 N0 j8 d% Q- H% R* C - buzzer.tone(1047, 500);" J$ A4 F' d) c2 q' T) P3 m" g
- delay(500);0 u o7 J9 @7 b1 ~3 G
- do9 i1 N9 w2 N" F) c% J3 l- j l5 }
- {) [6 \, [( h6 ?% V, g
- int Err = getErr();
3 T1 @3 q! W% O: `( X' t - if(D1 == 1)7 u* ^. W. j) t9 ^
- {' t) m: l9 ]* ]' b1 B; b
- Left = 1;6 J/ h- T: ] u) P- R$ \
- }
. B H# I- G' x0 F - if(D6 == 1)
- S, J5 ^7 u# W: ~ - {7 @/ d9 P( ` i% ^
- Left = 0;* ^3 e( _+ x+ v8 P) D; I
- }' K: r9 h. {, D8 G5 ^' ]) s
- if(Err == 99)
( n G3 c! U5 Z8 P; b9 } - {
8 b d. ?6 H4 W. q. ? - if(Left == 1)
2 n0 E6 _# g' i! M3 ]6 f0 x2 E - {1 f( \0 M0 z# l7 W! v# m3 z
- Speed -= 5;
- \( {, u' w0 p X5 o: S6 U - moto(0,Speed);
* ?$ q4 U9 U4 i8 |( E. f ^ - do
4 e1 m! @/ P* o Z, g: g" z2 j% I - {$ g% x) X7 N' F
- Err = getErr();$ L/ }5 W- E6 Z0 ], A
- }while((D1+D6) == 0);
7 b! q7 E+ M- ]6 B - }
+ u" i/ N7 d @/ F8 } - else
. \0 H7 K- a" N( p6 }" v* _ - {$ p4 ~1 D* p* X5 b" j
- Speed -= 5;% Y9 B$ ]# n$ B5 a1 y# [
- moto(Speed,0); 0 C' \+ W2 }: ^4 G
- do
5 P$ e' B$ l# V& ~ - {# t' ^# X( l6 x! ?
- Err = getErr();% u4 Z+ ^/ \! @- z6 \. G6 ^
- }while((D1+D6) == 0);
/ ^4 a# X* Q( ]& Z$ h - }& l# I* l! T+ X1 [% d
- }" H/ B8 A, Z% p7 f
- else
& D' a* L3 E n: R, Y% c6 j. \9 B - {
( t: ?, {. U- U, ^ - if((Speed < 160) & (Err < 2)) Speed+=1;
2 ~) w l1 h i - if((Speed > 100) & (Err > 2)) Speed-=2;
* X2 C0 b5 }1 S, c; } - integral = integral + Err;- e0 R0 ^; u# s8 w) z' }4 l
- derivative = Err - previous_error;
+ _, c1 h. s: a0 ^8 t - output = Kp*Err + Ki*integral + Kd*derivative;
; M E0 F* r6 P) \( J" Y+ l - moto(int(Speed-output),int(Speed+output));
3 O# Z7 ^; ]+ z8 a: L% Z- u1 n+ t - previous_error = Err;) Y, o! \& M3 @6 Z! J, X e* x
- }: l" ?2 Q2 P# r3 _/ ]( [
- }while(!((lightsensor_2.read()) < (10)));
) ~( m- E+ S' P) y - moto(0,0);" m* ?- n! R1 L n& f! k* ^1 x5 Z
- delay(500);# h( _& r- v# _1 D2 V0 y
- buzzer.tone(262, 500);1 E- Q |, N6 H2 h1 R1 ?! x* P
- }8 a x# n# T; @5 V2 Y# J- g; U6 A
- e- v+ S2 ~6 u+ I& i
- int getErr()! S2 o1 ^2 n+ Q5 j& X. o+ s
- {
( ~) M) p1 n# P4 w7 j% } - sData = linefollower.getValue();
$ v& X0 i/ f' Q, q - D1 = ~(sData>>0)&1;6 L k- |+ J6 B1 X$ y g
- D2 = ~(sData>>1)&1;
. `, X' u1 A' Q3 S - D3 = ~(sData>>2)&1;* \! K# o; x1 y# R$ u1 p
- D4 = ~(sData>>3)&1;
7 I, g: c2 B/ M( `: n! Y6 d; l) y - D5 = ~(sData>>4)&1;
% N6 o" P" Q9 d8 I - D6 = ~(sData>>5)&1;; c/ i8 d- F* h5 \- R. `
- int downD = D1+D2+D3+D4+D5+D6;
' ? @0 G1 y# r; Y% O1 | - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
( D3 a" F- l% { - if(downD == 0); C# I, k5 |" o; V9 d4 x3 {
- {
, `& k' }8 L0 ^ Y! s3 @ - return 99;5 B9 r$ R V3 d! ^& S8 ~# X
- }8 R: Y8 b( R! y; S6 u
- else
; N1 t5 k b! M: V4 X& Y - {
( u5 A+ M/ U* C$ C' Y9 A6 f" k - return upD/downD;
! d# b5 V0 G5 b! u- a/ a - }
& ~, T) W6 w5 e) D# P# L - }+ k3 W7 S, I, u9 I+ P
- 1 W4 k; p, C3 o7 s2 I) I2 O3 L
- void moto(int Speed_L,int Speed_R)7 }0 W+ l5 {2 T; w8 w
- {
1 D" g" O0 w3 s9 J) @ - Encoder_2.setMotorPwm(Speed_L);
. v, ]: w' P9 ]' ?2 W, _, Q" u - Encoder_1.setMotorPwm(-Speed_R);! g( C, n' K0 S9 m# T0 H, c p3 I
- }
複製代碼
. s" g/ `# W4 V$ j/ [- nMeLineFollowerArray.cpp$ P8 |% k1 J. V0 p/ \7 G' a3 I: k
- #include "MeLineFollowerArray.h"
9 C+ r' {$ P7 _ - & B7 g! T* Z: Q& i
- #ifdef ME_PORT_DEFINED% H. M" V) `# Z6 r2 u
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
2 \8 w, z: s$ \; _ - {+ x7 f1 x: W$ U
8 H- `3 o% b0 ?8 _! I- }- Z! z* d4 ` e+ _$ T% [% D
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)7 T" D. g3 s+ {) b" {: B
- {
) Z e/ |" `4 Z C3 d) T# Q - _DataPin = mePort[port].s2;2 W$ E) E5 g4 N" O! v! i
- pinMode(_DataPin, OUTPUT); % w7 V g: q% O c( ^) |
- digitalWrite(_DataPin, HIGH);
) I. f( q% N. ^: Q - }
* o7 v: W6 o5 S( S5 w8 ]% h6 X - #else // ME_PORT_DEFINED
6 g* n2 p9 e% K8 D: ~8 t - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)' k7 i( b7 s0 Q$ `$ G u
- {' K$ h8 Q0 F! L; K% z
- _DataPin = pin;
8 u6 M0 r, z( E0 i - pinMode(_DataPin, OUTPUT);
/ e1 H w+ S. T7 r* w) E; V: | - digitalWrite(_DataPin, HIGH);
, \# B& J! m* R5 M1 H" C - }/ Y$ u2 c R. H! {6 P* @; b# F+ x
- #endif // ME_PORT_DEFINED$ i- \; P2 {8 I3 C
) F j2 m: R) Y) }* m6 J- w# F
. T4 \* X/ p& [2 Y! ~! [- void MeLineFollowerArray::setpin(uint8_t pin)
( S6 [7 {6 L; e - {: S# v* c# q/ e1 P, c& X, [
- _DataPin = pin;
) Y: n2 N x5 E0 L1 E8 q- ^ - pinMode(_DataPin, OUTPUT); ( v6 z5 d% o5 X/ R4 e% S. L2 c& e
- digitalWrite(_DataPin, HIGH);$ a* u$ s- T4 M. z& Q3 J
- 8 D+ E* D/ e1 J, }, @; K
- #ifdef ME_PORT_DEFINED
$ K) D% @7 M9 R8 k- o - s2 = pin;
& l) T7 @2 Y2 ]; | - #endif
5 g+ r: ^( j( N) M: j) } - }
, {% z" d5 F. g* [# U
8 p# B8 x; W& A+ i" {6 C% T$ _- uint8_t MeLineFollowerArray::getValue()
1 N$ l- v9 O: f! d( K - {" K# y, M2 A* ]/ j; e4 o
- uint32_t LOW_level_read_time;
: H6 e% W6 Y6 ~* i - uint32_t HIGH_level_read_time;
C* F) O' H* Q) d, {2 p - uint32_t time_out_flag;5 O- A- d! i& i- ?5 O; ^# I
- uint8_t Sensor_Data[3];
# ^/ `4 w, R1 B; k - static uint8_t old_data = 0xff;
; I7 p& x' A& v7 X7 V - 6 N9 S+ [; Z! s7 S: I0 `6 |
- pinMode(_DataPin, OUTPUT);- b; ^" c+ [5 t# n( [/ `% j
- digitalWrite(_DataPin, LOW);
) {2 j) O: [( q; ] - delayMicroseconds(980);: l1 ~3 d* `/ A3 ^
- digitalWrite(_DataPin, HIGH);/ E" q2 ~* e) r j7 g! v) g
* M5 i3 Q* O% i- pinMode(_DataPin, INPUT_PULLUP);
" v3 V% Q" }/ d. G - delayMicroseconds(10);
8 M7 A( @0 C: L2 \1 G - # ]. u. c, T. ]! t- y- z" g2 O
- time_out_flag = millis();
/ ?$ Y! {9 G( Q - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );3 `3 B: R8 l; s1 ?" I
3 B5 J l% Y1 m6 p$ U, K1 f- LOW_level_read_time = micros();+ ?8 `, z( Z* t2 a
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 O- L' e/ n( d7 E! [. A! a
- {/ X% w. ]: H* u4 [5 _
- return 0xff;
2 a8 z+ f/ X0 P7 y; p. |! j - }
3 M. w# V$ ]% x& c
) j9 G5 `: C1 ^- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ n: t( t5 J" x5 e
1 f; [5 v3 T2 \! l1 N l- HIGH_level_read_time = micros();2 ]4 O8 P, ~2 @% c/ f. o
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
" J" T& K4 K' U! C% M! F1 Q
5 Q5 ]+ W# X. A- c2 J$ m T- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out# E+ {6 T& G2 q4 x- @
- {
: t2 `& {. W4 K! |: x' r; Z - return 0xff;
1 ]8 R: ^/ q- {4 a7 ~ - }' L2 b) [% `9 e2 R
" t; z5 l+ `4 D& b( g* p, Z- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
0 `, ^0 ]6 A' V# ?+ |9 E! t+ B - {
! N$ D+ {) y$ [4 v. Y - return 0xff;
2 V3 e8 X& J, Q/ I9 ? - }7 |; w3 L: H4 D+ M+ G
2 v8 j$ ?3 A" F* z) p- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 p: W4 H+ A! w: a; Q# ~
- LOW_level_read_time = micros();
; J4 K9 |+ ?) k. h# \6 W8 y - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level! q" V% N5 E1 W! N! S# H5 }8 G
- $ C8 r6 a# R6 L0 k* n
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out9 o k# C/ S5 _3 a. n0 ~6 k
- {, C6 O. A) w" T1 f4 n+ R, i
- return 0xff;# u- ?: G0 }# E
- }
, c }2 ?$ d* X7 M - 8 M- Q2 L$ x* d# M
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
; u2 [$ f( \% F E8 M - {
, a$ w' ^! v$ A/ b - return 0xff;
! `) B/ b3 C$ v& S - }
2 T( V( N4 A! k+ D" K& i - 4 y; W% z ?/ {% r
- for(uint8_t k=0; k<3; k++)
+ j- t$ O g/ x - {
1 `& I( j% R$ q: N - Sensor_Data[k] = 0x00;
- [4 O; ~3 n! I - $ w6 q+ ^0 [: \; M4 B) _
- for(uint8_t i=0;i<8;i++)
7 M9 w p9 ~- t* d+ `/ y - {: N) A" R5 U m" x, Y# R
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level$ w' q2 l9 C/ L* N* K
- HIGH_level_read_time = micros();
1 f; x. a: r5 p3 { - LOW_level_read_time = micros() - LOW_level_read_time; U% k' L( u0 G0 F3 e9 P
- 3 x& |# H9 M, E- `/ G* \$ C
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )2 ~' P' `0 v: \" K% e
- {
7 X1 w+ ^) C- J) K5 j9 F! k - return 0xff;
/ O" U7 |& @, g* F" a; j - }* C2 H8 f4 e2 O; [$ B( A, ^4 \6 ?
- ) A }2 n" `' m- B; y
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
2 O4 U3 d7 L% _" B- W( F - LOW_level_read_time = micros();6 s) [6 b4 n* X# h# _
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
* A+ f8 o* j Y& |. I Q# k
. ^% T i; v& i5 A2 C( A0 }" y& i* y* g- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
0 j0 g# D& | l% n+ o - {
4 M% a/ O; z9 j - Sensor_Data[k] |= (0x80 >> i);% a1 e( ~0 x" F3 C9 I6 `
- }4 |# N1 x2 K! Y5 q
- else if(HIGH_level_read_time >= 100), R, k: F7 h. m! P% E+ N; s. ?
- {
& {, B5 h' l( o5 ?$ b% s - return 0xff;
6 ~# ] q6 q1 m7 W9 _. Z - }
, K5 o% n9 s+ C
" S6 y( b. u9 `! }+ T& a- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)9 x/ ^, E2 w2 i& i6 \
- {6 Z( t s2 \+ K3 R6 U
- return 0xff;8 P2 k' A" |& L3 D" ]& m% [% V2 m6 l5 [
- }" i" h. _' C1 {' o N% \( h
- }
" h0 s# H7 G% o5 [ - }
, C7 Q) U+ G( f* ^& z/ p
* X0 ~- ~+ x3 s0 \/ s' M/ S- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
3 U/ ^' b: J$ [( D - HIGH_level_read_time = micros();9 o. |4 D% L* z- Q9 O4 a1 M7 s
- LOW_level_read_time = micros() - LOW_level_read_time;
6 y# ]/ K7 D# G+ Q - 8 Y# c% p; i* G9 O
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )& T. C4 N7 @$ m
- {+ o" P; \* V0 _0 O( t' K: j
- return 0xff;2 ^' f" _; T: i5 Z; H6 s" e
- }; J) d1 F" w3 L( @) z* m. U
- ( g- L3 _( E, q: H4 ]. i
- pinMode(_DataPin, OUTPUT);
B3 g6 F! Q5 X6 U* s" o2 Z - digitalWrite(_DataPin, HIGH);
' v- T& N: d ?' R0 i' V' }7 ?) _( g7 a
) j. A: v5 {# i$ _: v1 G- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))# S& @ [/ K! G
- {
+ C; z4 l- |& P! F1 V* t D: q6 F - old_data = Sensor_Data[0];
1 D& U+ _% H6 p4 S9 y5 {! n - return Sensor_Data[0];" f# G# } d7 w- V2 P9 U
- }+ F. r( O0 C ^7 T
- else$ X j% C+ W) w f
- {
; M! l0 J+ Y. e( ~6 E - return old_data;, I7 n8 Z; i( ~! j5 K+ I
- }, [2 p& _8 w3 Z
- }0 K7 m8 x3 ?. q6 ~- z( B
複製代碼
% Q/ A1 g3 {, ZMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
* U' ^/ }3 v0 T- T4 i( f/ \
; U- _8 p9 [# I; ?" |) L+ v |
|