|
|
0 u; e- I, D4 u: I* l" y7 Q
- #include <Arduino.h>, {) j: S A5 n
- #include <MeAuriga.h>
% w% v( O. y7 y% }% Q8 P3 a- k - #include "MeLineFollowerArray.h"
# a9 a3 P; _/ _5 \% ~9 p g
5 J* t3 r% {- K; r5 I- MeEncoderOnBoard Encoder_1(SLOT1);
+ K0 c6 U1 O. b7 K. x4 c" t0 c& l: i X - MeEncoderOnBoard Encoder_2(SLOT2);
( r9 |2 ]5 ?( h5 [+ _3 ^/ d: @ - MeLightSensor lightsensor_1(12);; h* n9 n5 p3 q$ n% p
- MeLightSensor lightsensor_2(11);
+ Y: T: x) a# C! J {- N - MeBuzzer buzzer;
' O2 _( K: U! b9 H9 h4 V( r - MeLineFollowerArray linefollower(PORT_6);
# t& o$ `7 o$ r; w% _3 M+ s
# P8 _+ ]+ H0 S2 N- #define Error1 18 |. X, u2 B; i" s* c0 S+ T
- #define Error2 2
6 u( ]% j- ?$ [. p - #define Error3 3
5 N; f% a' V( \ - ! E1 c( t, K8 n1 g
- #define Kp 150 j7 C( R) [. s4 k9 |
- #define Ki 0.15
( {3 e6 Q9 [ O' D - #define Kd 0.03
2 l# I; ]( e8 w# X - . ?3 ~& R1 @% o0 B' S
- uint8_t sData;
" B' k" x2 N" K - uint8_t D1;' {2 q% o. \- n1 j
- uint8_t D2;
9 L5 e! ?" c) E; v" O - uint8_t D3;+ {% b8 s* k( L! w8 B8 R
- uint8_t D4;
* L+ ^! ?1 p1 S& `4 v - uint8_t D5;2 s% o: `8 _% C+ t3 l
- uint8_t D6;. F& V9 h6 j6 {' y1 M
- ! M* j+ G! i' i$ J2 Q
- float previous_error = 0;
+ y! M& X8 K- K5 w& @ - float integral = 0;- J% ~: j U4 e' d+ g3 r$ _
- float derivative = 0;
* B2 h, W; K" [, V" X5 U' [ - int Speed = 160;- O( ~4 y( u d7 e1 [' I# f
- float output;( ]# {, }0 o5 ^; c
- . z& K! s' y* x# C* C
- byte Left;
2 x2 A0 F: ]( t0 y- a - ( G( j, e5 d* H9 t: \+ J
- void setup() - S2 V7 r9 v$ n
- {: F3 Y8 e3 K- D
- //Set PWM 8KHz) g( D* z/ Q, i# E5 K) d, _+ u
- TCCR1A = _BV(WGM10);
9 }4 N @" G. q: c J/ I, t2 P - TCCR1B = _BV(CS11) | _BV(WGM12);, b( {" _5 m6 R$ f7 ]
- TCCR2A = _BV(WGM21) | _BV(WGM20);& c1 k' Y5 p, c v
- TCCR2B = _BV(CS21);7 r: c2 c% L$ s1 B# ]' \8 w
- Serial.begin(9600);
% N) H0 s8 j% N5 v) U6 U - buzzer.setpin(45);
! J M+ J1 A! e# j* e - }
) j& _+ L; \4 e7 @2 T' i
4 `- q- u! ?8 t* M3 N8 N) f8 H- void loop()* `( q- g! h# |
- {9 U5 i, t8 O6 k6 q
- while(!((lightsensor_1.read()) < (10)));
( W( A Q5 s% |* X) D1 V: K - buzzer.tone(1047, 500);
2 @- G* W6 ?( E# |" L' h - delay(500);, k- X$ B: d2 e- U
- do
1 L5 _% o+ o* m Q$ y' H- o+ K! C - {& z' C9 P' w) S4 [2 d
- int Err = getErr();
) P1 u" n! n2 C3 p7 O$ A - if(D1 == 1)
5 R8 m8 b9 K# c: v - {
( I! ^5 N# u" w - Left = 1;+ \3 p5 y$ h" Z# P5 S4 s
- }
* ]0 l0 H( m" N8 d" d& q; ]0 P& x - if(D6 == 1)$ N* R$ Y0 S3 C$ u) s6 }
- {( Q# y$ H! ?; @6 O: ~
- Left = 0;3 S& J* ?8 Q* d* ^% x( ~, ?2 G
- }7 X( e7 f x5 g1 C+ q3 S" u; A& B
- if(Err == 99). \# f7 s# V, T
- {7 C. a" u( n o
- if(Left == 1)
$ @9 r& z7 d, f2 ` - {2 K5 F6 { v# D
- Speed -= 5;
' D) J* w/ A1 [" U3 [ - moto(0,Speed);
9 a( l. g, P" J+ u1 M+ q3 k, Y - do
+ H0 r4 O" |- L n0 P - {
9 y% y' R! k L) h @ - Err = getErr();
0 `) O9 i& V" ~' [0 T8 g - }while((D1+D6) == 0);
) [+ P/ Z3 `2 g) ]; G' ^: R% r+ i - }8 ^4 G" U2 B& r+ G; T, o/ X
- else
% o7 B( \' l& S4 A% n) [; D' x - {: X. N4 N9 p- l' I* ]8 D" W7 h
- Speed -= 5;8 s- @7 T H) F# m" t, K* @. s8 W
- moto(Speed,0);
- V0 @& ?' {6 \) D - do" _% d f& P8 I$ v9 x
- {( G0 ]( W' Z0 A9 e
- Err = getErr();
) d9 {8 R* j/ P7 {' Y! q1 X. N - }while((D1+D6) == 0);8 [) {! G: c* c' t Q, Z3 }: `( Z
- }! J* F+ U. v$ Q0 [% }
- }2 c2 P5 K% P( L! I/ X7 c& P
- else! @- u2 F+ `7 W) ~+ v
- {
( Q6 g" S& ^' r# l& G - if((Speed < 160) & (Err < 2)) Speed+=1;
; L9 T0 _9 A! f& q4 h$ c6 ] - if((Speed > 100) & (Err > 2)) Speed-=2;
/ ~6 Y4 ^3 q2 l, E/ z; j: M7 O" M4 _) \ - integral = integral + Err;9 N* O" H R8 b/ S2 c- n- V" A
- derivative = Err - previous_error;' m- B5 }, O$ G' U: P
- output = Kp*Err + Ki*integral + Kd*derivative;
' k) @2 p6 U( Z) Z1 q2 H) v# Z2 @ - moto(int(Speed-output),int(Speed+output));: x, N) @0 Y0 a; K
- previous_error = Err;
4 `8 F# z) W. ^+ |$ V4 f! [ - }) v( A- i9 }( H+ c$ L
- }while(!((lightsensor_2.read()) < (10)));/ J; g# _+ [/ K g( y% N6 Z
- moto(0,0);
* X% u. E6 c7 G" |* N - delay(500);( U) X' N* N; G% L# [1 t
- buzzer.tone(262, 500);
+ M0 o5 d( b5 g- H, q \0 }. e U6 P - }6 D! y2 T, }8 R
- ( c1 {9 Y3 J. b g8 ^$ Y
- int getErr()
; y& P. p) \! A6 j - {
+ E9 ]3 x, b% N |1 x. r - sData = linefollower.getValue();
# S- |! y6 o* K: A+ h4 _ - D1 = ~(sData>>0)&1;
- O# a' ?$ P. E+ g; N, P7 p+ a- V - D2 = ~(sData>>1)&1;
# Z* e. |0 z, o4 S8 h& l/ `( p - D3 = ~(sData>>2)&1;
" Q# F+ X5 C0 r - D4 = ~(sData>>3)&1;
0 T$ r9 C$ ~5 a9 E' W - D5 = ~(sData>>4)&1;
$ M+ W3 @5 u- V% U: H7 F$ E - D6 = ~(sData>>5)&1;. x1 b2 \* G' N7 I& P0 p- ~5 f
- int downD = D1+D2+D3+D4+D5+D6; L- v. a9 N) K. z9 p7 \/ ^8 e- ?
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);. C- {' }2 o& H; @) S5 F; ?/ l
- if(downD == 0). h5 G/ m. X+ ~4 }8 {8 C' s
- {& a1 n, s1 u' ^4 P8 H
- return 99;7 h! r" z" ?+ K1 Q1 z9 R" N* _( S
- }+ z3 o2 q! O& F% l% b7 D- c* E
- else
[1 c, s$ u, N; s1 l - {
) ?0 \$ u0 k, d1 M9 S/ z7 W - return upD/downD;& S& D2 k3 h& K& F2 e
- }$ t& a, C$ j6 o A/ J" Y7 d7 ]& x( c/ g
- }
( f9 O% J! g0 N6 y" P4 Y9 f
* C# [! U+ {2 }- void moto(int Speed_L,int Speed_R)2 V; i% t0 |! E0 W& U9 u. c6 H
- {
0 Z8 y! N( W: x, j, n3 K. k; {3 c - Encoder_2.setMotorPwm(Speed_L);
4 B$ f4 |; Z+ s, r1 b: J - Encoder_1.setMotorPwm(-Speed_R);; T! C/ |2 K+ P& R2 K
- }
複製代碼
6 G" Z, l' Z. S$ {( n3 \1 S& }MeLineFollowerArray.cpp
! |! E- a0 h% _' o1 t- #include "MeLineFollowerArray.h"
9 l4 k1 z7 B8 ]% N: O! R - $ A2 E% q* g& k# a. |7 y
- #ifdef ME_PORT_DEFINED$ n! H3 j8 U; P3 v
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
+ v1 p+ H& K! b) i- z, W. { - {6 R% j5 _9 F( j% P5 G9 c
- ( O% E7 T/ N- P( c- D4 v
- }
2 v5 |& {+ f; g& P; O. t7 b - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)- m( o8 @6 j5 G. H3 A2 u
- {
3 m- W4 P" R+ o9 z+ y - _DataPin = mePort[port].s2;
# d6 M, ]8 R _6 y* k& J* |# ` - pinMode(_DataPin, OUTPUT);
% q5 X1 J- S8 V, A) V }' f$ U - digitalWrite(_DataPin, HIGH);
5 N5 M% O5 L) Z% r. p8 p+ W5 c - }
1 n1 Z) L0 [9 o6 Q3 ~* z - #else // ME_PORT_DEFINED
3 }& W3 ?4 L+ o1 C" c' v7 i - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
7 m- {- i- A/ L) k/ |0 X - {+ Y8 r0 n- E' I6 q7 _8 ]. |
- _DataPin = pin;4 _5 K- u7 M% J9 l2 E7 P
- pinMode(_DataPin, OUTPUT);
5 d, C8 O& |9 Q# [" y - digitalWrite(_DataPin, HIGH);7 Z* l5 K8 }4 [6 R0 Z
- }3 D3 X2 e8 S) u$ _+ Y" a- m
- #endif // ME_PORT_DEFINED
. U# G+ U: V* H8 D% f - . } ]4 _ O9 N4 a4 |
' p; `( h; D( F/ T' H E' M) @- void MeLineFollowerArray::setpin(uint8_t pin)1 g2 q$ _! }. W( d9 C0 F' P! Y
- {
) L$ o3 G o% B - _DataPin = pin;% x/ G4 t5 T2 q+ F/ ^* g
- pinMode(_DataPin, OUTPUT);
, p5 q) r; l5 |) D - digitalWrite(_DataPin, HIGH);
: ^: ]+ M4 b' {+ c1 V
0 n# J) N4 H4 l* S- #ifdef ME_PORT_DEFINED1 x; Q5 ?8 ?0 l
- s2 = pin;. v3 m' E; Y$ x; W& E* \- b Y
- #endif9 v" T& }' z7 k0 q* i& W
- }
A3 Q1 s" q4 `1 K5 q$ K- \4 q - 6 @) x3 | g- o# {% Z% F2 P; z# c
- uint8_t MeLineFollowerArray::getValue()
6 l2 k: M, J7 k8 w S# |) C - {
' Q0 n5 [6 ~8 b% g& C' t - uint32_t LOW_level_read_time;3 R3 s4 a, N# U' r
- uint32_t HIGH_level_read_time;
' r) ^* l& B0 ?6 b; \4 V( z - uint32_t time_out_flag;% ~' Q0 |! ?; ~+ P- _
- uint8_t Sensor_Data[3];. R2 U: h- W i- [* h7 J
- static uint8_t old_data = 0xff;: I2 v' U- w( k1 U3 r8 l% J( \% W
- 3 `3 ^/ q# b: {" r# Z4 [
- pinMode(_DataPin, OUTPUT);
6 e1 Q2 g |) ^+ ^. K - digitalWrite(_DataPin, LOW);
- l/ R9 T+ A% h$ ~" Q - delayMicroseconds(980);! e! Z4 v+ i! z( a6 }& p
- digitalWrite(_DataPin, HIGH);
7 Y1 h* P5 p4 r8 @0 M# Y4 }2 B - ) S6 n0 }# g" j( ?1 M% F& S
- pinMode(_DataPin, INPUT_PULLUP);
& g/ d! W' N" u& g' e: X$ Y3 b4 y - delayMicroseconds(10);: v3 W+ E: b! O6 \
- ; }$ {0 K3 e0 X4 o. l# y1 l' }/ v
- time_out_flag = millis();
x8 ?1 \/ o# H( C' b - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% \' s8 _% o* e/ B, o0 c - ! K+ P- s+ _; Q6 V Q1 Q; G
- LOW_level_read_time = micros();
1 \" F3 Y; k7 h; e) {! r - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
: {" V* f5 U# H t - {2 T0 T. K2 e3 u' K
- return 0xff;
- G4 h5 t8 R% O2 e& K - }
/ Y: |6 S! d; f7 r- Z
. R" z0 I' ^: Y8 }- X* b+ j# k- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 R+ L7 z: I( V - ! T% t! _- Z% H# Q0 f; a! j ~
- HIGH_level_read_time = micros();0 i* V3 ` {5 V
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level6 s# ]1 O; d6 ~4 f; S; L
4 P) i* T2 c# V( ^7 n0 a/ ^+ d x6 E- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out% c& R( D8 f0 M
- {
9 Y! R A- @4 E- s7 P - return 0xff; s) b5 O- M4 a1 i1 k
- }
$ d% ^7 A% A. a4 v+ D. d - 0 w' e) d; f8 {5 u
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))6 [$ a# g* R2 h. n$ q* A
- {
$ ]' U! o2 g- U4 k+ ~ - return 0xff;
7 A0 U5 d9 t( h/ z& Z5 O) H - }7 v% ~9 N0 H" V% p1 I
- - h6 j- r8 G u0 K8 N! C: Q, M
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 U' o9 G) O/ R, y - LOW_level_read_time = micros();* m+ a# p- e+ Z$ i
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level! `) W8 W2 m3 k1 x1 | S
9 E. @$ T$ Z5 y9 o: P5 e j- k5 ~. Y: d4 R- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
' s- j; g; |# v, s; e9 I2 P - {4 P+ Z- t5 l' a/ t. F* t' f; W
- return 0xff;1 o1 n5 U+ \, J6 Z2 ?$ k
- }2 u, [! c* h) @ K, v6 K9 G' y
$ M* Z- e" w0 F6 e& q& k- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
/ @+ j/ m1 W9 }$ f# V$ i1 E - {
7 g3 y$ J/ c, F5 L" e- M! b2 l& h - return 0xff;
) Y; N$ Q' }- m8 |: p& X: ? - }3 t H I. \$ U: }# H0 r
- 2 v; K) H$ @: M8 K( }) `6 Y
- for(uint8_t k=0; k<3; k++)
: s- C z) b+ G }# H; X$ s - {
! ~2 |; ` j* N$ N - Sensor_Data[k] = 0x00;$ n6 a' N( p1 e8 o( m
- u( s: n7 x4 ?) q: Z. u5 k& ]- for(uint8_t i=0;i<8;i++)0 Y7 _9 L* @6 L& L: t0 T
- {, u7 r' H& d/ u
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level7 ^ R' B/ T0 v/ `% S8 h
- HIGH_level_read_time = micros();
* o! ]& {2 D4 P9 N4 p) D! b; Z7 G7 t - LOW_level_read_time = micros() - LOW_level_read_time;
3 N" o2 w! D @; d2 H0 S0 { - , |+ N$ C% T. q: m( K- i
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
8 v0 c# c! C9 U, M; t - { Q' E( i! @# B1 t x- m
- return 0xff;7 g& V9 H- `' v3 S: ~5 [) s
- }
' S' _7 v: ?/ S: F - % Z! v8 `0 w3 \& e- A @+ i3 n
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );5 N! c' e( Q9 U$ H0 y1 W( O
- LOW_level_read_time = micros();
; i6 F' w1 \' P# _2 F0 v3 O" Y - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
* T9 G& o* V3 Q. A( j: D( F7 U1 c2 F
0 @/ x- G5 Y s! g- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
$ ~0 y$ N, _3 L9 P# y! l - {
' _& Q# J- t8 {) q - Sensor_Data[k] |= (0x80 >> i);' N( I8 I- A# L3 P
- }5 i$ ]: ?. B; I- J: ]" \. q
- else if(HIGH_level_read_time >= 100)* H2 v; \3 R2 O
- {
1 t0 ~% E- P- p( f5 d - return 0xff;
" `% p0 ]4 I5 ~9 A, Z- U0 y - }
: R& o- f* Y" f. U7 t! t( N) F9 | - , |0 U7 H3 r% {/ n
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)( O1 k5 v. `2 v+ f9 m4 H( Y2 n( A) ~
- {
" g& ?/ ^$ v# N - return 0xff;
$ D2 U3 G5 m$ @+ b" o m1 d4 K - }
& c0 r( U! z& o' U5 |2 b+ E - }7 l6 d) W; O1 y8 |" c5 N9 w
- }
& s6 a' Y' n$ x) |3 g, U - # H3 }6 a+ {4 B$ E" ?
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level6 r/ B0 t6 J' c, D& Z# X
- HIGH_level_read_time = micros();" d; }- o( |7 ]6 w' C
- LOW_level_read_time = micros() - LOW_level_read_time;
7 L" M1 w: R6 b5 ^- F - $ d p0 a3 T8 r9 l
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )7 Z& J: ^% A% x3 v; F
- {" {" S' ]. M$ z" F$ {
- return 0xff;! Y4 N, i0 i6 Y, e
- }
J: x% N L$ c) u6 i9 J
) U W& Y4 z! S, m' [8 K3 p( z( |- pinMode(_DataPin, OUTPUT);
& }( v2 b0 |0 b - digitalWrite(_DataPin, HIGH);
! Y, `9 K. Q! t+ p0 Y' N& L
, p3 M: S+ t! h9 g3 B7 s: Z& r8 M- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))8 F7 M k) W# _; v! d
- {
; ?9 Y( p J1 f5 W' Q. I0 b$ n* ] - old_data = Sensor_Data[0];
$ x% d: \2 e: }3 d) C- V4 m - return Sensor_Data[0];7 m. b. A) P+ Z0 V6 d! i6 v
- }# Z3 b; X+ m, J" H
- else& d! |* x* A5 Q7 H$ ^) ]* d& y
- {+ w5 P2 z" I3 X* K8 v
- return old_data;/ U! |) S& l. u/ u
- }/ T/ l" S% e/ p; D! W( S& p5 a- U$ B
- }) k" V a: i7 j& O0 Q8 G
複製代碼 & ^0 e$ t; E6 b+ T
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
! M# T7 N% u" W: {# b& u
8 r! ^( O) T/ J) S |
|