|
|
" w& T: K% z6 M) t! o- #include <Arduino.h>
' h5 |6 A3 E$ s8 p - #include <MeAuriga.h>
/ g2 Z# E8 C* F$ t9 \2 p( X( `0 A - #include "MeLineFollowerArray.h"3 l" t- @2 X- ~, o
; v$ [& h% q, G$ {% f7 |* s" V- MeEncoderOnBoard Encoder_1(SLOT1);
$ s8 A# x3 d7 c) K - MeEncoderOnBoard Encoder_2(SLOT2);
; w, [: W& }, C6 `: ]' v' J4 p* k - MeLightSensor lightsensor_1(12);' ]+ j" A4 p' P. K, l" {
- MeLightSensor lightsensor_2(11);3 m; A: {+ q6 F1 X) [
- MeBuzzer buzzer;# ?7 |! E/ d& _3 @5 U
- MeLineFollowerArray linefollower(PORT_6);
* ?" U% G; [4 r, v
. e; Q; Z( a0 }- #define Error1 1
6 C3 H8 I; \ O0 J7 F8 B6 y - #define Error2 2
4 d( `5 V- ~, c4 i4 ~ - #define Error3 3
/ R# q2 k% J* x - 9 l' j+ n* D$ _/ l* ~
- #define Kp 157 `$ y9 A7 ` ]8 u2 d+ D5 S
- #define Ki 0.15" N+ m7 \& t% l
- #define Kd 0.03
# |% Y0 T+ q: @& b; s: l- t( ^
, T9 [: A9 t: r- S- uint8_t sData;; p2 k" r+ K" M: }* J' Y
- uint8_t D1;& }- z0 Y+ F h* F/ z {$ |
- uint8_t D2;
7 u$ V4 e; R9 y2 m+ |- q8 N8 ^ - uint8_t D3;4 c. r4 Z& L9 f% L- M
- uint8_t D4;% F7 j7 t( Q+ _/ m( B- L2 R
- uint8_t D5;
& S, F, { t$ Q4 a' `7 {$ i - uint8_t D6;
+ J8 C, Q& J2 l, m0 r! G1 H
( L% X! q2 `" b$ C# N- float previous_error = 0;
1 B: ^# O- |' `' X" X - float integral = 0;
& N( b. M6 f! `: V - float derivative = 0;0 Y7 H+ z; b/ g# W0 V
- int Speed = 160;
2 y; ]+ r, N" t g! @& T8 m - float output;8 t& m, j8 K+ ~- @$ ~- U
2 u5 X) S5 l# R. t! C- byte Left;
5 x z* u- v& M" \" K; |; P
! d' Z) g) p$ X! q, A- void setup()
. {& p4 `" }$ A- G - {
. d& S; H3 ^5 ?$ b2 `* y% T9 n - //Set PWM 8KHz
( A3 {) c7 L! L - TCCR1A = _BV(WGM10);
8 {9 u$ b6 z3 w. T2 [6 y% _ - TCCR1B = _BV(CS11) | _BV(WGM12);
( G: l( ]6 i* {; q$ H: p - TCCR2A = _BV(WGM21) | _BV(WGM20);
& t5 S2 e: Q0 [" P - TCCR2B = _BV(CS21);/ P# S9 {: S! r% ~ V& i
- Serial.begin(9600);0 @- F* Y( [+ M- Q
- buzzer.setpin(45);1 R, D( K* N0 H$ @- _4 m0 p( B
- }9 e9 M+ X; Z5 h0 t3 y0 f( X) [% K
! B+ k: H W W8 \. l- void loop()) t: a2 Z/ O$ ^8 O2 [) W1 O
- {0 x; X, `8 W1 `% ]. m6 h: d
- while(!((lightsensor_1.read()) < (10)));! @* ?/ Q! s! p, n, U' h1 r3 x1 K& U
- buzzer.tone(1047, 500);' k; K6 G- R. V) I; l, L
- delay(500);
/ K) w/ H" E: v) r4 v: l& h - do
& d" N8 R" T$ r - {
6 ^0 R: d7 V6 B. E; H3 o# E" n7 b - int Err = getErr();
+ W( | K+ q$ ?' L V: r' |3 b - if(D1 == 1)
8 Z* v* M8 P& \" b* ? r( @ - {
3 J: ^: |1 }# @ ]- c& m: p - Left = 1;- n! x: c- t' l* M1 d; g" t) D
- }
: Z+ D$ [! R w/ O/ r - if(D6 == 1)
4 H7 \& H# _7 o# x5 L" i - {# J: N5 v. X, K7 d7 v
- Left = 0;
. t) c% z& B* \ ?& Z) C9 M - }& w4 O2 b2 O. O' e2 g8 P* o; U& m
- if(Err == 99)
2 ?8 D4 d' Q1 [/ A4 {8 ~! \ - { V! ]" W1 K' B5 h& G7 q
- if(Left == 1)- e3 t: p8 g z
- {
2 Z9 \" Q5 ^# j7 K - Speed -= 5;6 v7 C( I2 [! u+ I4 w. I
- moto(0,Speed);
4 d4 j v1 I% t& O - do# B' K2 O: @5 X" B7 Y" |3 E8 S
- {8 K) W/ p! Y ^7 Y: N, z: t
- Err = getErr();
7 I1 [6 b4 o5 |0 _3 h: D - }while((D1+D6) == 0);" o3 t) N" C1 q3 Q" h
- }5 W% o: c ^5 C: [- T1 q
- else
: H: E0 r! t5 @; R0 v) J - {: X) `4 n1 ]; |: Y. r# y
- Speed -= 5;
, S0 {$ o! G0 q) O3 H/ l; n8 ` - moto(Speed,0); ( L L d9 d. m8 y
- do
1 J6 j" K, f! M5 k - {
. c" @+ z: F% I2 a4 h$ E' N - Err = getErr();8 G" {# D R2 L
- }while((D1+D6) == 0);( W3 a2 i% [- X" a! m& V1 k
- }
: c2 o5 W, Q( ?4 a - }
, f7 z+ O( a. ~6 L& { - else
3 K: ~6 q" C. q - {
! s$ C1 `' N" d+ R+ j/ ? - if((Speed < 160) & (Err < 2)) Speed+=1;
7 R( F1 q3 C4 M% v( B, n E9 L - if((Speed > 100) & (Err > 2)) Speed-=2; # B: M: _# b* N8 P, b8 o5 X% u
- integral = integral + Err;
* W+ Q( I0 d U$ W' u) T - derivative = Err - previous_error;: P8 k3 g' T2 e1 p- p
- output = Kp*Err + Ki*integral + Kd*derivative;4 n v; @. B/ p' q) b- [. \
- moto(int(Speed-output),int(Speed+output));
" w. H6 h+ _, E. X - previous_error = Err;
: D! h$ o* r& B7 \ ]" {4 l4 h - }
* j" E, l/ o1 Q; t& k! l9 x - }while(!((lightsensor_2.read()) < (10)));6 `, V* ~3 L: a4 i
- moto(0,0);( e. n# ]; [1 n# k/ ~) X: |
- delay(500);
) U! ]: {# I: L; n: V5 e - buzzer.tone(262, 500);0 {, M2 d4 G. {# L: q+ i+ c0 G
- }0 i# Q! e- y- a) p& I9 v8 D" f
- 1 {3 `! d6 C6 ^2 y d H4 A' e
- int getErr()
! B* Z) b& s' F; Y& u) T* M - {
' a7 z& x/ w# P/ n( b3 i. y - sData = linefollower.getValue();; }4 A0 G; k; r6 m5 r1 n; e
- D1 = ~(sData>>0)&1;( J" C: A9 p& V. j8 {
- D2 = ~(sData>>1)&1;- a- K$ o( N- y: ~
- D3 = ~(sData>>2)&1;
7 B$ @; v; X9 o/ h7 f! ? - D4 = ~(sData>>3)&1;
9 s8 {7 Q; w) _, o4 _$ h2 j: o - D5 = ~(sData>>4)&1;
9 F& M; Z% q2 u7 r' \8 c - D6 = ~(sData>>5)&1;! `" f# O. J1 o" [
- int downD = D1+D2+D3+D4+D5+D6; b3 T9 M# v4 z; Z8 t2 _2 M. ]
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);4 x8 h3 g7 i" T/ w& f" X" v
- if(downD == 0)
3 G* u8 G* S9 [1 o# F% S - {
) S6 F+ L; f/ C$ V/ U - return 99;
. t1 U) @2 R" f& B, V/ l& { - }
4 y# K5 q* [+ | - else8 U/ H _5 ^, E; z2 y- [
- {3 s! P* \9 r. T
- return upD/downD;$ k. K, j1 Y8 Z' X7 c
- }8 u( V3 c1 J+ F. p
- }, G9 _6 c1 L6 Y& j6 U
7 O4 ]5 p; B6 C: J/ u; P n- void moto(int Speed_L,int Speed_R)& [3 p7 q( s% o
- {
0 A; _! X! {! t$ ]! ^6 p - Encoder_2.setMotorPwm(Speed_L);
M9 D- \6 H3 F2 y1 b+ I9 T- ^ - Encoder_1.setMotorPwm(-Speed_R);
" ^2 c7 I/ Z4 \/ w) D: Y - }
複製代碼 ; u9 z y5 X& H1 e- P
MeLineFollowerArray.cpp q2 [- A5 i- i+ D
- #include "MeLineFollowerArray.h" d4 h) Y1 I# ]. ~
- o5 _. @5 X' i$ }( q/ }- #ifdef ME_PORT_DEFINED
5 T' ? a7 Q# q& O3 s3 u' G - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)! I {4 A# O$ [* s- s
- {" I9 s0 r3 M9 x7 J/ p
- 1 h, Z! ]! k6 ~ O! w
- }# Y, e G! S. T* O) B
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
2 S# ~3 p! `* B4 T! L - {: a* p l1 G9 P. G* g8 |/ X" e ^ f
- _DataPin = mePort[port].s2;
8 M/ [# \0 w5 P+ h) v9 Q - pinMode(_DataPin, OUTPUT); ) y' q9 K/ w; s6 c
- digitalWrite(_DataPin, HIGH);$ S/ M; ]) K# i8 n: p* c
- }
, C2 E, e. _, z- m - #else // ME_PORT_DEFINED
. _, d0 N9 C- c0 i - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
. y$ p1 ~: N6 b1 z/ @ - {
% O- R1 r- O$ j! C: E" A - _DataPin = pin;
- z# e4 q# V( {( k - pinMode(_DataPin, OUTPUT); & T) i% F8 t3 l& X$ {" F/ L
- digitalWrite(_DataPin, HIGH);. \) S/ U( F% p8 I8 [* E8 `* h
- }9 _' p0 \" R& |8 k
- #endif // ME_PORT_DEFINED/ b i) X( L9 v9 w
- ) ?) q# s9 t. v: {- [
- & B* \: a4 S0 x, ?8 V: ~
- void MeLineFollowerArray::setpin(uint8_t pin)
+ W: Y. q. G8 E( [ - {+ n1 r9 I+ E* n% {3 v) h
- _DataPin = pin;+ h% d3 j) P( R
- pinMode(_DataPin, OUTPUT);
: o& t4 s7 [' ?0 b5 N; Q8 I6 d - digitalWrite(_DataPin, HIGH);4 O) P* ]# `; C. d2 O# k# u/ B, U
9 {+ b# g" l5 [$ @. V/ S- #ifdef ME_PORT_DEFINED. U% G# \ ~% h/ @) C( j0 Q8 p* J
- s2 = pin;7 P# A& z u% V
- #endif
" r' U9 X6 v( V - }0 R9 L$ ~% Z/ W; v" `- y4 n. f
- ) q% X6 l! @, a+ t" r
- uint8_t MeLineFollowerArray::getValue()! I- }: Z3 D0 \* n0 a1 d$ j& L$ B8 s
- {3 o i, g0 x0 y
- uint32_t LOW_level_read_time;/ ~2 s8 K) m4 S$ O; o/ I, z% E
- uint32_t HIGH_level_read_time;- \7 L7 u9 m1 ~. S* W. y5 \
- uint32_t time_out_flag;
# |. n" t8 z4 K ?1 U1 _1 z9 b3 O/ K - uint8_t Sensor_Data[3];9 z5 o" D% N' K. _& | U
- static uint8_t old_data = 0xff;
5 W; D1 ]2 p4 E G% d
5 u! q3 ?8 J+ U) |" D- pinMode(_DataPin, OUTPUT);
6 ]1 F& t: Y3 ^, @, b - digitalWrite(_DataPin, LOW);
/ P4 O* i2 m) Z J4 |/ a. r - delayMicroseconds(980);5 K$ ?' o% b7 W8 Y1 o [: c
- digitalWrite(_DataPin, HIGH);
" v' @5 k2 K; ^* E7 t0 z$ p8 W - * S$ ~; O: h: C0 \
- pinMode(_DataPin, INPUT_PULLUP);
) D$ g% g9 Y/ U7 Y! U7 M - delayMicroseconds(10);
4 R/ `9 f2 i2 T - ( d$ U0 q/ a& }' c
- time_out_flag = millis();
- n- y+ d$ f# z/ N3 l( L7 F' a - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% M& |. B" a. _" @1 G& @ - 5 R% ]; P" H! j# @8 V
- LOW_level_read_time = micros();6 w" z, v1 a5 d4 \) V( r a. z8 }+ ^+ ?
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
) W- W1 D$ \4 w5 D9 k; b ] - {5 r1 [5 n3 P8 \2 y% }$ P
- return 0xff;
0 D5 }, ^9 t+ j( v - }
6 I8 M+ n+ U! K/ \2 }" p0 K - 3 u( u% o% `& [
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! S+ I% Q- P1 N+ M( d0 U& E/ D0 W
) {, m3 E* p; ]; j% s! x- HIGH_level_read_time = micros();# O. n- M8 ~) [: N9 X" D9 F
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
3 c3 s& B1 y& L6 u
. i& d. y" D" e! Z: w/ {- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* ~/ p2 Q9 A$ e4 y+ N: f
- {3 P& _4 S) K1 S
- return 0xff;! A' `; j* k+ h$ e4 R0 }/ y% I
- }
g( J) G7 @4 i j
- q }% n4 E" a- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))1 p6 @ ^) J B2 n8 A
- {% b6 K/ r `0 ]
- return 0xff; [! a+ T9 z( O( r0 A
- }
7 g) M, y$ r" [" Y - ' F2 z2 `) g* m; X, Y% k
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );# H, Y/ K9 t2 E$ Q7 T, ^
- LOW_level_read_time = micros();& f4 T" O) i: P$ U
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level" }. b% b8 W$ H9 K# O: i. Y
- 0 j& G6 Y: F5 Z& u/ ?9 G6 f, z- g
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
& }1 h! f. }/ r6 H/ b! o- u - {
1 R2 r0 D& _8 V3 d% B) T0 \ - return 0xff;
) \4 g1 V# r( D& @* s. k1 R - }7 x( k; x/ Y0 S
- ! \: Q& q3 Y) c9 q. P/ f6 F) _
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
7 a/ a; D* e& l8 m V8 @ - {
8 K) e, T$ N" S9 j+ X2 Z# P - return 0xff;
8 D; b" s" F( S* M$ x! p - }
; k1 R0 N G$ @3 z/ E. j" O. X$ j9 A - . U2 g6 Z: j4 G* { J& ]. r' Q$ H
- for(uint8_t k=0; k<3; k++)
9 |( b/ T! _" v7 m/ M - {4 O% W: K) ?' B6 i! Z+ p' v
- Sensor_Data[k] = 0x00;5 t" e8 p- F6 O0 G3 T
- 7 I u4 w2 u7 }4 h4 X& n
- for(uint8_t i=0;i<8;i++)( v# | E% `) r. i9 z4 u
- {
; T4 M u5 P6 v9 x' P - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level( g* c' P# S9 ~+ y' R
- HIGH_level_read_time = micros();
3 G$ W4 z0 R9 A" O& q8 V - LOW_level_read_time = micros() - LOW_level_read_time;1 Y$ x# i" f9 X, P0 C
- : U- A9 j% r' c
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )6 u$ }% s" \& M
- {
) X! I9 ]- _9 Q0 @! g, I% Z2 C, m - return 0xff;! d: P" u% V+ [$ k7 ]5 D; \1 t
- }
J5 s2 w( g. @- r8 z: A
9 F3 n$ j: C$ Z8 d2 O- @! A- t, T5 k6 {- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- X0 o8 X2 ?$ h+ Z - LOW_level_read_time = micros();% F+ W' g ]- ]7 I1 T
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level* [* s" `3 y% j4 P. ]: |) X7 G
- # L0 S& ^! ?0 q/ `
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
& g4 @- Z! W! x7 d0 f7 g' a - {0 U- ]6 A( L5 }' ]5 R
- Sensor_Data[k] |= (0x80 >> i);
( m4 F$ x9 k( ?, v0 a' s& @/ ^ - }% |! e& N: r C3 z7 L) \$ B) a
- else if(HIGH_level_read_time >= 100)
9 E7 F+ {5 u4 @5 C) b- p" J - {
+ _# k. N3 M: r% u6 E' q5 h - return 0xff;1 }% q A" ]/ ]4 T
- }
( O- F T* ~7 t/ L - 0 a F2 a- ^! A: b
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)* O, R \1 n( V3 S
- {0 C( l$ `' c0 @6 o$ i/ e1 Y
- return 0xff;
9 m9 G6 T( v# c4 `3 u8 m - }! t7 J* r; K: W% n6 ~
- }
( X% p& j9 \ J - }; G$ q4 N) X: g" L4 R
- 4 @3 M" N; \, I+ K$ Q
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
+ n5 n; B- h* C& U0 `- Q; ^9 V2 ^: O - HIGH_level_read_time = micros();/ p) I( Q2 O) D* q, ?" T2 ~
- LOW_level_read_time = micros() - LOW_level_read_time;9 ]8 ^( n0 q0 p2 k5 r8 I$ K& M' }
- % h1 O9 }- M/ ~7 T8 M2 Y. o
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
- u( S: j' ]- m6 Y5 d - {
# f- C5 V. c9 p6 H8 d+ J6 j - return 0xff;0 Z% ]& I" u: u& A; `& k( M' h
- }
. d6 z6 s9 ^7 F4 S9 [9 U/ M - 0 W8 X$ ?( [* ?- I
- pinMode(_DataPin, OUTPUT);
. A/ z& n7 }9 ~9 C; w - digitalWrite(_DataPin, HIGH);
4 K7 J9 ?; A6 i m' q4 Q1 y1 q
' n" d! R4 s# B1 j& b0 V3 ~- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))4 j- x9 Q" {, o! b" P I' G1 G
- {
8 v# E- b0 a7 L5 [! X - old_data = Sensor_Data[0];
& n1 }( O1 n# c - return Sensor_Data[0];4 W# [, E( U2 `, x! i7 W8 [
- }
3 T% T4 P5 @. j6 {- }. ?* M - else
7 I) q+ P8 `! D. P - {+ H, N0 }. c8 s$ T& ?0 j
- return old_data;
! ]' A0 Q% Y: {/ K" g4 U- } - }
! [( ~ Z6 V; O1 c/ f4 O) w - }4 A, Y4 L- S/ m# w# H
複製代碼
8 Z8 I0 I! k" l9 [ W% E: ]MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
* K n0 u3 b" e# j: X3 I
' o, [& h' h( S! @9 `
|
|