|
|
( D$ ^. A% ] p& w- #include <Arduino.h>. H( M" J7 I. g7 F3 c9 ~- T8 ~) p" N
- #include <MeAuriga.h># b7 ]: f& p4 I5 w0 y
- #include "MeLineFollowerArray.h"' v3 ^7 Q! I# l# Q/ v9 V
- r9 G5 k' I7 Y+ u# R: ^
- MeEncoderOnBoard Encoder_1(SLOT1);' g* T1 a& g& {- V* l3 P0 b
- MeEncoderOnBoard Encoder_2(SLOT2);' \/ X7 B! U0 P- a: A7 c
- MeLightSensor lightsensor_1(12);) r2 Z% [2 g& R/ I+ f; {
- MeLightSensor lightsensor_2(11);) J" @* s4 d9 h& G
- MeBuzzer buzzer;
2 {) Z6 Q* o( Y: Y7 A9 {: a9 z - MeLineFollowerArray linefollower(PORT_6);
- f5 v6 l2 J b; N. X5 g* h8 I - - F! w0 t0 M I7 c% R" V" d
- #define Error1 1
- l/ ~1 l" e( @8 ~; a - #define Error2 2
% k0 M% J3 r- y$ s - #define Error3 3
% v. q( `; E4 o/ b* E- H/ }2 g( L2 `+ `
3 g/ ~4 s& W7 K5 E- #define Kp 152 R3 i4 k. v7 X4 P, ]
- #define Ki 0.15
& Z) {7 j' N0 v, b4 q! t - #define Kd 0.03
, K) p8 t5 ~1 C
, f, Y, H1 q) l- uint8_t sData;7 r' ^8 \+ Z5 ^6 O% Y1 ?! [( B$ [& ]
- uint8_t D1;( r1 B5 Z: Y' s D( r
- uint8_t D2;
, g( H& v' |7 n* D. I/ j5 ~, A - uint8_t D3;
3 A9 W8 `% v6 M% p$ c" v - uint8_t D4;
- ~+ a, C4 d* q8 ]1 [$ F# }/ K* j w - uint8_t D5;
, d& w3 h. \% C - uint8_t D6;+ {* t3 C0 t: b* t1 `: w
0 V# V y( v7 ], h1 l: k- float previous_error = 0;
" v' Z2 _8 [4 a& A# N - float integral = 0;( @; v9 C, n3 f- Z. ` i$ f7 O
- float derivative = 0;9 N4 u8 a1 [ S' d9 C
- int Speed = 160;
h0 s. w0 a/ i% |) r - float output;& e& t/ U, W8 @" U. |9 A! g
8 c, L H0 b! P$ u4 f# c( n- byte Left;
' {* _9 o( Z, A2 J" R - : F2 x1 l" V& U9 N0 O4 q* ?
- void setup()
" M H5 E; g: d - {
4 a, _# k* Z0 r2 U, f6 `/ J - //Set PWM 8KHz
7 E& w0 c. }/ ~9 r+ Q6 c - TCCR1A = _BV(WGM10);
- Q+ f- x" M# c - TCCR1B = _BV(CS11) | _BV(WGM12);& `7 h5 E! v9 V- |' B
- TCCR2A = _BV(WGM21) | _BV(WGM20);
6 K/ _0 e X% D9 H - TCCR2B = _BV(CS21); F ^5 q. ]! u# A2 E3 F
- Serial.begin(9600);" d S/ h7 w1 [( |) S; H: e5 a
- buzzer.setpin(45);& Z1 x2 `3 c: R3 m5 U* m0 Q2 j
- }% d+ s; s P0 z4 ]0 S" |
- # J- |: m/ D; n7 q" i" N
- void loop()
4 u0 o/ J+ Z7 t$ a- E0 I6 y - {( A! \# b1 x! j: `# \! Q
- while(!((lightsensor_1.read()) < (10)));
# l! |# ^7 l4 H( D* J - buzzer.tone(1047, 500);
$ c! j; l" U, A4 x - delay(500);2 I, }" A6 a: T" h9 O! q! U
- do
6 I$ y- D: [8 }9 a2 ^4 F4 I% G - {* I) l9 e8 t& }
- int Err = getErr();
0 S4 h. h9 P& y! t4 N: l9 P T - if(D1 == 1)
. J. |5 W2 R p- h8 L - {9 w$ k/ o" L/ W& N
- Left = 1;# M t C" A' Y/ L& |# k: }$ G
- }
2 N& ?. k# {8 z - if(D6 == 1)) d C7 f' ^2 k! F1 ?, @4 [
- {" g# c$ S' f6 o4 y4 I, j
- Left = 0;, f- N3 e/ ]4 C3 q! e
- }
0 d: a6 p/ K3 {3 c/ R - if(Err == 99)
# ~* j, \2 d/ P! E7 ?" q+ [, k - {8 \/ J5 k/ B* O% ^8 D
- if(Left == 1) r: q) O$ q+ h8 ^4 `1 u3 f9 ]
- {
2 |1 u! h# q* h6 r* b$ H- F( a - Speed -= 5;5 N6 @0 H5 p$ i, X# \$ g L
- moto(0,Speed);$ W) i* l6 }# s3 w0 V5 A2 ~ p2 W3 ~
- do3 i# @" h" d n1 d
- {2 ?2 r3 u! p) A, Y& E; c" Z
- Err = getErr();
! M& |1 B' L U8 j9 p - }while((D1+D6) == 0);& |/ ~( K% e/ r4 H% f8 ?
- }
9 _, H6 b1 [2 f0 q: z" N, _1 p u - else
- D1 P0 Z8 C. q2 k! @) h - {
: K' F* i5 x/ j- s+ R% L - Speed -= 5;
; Y6 a& a3 n" J6 Q( s! Z Q4 M - moto(Speed,0); % k- ~3 w L7 L ]& k+ E
- do
+ A: [; Y& w& K& p) c - {4 p. S. Z. ]2 t) ]# {2 ], [% S
- Err = getErr();8 `2 K/ j0 V' l; m3 R( G
- }while((D1+D6) == 0);
8 h2 J1 Q6 K m( G - }
2 B8 T) A* M. J1 u1 x1 d) V5 B8 R - }
: k4 y/ s# `3 k$ t* { - else
( |" ^0 n1 s* `% ?+ I3 R1 G- D! o B - {
5 @1 x0 q( D4 U, ] - if((Speed < 160) & (Err < 2)) Speed+=1;
0 ~" L$ s9 J0 V% \4 U - if((Speed > 100) & (Err > 2)) Speed-=2; + l' p8 u0 `/ \" u6 p8 h
- integral = integral + Err;4 r% z$ P! R9 c u6 R/ }
- derivative = Err - previous_error;
7 N1 ^8 W) L3 y% i5 | - output = Kp*Err + Ki*integral + Kd*derivative;
8 x5 k! O ]0 q/ D" b1 ~. {9 Y - moto(int(Speed-output),int(Speed+output));4 C4 ]" h( I$ w
- previous_error = Err; b* y# v& @2 t
- }" [& n8 e! X h" }/ ^. d6 z; f: M
- }while(!((lightsensor_2.read()) < (10)));+ K' D4 S5 B( ?
- moto(0,0);2 ?" E6 H1 e# a, c
- delay(500);- z. F& l% x+ ]% k
- buzzer.tone(262, 500);
5 k K& ]3 _* o7 e - }* C$ f, ?8 ]+ \) C9 z6 V
- 9 j# Z6 G- F6 @" C
- int getErr()
! z& [% J3 M/ T9 Z& E q) u - {
6 m0 n) J; ]4 M& V: w: N& W* e' ` - sData = linefollower.getValue();1 W' V5 K. @- y( {
- D1 = ~(sData>>0)&1;. T g% P @, B n) M
- D2 = ~(sData>>1)&1;
! F1 r4 V# D+ [7 | - D3 = ~(sData>>2)&1;
, E# @1 H- O9 \+ R$ ?9 t - D4 = ~(sData>>3)&1;
* H2 r1 b- n1 r0 i }6 `: K, X - D5 = ~(sData>>4)&1;
8 A9 M g$ q5 @% L4 h+ [" _9 p2 D - D6 = ~(sData>>5)&1;2 ~, A% Z- X. J+ F, Z& E. E
- int downD = D1+D2+D3+D4+D5+D6;- I) y$ R( S4 ^7 S' c p: g
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);' e. J# Q2 `9 i2 [; }* K2 f( e% N
- if(downD == 0)+ b9 c- D' u$ N& p n
- { G; |2 N7 J8 q5 f; p' O; n
- return 99;
) ^; M1 Z! V( Y& h - }
[; v; O8 G/ y: \ - else9 w5 r1 Z+ O- A0 Y) W) w( G
- {
# u) w/ x3 `4 p" f5 T- s - return upD/downD;4 G `+ ?( r; I5 X5 s. n
- }
8 Q) l/ y. U0 J- E( U! { - }* i- A. K- }6 ?* I+ s: ^/ ^
- 4 U) y f; y5 B$ w$ W
- void moto(int Speed_L,int Speed_R)0 |* J1 \- |8 k2 K& r3 E6 v X: ~
- {
0 F) d; d+ a; u3 l& _& T0 x - Encoder_2.setMotorPwm(Speed_L);* F. ]$ [2 n5 D: G- F- t: l2 V' U
- Encoder_1.setMotorPwm(-Speed_R);
" S# S2 h. j. Q( O5 j/ ?1 A/ A - }
複製代碼 & y: J w" \, P. }. D
MeLineFollowerArray.cpp: Y% M5 [' {2 O
- #include "MeLineFollowerArray.h"& W/ G5 T$ z6 c) i
6 `8 }1 s; z; o9 M2 R- #ifdef ME_PORT_DEFINED
5 \# J! j8 E; |9 t4 ]. l - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)2 _; _; H- ^( Q6 H, X
- {
/ \, F) s, b A/ y# U - % f; w' L, N! c. d w3 _) c j
- }' N C$ J: \0 I. G/ @
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)& H7 s/ D" n; \/ \5 E
- {5 w+ H, G) t+ N; {1 F
- _DataPin = mePort[port].s2;8 w$ P9 L; C9 K: S& h$ C
- pinMode(_DataPin, OUTPUT);
a8 h! ]) H; s% c - digitalWrite(_DataPin, HIGH);3 w6 P0 Z7 D. X. T1 |
- }
+ C; }# Z' S! p% J - #else // ME_PORT_DEFINED9 p5 o* O# z3 I) n
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
/ @2 T m$ S) ~; p. d# { - {" @! e$ L* ?, D( `* n
- _DataPin = pin;
( z) [# X' [: O5 @ - pinMode(_DataPin, OUTPUT); 1 ~9 g7 ^0 k& A/ r U
- digitalWrite(_DataPin, HIGH);* _( J0 h4 Q6 K! u3 }# j" I
- }
) s5 t8 M3 D V* A - #endif // ME_PORT_DEFINED
+ p- p) W8 X {1 h: ?
" s( \6 z) u* i7 ?# u" [2 {- & x1 b7 o3 Z. N, W! X1 m3 f, ?
- void MeLineFollowerArray::setpin(uint8_t pin)/ |- B$ M" e% J L
- { j- L: V, B1 k. a2 R; u
- _DataPin = pin;
' E+ V- N; y; z0 ~; \ - pinMode(_DataPin, OUTPUT); : S* `7 z/ B6 r* z$ W0 [
- digitalWrite(_DataPin, HIGH);* T8 r0 L! ~ n4 R' C2 z" R5 C
- $ H% a+ X8 O3 f- m5 x
- #ifdef ME_PORT_DEFINED8 X7 t1 @# B ?8 |$ M) P8 Y
- s2 = pin;
- n+ g$ T( F7 R$ [% L% C! v* K) M - #endif! Q; I5 m4 G* x: {9 ]! h. A
- }
" ]- a; b. n; ~: H+ R: P- z - - W9 ^) R: A. @ S$ ?/ g
- uint8_t MeLineFollowerArray::getValue()* A* @( V/ O5 X+ B: j
- {% ~( c/ F9 S; k1 S
- uint32_t LOW_level_read_time;2 {( V: N1 k" w5 S
- uint32_t HIGH_level_read_time;& e% {: f5 }" _% f1 g
- uint32_t time_out_flag;3 y0 Z. X* h1 }5 L( g( A3 {5 }
- uint8_t Sensor_Data[3];9 i6 P% V6 A5 l$ C3 d9 z" S( @
- static uint8_t old_data = 0xff;% I4 _9 x3 ~, R7 i
4 P b- d n! m" o0 S- pinMode(_DataPin, OUTPUT);
" t5 h! L. K6 n+ M4 i - digitalWrite(_DataPin, LOW);
; c' T# h1 ?/ C5 W - delayMicroseconds(980);
m O, q1 y( O/ \8 T - digitalWrite(_DataPin, HIGH);3 N- }) F" H0 F; m
- ) t# ]7 D: C, e- P0 P
- pinMode(_DataPin, INPUT_PULLUP);
9 \- |9 r1 D" V0 i' }) u# t - delayMicroseconds(10);
5 ~' _8 T" I' m9 k _
0 Q, U. M0 g( J$ r- time_out_flag = millis();' F# @8 T6 W+ H7 t/ f) a0 f G: p. U) w) G
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) V% a$ G0 o) X7 `& ^5 F0 ~! s+ C
: q/ |* N/ _' _6 J$ b- LOW_level_read_time = micros();7 }/ _& l6 U# Z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out4 _! p( d' g8 X J
- {
, q/ ^7 I$ z$ d0 J% X2 ?/ F - return 0xff;! N& ?7 r0 k1 U- |
- }
( k& }2 g% u/ ?# M9 n3 W - % `2 ?' e$ Y$ P) X7 y
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 e$ v. V; M/ G
5 d t8 ^; m# V& V4 K- HIGH_level_read_time = micros();" T3 m8 h1 T2 S: k- j) |) C3 S
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level. r! e, i$ T( O2 f4 M
' W4 n2 Z2 y a. q- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ M: V* X1 j6 G" j8 ~4 D
- {
4 Z! |" L. z$ \$ H# ]& M' O7 }; N - return 0xff;7 e1 b7 r4 D+ s$ h
- }
3 A4 A: \; Y v5 |* d4 M
( b, X- I3 k# p. L- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
% J" ~( O; q K; r4 a - {
9 x( l: n1 z2 L - return 0xff;
5 X) Z7 Y( w, u/ p - }9 H1 G+ P8 X# t3 H/ o/ o
- ; H: o0 l0 b5 Y
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );% S/ Z+ o) A* D0 ~5 @; N0 @
- LOW_level_read_time = micros();
, \4 y: T7 e8 v; b2 q- T/ }' | - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level0 v( A5 \ L2 O* G! c
+ u0 J, I ]: C9 y$ r- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out8 K p6 C4 K6 N w: q
- {
' B2 `, k/ f4 l+ A - return 0xff;9 o+ a8 k7 i0 F8 m$ A. T K2 l M0 R, y
- }2 p" H; S/ d( d% }7 s
' c! g( t6 s+ V0 y- q: t. C7 o1 j- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))/ l. s1 D* H& e% E8 t2 i
- {
8 I' M r6 A. R3 w+ J4 J3 X& a - return 0xff;
: N3 v' z8 J- e& l+ f' t- [ - }0 \. S$ D" G! Y6 D1 ]& h- P
& M- F. B' ~; P; p8 Z# W- for(uint8_t k=0; k<3; k++)
* A+ _( f) d' A' k, O1 E0 O" V% v - {
; h& ~9 V, L- y) D9 n* [* Y; [) `# g - Sensor_Data[k] = 0x00;
6 R! F. c, T( O. s5 g - + v% Z3 W- u5 ~5 o3 W
- for(uint8_t i=0;i<8;i++)
( m( p# z4 @" i+ _ - {
; ? B( y% s( d- A - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level% |2 P5 C @$ F% k7 O0 i& K
- HIGH_level_read_time = micros();4 G9 M5 S8 t4 V7 W
- LOW_level_read_time = micros() - LOW_level_read_time;
0 e3 [: U% ^9 g# t5 }
% p8 G! v4 O, q- ~$ p* L- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
% ]0 t& J5 ^' Q2 q: T+ V# F! B - {
! b3 @& u6 h# T& D6 X2 n. ~ - return 0xff;: W8 p$ ^( R4 [! G! N
- }
$ q g. m) e3 c) }# ?1 n - . L& K) B6 V2 f7 D; H& M' P
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 [$ @( p" X9 ]
- LOW_level_read_time = micros();
2 b8 h# H! _8 ?5 b; s$ r - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level! w$ g; w3 h* f& Y! h& [- @+ a
2 h# _/ ]& i' T1 P2 F/ w( M+ \- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1! z" U. W6 }9 N( v' D* J P" ?
- { y L6 p$ r! n0 s$ U: q' }, o
- Sensor_Data[k] |= (0x80 >> i);
" {) x5 p# b5 l) p' m" K - }
/ O* m6 x& y$ N$ M - else if(HIGH_level_read_time >= 100)
, @' l" ^3 l& M0 g - {
" S3 @" K. b# c7 M H - return 0xff;, Q& ]1 U# T8 i {. J* M
- }4 j) E2 T) t: ~; D. b& k
- ! C3 [% A9 q; h2 q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out), C! ~9 L. x% |. n' G
- {9 s+ O3 X+ n# C
- return 0xff;2 S5 W( f2 }; d) _+ {
- }
) _% k% _0 ^! Z1 { - }
: f5 Z( ]. b( _" j5 ^0 }4 u - }
[" k8 Q2 [. {; X& L1 Y8 y - % B% T, A, f; E* x4 j/ Z
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level7 }% q: B* I- ^% G9 L
- HIGH_level_read_time = micros();4 P5 e" _& ?6 u& W. w- Z+ p
- LOW_level_read_time = micros() - LOW_level_read_time;
" W; X1 y/ Y" c* l0 }# ]8 z/ V
: M; {* ?, U# H4 v$ _6 |$ P' Z5 S9 i- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
b, O/ d+ `7 B+ c. b: s - {
9 t# x, V! w( q8 L) d9 z6 B - return 0xff;5 {0 B+ D# w' e& X: s2 _4 ~4 |! }. \
- }# g; m( {" g9 x c4 E: ?) P( V
- ! f: C, Y- w- G- s
- pinMode(_DataPin, OUTPUT);
0 M; L. x S* w: P/ g, y - digitalWrite(_DataPin, HIGH);8 D5 n! j/ n s/ J3 i/ v1 @/ V" M
8 q. s+ p8 Q1 g6 ?9 }- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))2 i: t7 N$ i( O) O; M4 H& @$ R% t
- {, b' s5 S- x+ P+ F ~! n3 M; |
- old_data = Sensor_Data[0];* Z- r- O" t+ m
- return Sensor_Data[0];
& N0 f6 t9 V3 d$ p7 b - }, V" [4 X' f( M9 z4 K
- else
6 W1 B/ E' V* z6 @, y3 R - {
" R) o# \3 A! J! k3 e; a3 ] | - return old_data;
$ a3 }/ V) A" t% t - }5 O' X4 W: e+ t8 K& z
- }
0 @1 r! Y$ R; O u3 i6 ]4 y
複製代碼 ) y% q4 }3 u; x' h% t
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
6 x# ?$ ]4 O! d& k
6 c: x- v. |3 o |
|