|
|
, i" A: A5 H. K) @
- #include <Arduino.h>: M- }5 `9 z6 a+ p) O' K! s
- #include <MeAuriga.h>
& D; ~ q3 D1 v& R) X C; _: _9 p - #include "MeLineFollowerArray.h"& w+ n; e7 v! Y9 f8 o2 R$ k
& P2 N) j9 m. y3 P) F- MeEncoderOnBoard Encoder_1(SLOT1);
7 H9 z* V0 u' P+ v7 Y. j2 V - MeEncoderOnBoard Encoder_2(SLOT2);9 b) N( E, ~ R8 [0 o
- MeLightSensor lightsensor_1(12);
' G7 i& r: R7 w - MeLightSensor lightsensor_2(11);+ A, o7 b$ Z1 S" O0 [
- MeBuzzer buzzer;7 G3 R t6 t: i' [
- MeLineFollowerArray linefollower(PORT_6);) u) @' d- Z( X
- \$ Z5 @/ r( ? c& G- #define Error1 13 c, v k U! d9 |
- #define Error2 2! k. X7 Y$ \5 `6 p& J
- #define Error3 35 `$ e, k+ g) k I$ Z
- : k; W" w' Y$ ^. A! m
- #define Kp 15, n' }) `2 @, t S$ J6 C
- #define Ki 0.150 a1 K* f! p1 m' q
- #define Kd 0.03
6 W! E0 P9 `" u6 |8 N ? - 6 `# b6 N9 o3 b/ ~( u
- uint8_t sData;
9 a. h; L) w& z9 r6 f3 Z - uint8_t D1;8 K0 _4 D. t. V/ w- h
- uint8_t D2;
7 a5 b9 b5 z$ v. D" p - uint8_t D3;
: R- \% R$ {* P+ k' \) V - uint8_t D4;- A( A4 k4 P- ]( L" S
- uint8_t D5;
# M3 Y/ u) n/ p+ u# u3 e - uint8_t D6;
# I( o. |5 X& ^) o( j9 w7 M% {
1 W4 b+ P3 e1 Q- float previous_error = 0;
% e. t& `9 o" b9 {' \5 f$ r. r - float integral = 0;
' V# o. F1 t# A* U! z - float derivative = 0;
# |" k7 O" l& ]- p6 ]5 ? - int Speed = 160;( n4 D! G T* \) X2 r! u% c& j
- float output;* G% A! P" B# @. g
- * H0 h" a& W3 A
- byte Left;
1 e8 j: z# n9 N" ] - ( _4 }7 I3 L' F
- void setup()
7 `# J: Y# ^9 a2 U - {8 Q8 d* v. f5 j$ T0 x5 g
- //Set PWM 8KHz% L* b' J- I& Z5 G$ K( @
- TCCR1A = _BV(WGM10);
/ U( N0 Z: a* H - TCCR1B = _BV(CS11) | _BV(WGM12);: A3 {% P( v) o1 z' L5 m
- TCCR2A = _BV(WGM21) | _BV(WGM20);
. J0 w5 x; a! x) g- N" o( u0 B - TCCR2B = _BV(CS21);- c. O- ?. P t6 \! ^% R
- Serial.begin(9600);
6 P+ M0 x9 `5 e - buzzer.setpin(45);
* h) Y7 }4 z' S; @ - }
/ _' N$ l8 n0 K) _: d C - 2 `' p) e, I( o. }
- void loop()3 |/ Z- x$ N$ G- h# K6 w
- {
+ B! ^1 \5 b8 k6 U/ q/ U3 { - while(!((lightsensor_1.read()) < (10)));
/ M& y8 W0 s6 q. ?, o) P1 e2 Z! p - buzzer.tone(1047, 500);
( W V% S; a$ N5 ?3 b! o/ } - delay(500);
7 G% C2 B! F) C - do0 P' M( c) t: W
- {
$ H. r6 E, }& I% W5 y$ p$ X, d - int Err = getErr();
+ \& G6 F5 B, r& a+ T$ H - if(D1 == 1)
- I. H) u+ o$ q/ }0 y0 j; ^" ~* u - {; A. h) B" k, k( y% s+ e
- Left = 1;
3 b0 K3 z7 U4 Q% E8 d0 D - }
; H5 o: R d. _8 N7 E. {$ w - if(D6 == 1)1 V! x( J- _ x n. t9 _, n
- {
8 b9 b) |" {+ m+ |4 } - Left = 0;( e6 `# Q$ k. _) d2 T
- }
5 g# G8 G% g5 W3 Y" G" I, P2 Z - if(Err == 99)
- o; W/ `6 B% a4 c- M& t% S) [ - {. ~: O! M' H) \# a7 K
- if(Left == 1)6 o2 m) O% g. Z
- {
- T: ?+ ]1 A: f3 h) ~# M# ~9 v6 d - Speed -= 5;
8 c% i8 |4 I# r3 ^" }9 f - moto(0,Speed);3 P" \0 e/ X% i& n3 b1 Y
- do8 H" |9 L7 g L' q
- {' B5 Q& D0 L" ^' l/ G8 C) P
- Err = getErr();' t: F# h/ h @: H! W( U
- }while((D1+D6) == 0);% x1 e$ m& f5 p" C% F; A
- }! H/ M, ]- `! P# S; G1 h( y
- else
' E9 ]2 k( N$ l. o4 l n! W - {6 E" X0 x, q! M" ]) _9 O
- Speed -= 5;
% a- {) O) b, f2 x0 c/ r - moto(Speed,0); + r. s( @3 E3 \
- do) @- b( |0 V. d( @
- {
1 L4 V& E7 V2 Z6 M/ s9 Z% \ - Err = getErr();* D) {5 a7 j" C+ {
- }while((D1+D6) == 0);
! T3 {. i/ z* o: v9 E; X s - }/ z6 s3 N" T g* ^
- }, w$ v% f9 o0 N: H6 M. }1 O
- else
; f/ t+ Q2 ` w) j5 q! j - {% |( L# `! N9 b. U
- if((Speed < 160) & (Err < 2)) Speed+=1;
+ ?3 p/ m2 C2 O8 u5 N - if((Speed > 100) & (Err > 2)) Speed-=2;
8 j0 b$ Z2 i/ j7 U6 J - integral = integral + Err;% L8 U [$ Q2 A( p2 J+ _4 o2 q" [
- derivative = Err - previous_error;' f# a0 P! c5 I6 _
- output = Kp*Err + Ki*integral + Kd*derivative;
) W6 f1 h6 k! A. h& ~ - moto(int(Speed-output),int(Speed+output));* i6 M& K; D3 Z; [; K, t- @8 J
- previous_error = Err;
/ |7 `6 S, a4 n3 r" U+ `! y - }
, W- \" \4 j! I+ D' E: P - }while(!((lightsensor_2.read()) < (10)));1 t6 |) \& m+ ]! F, @4 |
- moto(0,0);+ d5 i9 c: B& Y X/ C( W) m1 f/ [
- delay(500);
3 k. v; p1 i+ U& R" R8 t - buzzer.tone(262, 500);
" d0 m0 [1 V7 u3 b' D - }
, m6 t' A7 X2 {6 N, a: H$ }+ h - : Q( F) f% b, f" I g5 C$ D' x
- int getErr()
) m7 U" |& c5 y' X* {2 R. { - {
8 R7 S9 y! T% U* _0 J - sData = linefollower.getValue();9 T( U+ g3 ]3 p6 T) L. K
- D1 = ~(sData>>0)&1;
6 O, {5 O) a- G+ q# m) J - D2 = ~(sData>>1)&1;
: M- {% j0 c+ t. \; @7 B - D3 = ~(sData>>2)&1;
+ V. k3 N- [. W4 M( M - D4 = ~(sData>>3)&1;( L! J! b1 M0 d3 H& n/ P& w; x
- D5 = ~(sData>>4)&1;
7 M" b' B2 R' E Q - D6 = ~(sData>>5)&1;
/ B1 o6 T$ s/ N) C& T - int downD = D1+D2+D3+D4+D5+D6;
" Q4 T9 }- ]3 w0 Z3 [; b% T" j ^ - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
: h9 s G: w Q$ Q) G - if(downD == 0)
. @4 Y {, m$ ?% I; e! U - {. h$ n4 b+ V2 U1 ?1 L+ c
- return 99;) _% ?4 _, c7 S/ d( n1 o5 g0 K
- }* f( B3 O6 H% d( M
- else
' Y6 R+ P* y( T1 ? - {
$ \ l ?1 ?5 N0 n& P$ o: h! N# F - return upD/downD;
8 }3 u4 p& ]1 f5 t3 X - }8 } t, Q9 H/ q* k5 h! [4 R
- }. }9 j" F& |. @( s" i
: d2 U# v; s0 V: m4 M& S- void moto(int Speed_L,int Speed_R)0 R) R! }; }- o& A8 C
- {! q: S3 X4 y; A/ T9 Z) Z) ^( b3 M, p
- Encoder_2.setMotorPwm(Speed_L);
% p A6 g# }8 `% t k+ T, ^ - Encoder_1.setMotorPwm(-Speed_R);
5 v! m: T% @* E9 j3 n4 V - }
複製代碼
8 ~# t- e% b, a4 G- `5 sMeLineFollowerArray.cpp& f2 L2 X1 ~3 V) T' S6 Y" U
- #include "MeLineFollowerArray.h"
8 w/ d! y+ a7 L! W! [; u6 e - / Y! O2 o) S6 ]; f" m# l! r
- #ifdef ME_PORT_DEFINED6 K0 Y7 J( m. k, c( g( O1 S
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
# Y; `2 x* s5 g0 {0 i- E# U, _: T; c& I - {
/ N+ }5 H4 ]3 z2 M! @7 v
* v+ w. f! v1 Y- }
8 A v3 I z& L8 _ - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
$ k: D4 N. }3 D2 b - {
' h. s2 O) V Z1 y1 [& V - _DataPin = mePort[port].s2;9 K$ `1 l% C6 ^- }, _5 r
- pinMode(_DataPin, OUTPUT); 7 S# e: G$ d# ?# w
- digitalWrite(_DataPin, HIGH);
3 V& S. S" o: p2 G# s5 I - }
7 p1 i0 p2 L1 a: t - #else // ME_PORT_DEFINED' r5 P) {! I1 ~
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin), Q8 @; q( o. e# J1 P; S/ E5 s
- {
. t' I0 r+ Q7 J0 ~6 O6 {" e - _DataPin = pin;- u# T) I7 B1 w) _
- pinMode(_DataPin, OUTPUT);
: F g' G2 W6 S5 s2 k! D - digitalWrite(_DataPin, HIGH);
5 ~+ ^) Z x: \% G/ [) j - }
1 R1 \1 }# u# E* Q9 F* W - #endif // ME_PORT_DEFINED
& |7 J6 C1 y8 @1 } - , `& w& @2 i. Q6 i% `
- 6 n9 D. C0 R1 p- a* @3 z
- void MeLineFollowerArray::setpin(uint8_t pin)
) u/ R: O7 u8 }. k2 X# T - {8 V! H' ]7 n( d0 J
- _DataPin = pin;: ]' z' j* M$ p" b- f) E$ L5 ^
- pinMode(_DataPin, OUTPUT);
5 s2 [) B0 _8 u% p3 o! O - digitalWrite(_DataPin, HIGH);7 M& K5 k+ h* f/ b2 F3 S1 n4 p, w
- 6 G: {0 L1 R. _4 K, X
- #ifdef ME_PORT_DEFINED
/ r7 B R i' c, H - s2 = pin;9 P1 ^; E! g8 v3 F
- #endif% }, f! [8 P! ^ M
- }: L' z% V! ~# ~4 `6 I
- 9 n/ F8 f, A, a7 u" R
- uint8_t MeLineFollowerArray::getValue()/ [! J5 S+ s4 C) E3 b
- {
, m e0 h' W6 y& h9 l# v$ T - uint32_t LOW_level_read_time;( N% C( U" Q; j- r0 E" D
- uint32_t HIGH_level_read_time;
- a# {5 z5 _5 K$ `/ o' v6 S - uint32_t time_out_flag;
7 W' x N+ U+ t! D! }/ T" L - uint8_t Sensor_Data[3];
# g0 a& B3 u# f2 w4 X+ [/ Y. E: K: } - static uint8_t old_data = 0xff;6 P; _" A8 B% }5 `( R" U5 T$ q) R
- Y; x# y% c, f4 @5 a1 J1 c0 z
- pinMode(_DataPin, OUTPUT);
3 k; ?8 W! b+ y6 p - digitalWrite(_DataPin, LOW);
( ?8 ]' D/ ?$ b* {& _: P$ { - delayMicroseconds(980);
% i9 c( k. r# p+ ]0 l& E - digitalWrite(_DataPin, HIGH);2 I# u @1 b( ]5 n+ ~
- # F) Y- E) z# [. R: O
- pinMode(_DataPin, INPUT_PULLUP);4 J z$ W* j6 b6 P; w
- delayMicroseconds(10);2 k9 F9 H6 E7 \& c$ ~8 S
- + d2 r3 f4 M7 K" S0 V
- time_out_flag = millis();
$ K) ?5 Q0 _7 S' F( b6 @; y - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );7 t9 S( H# _; Q' n$ c
9 m3 h8 b# a6 o" a+ [( G- LOW_level_read_time = micros();6 `2 \( p9 d6 I2 k
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 `6 {) o" N9 r - {9 \6 i) x7 x' J* c2 h6 e
- return 0xff;
# e, Q. T- W+ _ - }
$ b! o% C- H5 E
! ]' N- d! B5 a5 V7 o2 F& L- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' J2 V" @1 ]+ }9 S# E6 E
: _0 p% r) ^+ K, d- HIGH_level_read_time = micros();
4 S& v3 Q0 M+ h$ a& { - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
. Q( \) |1 w8 P( e; S, ]" n - 9 t$ F, q' _% W, R. F& e$ g
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
H, g0 D- N- ^3 |! \ E - {) ^/ g$ L9 T0 f( M% F
- return 0xff;, F- `- [1 w6 [8 M+ Q7 x
- }
0 \0 y3 f" Z8 e/ i; V
% p/ ?7 E" {' B( f- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
a6 v' q- ?. F4 h4 [ - {6 b! F% J1 Y* g* |' T
- return 0xff;
3 l! G0 y- p8 l" y" t. M0 \ - }
: A B! g' [( _+ |1 o" g* I
1 O1 V- S* ~; D: ?: A" |% C4 q, z- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 S# u& H4 b! v c9 v8 e - LOW_level_read_time = micros();7 P& }- ]; C7 }3 S2 Z3 @$ x7 J
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level+ R: k" a* ~0 a" @
/ E" e- u$ ~7 q- [8 V N, q+ G4 Q, `- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 C0 t- t* B# p! t4 b' A1 D
- {
' e0 l5 M- a; l2 h) k - return 0xff;
4 f1 j7 y) n# m1 f% U3 m! O. _5 Y; C - }; a# u- l: {; l7 U4 @
- * @4 W* H z- [. ^+ A
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))9 `* l! w- |2 x) C. X) I1 d6 H
- { {* a- O' H8 `& k1 [, Y8 I8 y5 d
- return 0xff;
; I5 a6 C+ t# W$ b9 x' o - }1 |; H$ _9 U. H; ^) u
- 4 z2 M' j8 h( A5 ]2 [' @: z
- for(uint8_t k=0; k<3; k++)# U, v( u" V/ h% j! o$ x
- {- x/ K7 D$ [$ d. B5 m
- Sensor_Data[k] = 0x00;9 I( x6 H8 p' P; ^" x+ R- O
6 T8 N4 I* Q( W" _* K- for(uint8_t i=0;i<8;i++)
5 U0 \0 F, X4 ?2 R Z - {
: m* i) Z' z. V- n* \5 X - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
( a, }) X) T2 }9 P3 E6 `- a - HIGH_level_read_time = micros();
8 C+ C, e% M% i$ `! |# w% p# E, N - LOW_level_read_time = micros() - LOW_level_read_time;7 Y6 G6 \7 b% W- [
- + J4 q& |7 |1 w: l A
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
# O' \" Q2 _2 ]5 I7 Y% d: F' l4 D - {
6 f9 l$ w0 I M) F - return 0xff;0 X$ Q5 k" d1 G4 ]
- }3 {& k7 `, U9 s: l5 f6 \# b
/ g9 a( R) D7 K, Z+ q- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
1 _3 l( S5 Y( ]5 B5 x; b - LOW_level_read_time = micros();
0 R* F0 ~+ l' H. x4 d - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
! l+ R1 o! W0 j' y: k - 8 ]! g: m- Z8 D! o) t/ T
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
{) `0 L3 e% S, e5 O - {
/ W. r, _9 ~( J9 M# G - Sensor_Data[k] |= (0x80 >> i);
( F/ y. z2 J) F1 o - }/ \- J) ^+ e- E/ b( r) N
- else if(HIGH_level_read_time >= 100)
% {' P5 M/ _1 R' `; l4 n9 \ - {
0 f% `8 s2 h+ F. Q - return 0xff;
' d) i) N+ P& ^ - }
6 W9 [# f2 c& b) u) K- Y - - v" m( G' ?% [8 ?; s* M7 g8 J
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)1 N* T2 L6 e/ y; B$ K. N
- {0 C& o6 h4 Q* W$ {) G
- return 0xff;
a% N- Q( m' s, h' g4 _0 M - }
; |6 ~% }3 Q9 Z( B5 }5 O5 z( ` - }+ `) t; Q8 l" X
- }
9 Z8 h* q- D6 v3 [ - 1 W2 y+ J }: T' D
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
' z" I% {6 j) [+ r* N" w - HIGH_level_read_time = micros();' F5 c6 h, l7 W. R; Q1 a
- LOW_level_read_time = micros() - LOW_level_read_time;
4 ?0 y1 `4 K2 B0 g( a& a; k5 F - ) ^2 u" k' C5 e$ V) L
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
9 f. u, H% ~0 e7 M2 r2 c0 j1 g - {
* e) l" k: R0 j; O# q# @, R - return 0xff;9 H; [- f" i3 j, A6 c( Y
- }
) W$ A+ W! A6 G6 q - ' W. t. d6 y) |. Y
- pinMode(_DataPin, OUTPUT);$ k) P" W2 ]/ o6 Y* j( Y
- digitalWrite(_DataPin, HIGH);0 D% e' k4 `9 C) _- ^9 W
( T. P, y9 p3 D* S- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))3 {2 B0 H, W3 O8 u$ l
- {
1 B$ ^3 f2 l' B8 w8 [* c l - old_data = Sensor_Data[0];0 H U6 @$ y4 L. C. {# W5 i2 E
- return Sensor_Data[0];6 W2 u9 S9 \, ]: z# D# j3 K* r
- }$ B0 l. P. O" }
- else f |, O; C: H; @
- {+ z7 {/ ^8 ~7 F$ E$ g- l) b
- return old_data;
0 c5 g4 O$ S# P - }
$ ~3 Q# J# g2 V7 Q, U1 t - }/ J5 [: R4 j5 r0 M( R8 N& ]' B
複製代碼 3 G5 b% w- Q! z
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
5 Y8 Q7 U5 g8 J( l, Z# T3 o2 n2 N4 n2 g4 f8 X
|
|