|
|
5 g/ F6 O2 Y9 E0 P7 B( b+ E. n, p
- #include <Arduino.h>. }0 H+ J5 @+ L5 I/ @" L: g
- #include <MeAuriga.h>/ ?/ I& I- o7 s1 Z% Z. S
- #include "MeLineFollowerArray.h"
+ i4 G7 l5 U T2 \1 m' S
% u( l. v5 y1 r2 `0 ~# `- MeEncoderOnBoard Encoder_1(SLOT1);" s9 ?* ?- j# M. V( r
- MeEncoderOnBoard Encoder_2(SLOT2);# U( Q7 Z' ?0 H% M" B! d
- MeLightSensor lightsensor_1(12);% w, s9 J$ @; m+ C* v
- MeLightSensor lightsensor_2(11);: j/ [$ j, J8 B
- MeBuzzer buzzer;# W! j9 o4 X( t8 K$ }
- MeLineFollowerArray linefollower(PORT_6);
; u! e3 x6 r. l! ^: Y' o - , _: B/ O& V/ |3 ^
- #define Error1 16 m) `0 p$ q: f+ F1 @! t1 l
- #define Error2 2
" z* L2 E' V# Z: O& w' _ - #define Error3 3
" `* `7 u- O4 t: _* f* Y
. @0 `* K, I$ p. T% I! H* f- #define Kp 15
& g1 U) J9 U+ t( a - #define Ki 0.15' G* j3 D; s; S
- #define Kd 0.03) G A/ u2 o7 o, m
8 Z& M1 t5 g/ N4 K3 e8 y3 ]5 F- uint8_t sData;
$ x% Z' h+ Z% ^# K/ U* @/ _; w - uint8_t D1;
" @4 O4 p9 R) v% r - uint8_t D2;+ I* y, W3 b, o Q
- uint8_t D3;
3 i' y; z/ H5 S5 d, ~ - uint8_t D4;( j* i; O3 M9 W' M( y* |7 O
- uint8_t D5;) {7 g2 O& ~( g' O5 S
- uint8_t D6;* M" T* }7 H* G: C! |, Q
- + K$ u& s0 n; u0 K1 ?/ b
- float previous_error = 0;, [ _* P& \1 W. }4 ~
- float integral = 0;/ W( Q- P. i0 @# l
- float derivative = 0;
, x; g: }6 t1 p& I2 _; j4 ?& J - int Speed = 160;7 k( B* {0 \ C/ q) ^; d2 s
- float output;6 `- _9 Z1 Q# v5 w. T! U0 ]9 k
6 d. k( a ~% t% B5 q- byte Left;
9 b1 _2 c* X' z1 V: z1 i
, g; Q$ R5 ^0 T& b2 Y- O- void setup()
/ K! c5 ~1 A' c6 v" | - {7 j3 Y& R5 `5 @2 c k6 f, y
- //Set PWM 8KHz
% U* J \$ u/ d2 I3 S - TCCR1A = _BV(WGM10);# @0 ~2 Y$ F8 ^2 E( N
- TCCR1B = _BV(CS11) | _BV(WGM12);6 c; L+ |( F, n# _
- TCCR2A = _BV(WGM21) | _BV(WGM20);9 f6 ~$ V* J, g, k. r8 g+ }
- TCCR2B = _BV(CS21);+ x" [4 ~6 J+ h" d$ H9 t( v
- Serial.begin(9600);% g1 _) k) m3 v
- buzzer.setpin(45);+ u0 ?0 ^7 Q7 r) _
- }
$ g9 Z3 m$ ]/ o6 y0 X6 }7 x+ U
" n4 M% s a1 K: H- void loop()
2 B" ^1 h) q2 v: r - {7 {6 v4 ^% R" o7 R& T
- while(!((lightsensor_1.read()) < (10)));
4 l5 K) m! L* u# `, c - buzzer.tone(1047, 500); h) d5 a B6 K" P/ Y
- delay(500);
" d/ _; B2 j O5 e - do
' a: ~3 R4 e! e) ~1 y( y - {
. }6 b2 [3 B6 w) I - int Err = getErr();& m" w1 m; U O, c- d
- if(D1 == 1)+ q; v# \1 E5 A( }+ {" k# {. \+ ]
- {
7 c- C$ D1 j* e2 V3 ~ - Left = 1;
; P4 ]/ o/ Z U9 l4 H - }5 o4 A& ]( N! w9 D& r
- if(D6 == 1)
! E) ~! f; o3 _, \6 m6 t - {. ~- Y) o T+ |, A, u3 m
- Left = 0;4 c( p8 F7 z( X) g: t d5 C& E
- }! a8 U0 s$ h! G& f: D
- if(Err == 99)
; A$ u3 i, _- i4 j - {) D4 c5 Q, H! B6 I% z" c
- if(Left == 1)# G1 t" }2 ]+ W8 i0 U
- {
: w; v3 {) B6 a+ O) S& ^8 A$ m1 F2 n; ` - Speed -= 5;! m; C% J, v6 I- A
- moto(0,Speed);* q4 h' e- m5 }( P5 i7 I
- do
5 J/ J! ?# c) j: U - {
& s' k2 M5 t$ \4 }. H( U2 V8 ~ R - Err = getErr();
! q* z" l4 ~, m7 B# \ - }while((D1+D6) == 0);
A8 f9 J4 ]4 d' A: h4 w - }: X# R3 k K/ R/ E, y! r4 G5 s7 ~9 W
- else
8 D0 f" c F" z9 B: K1 P - {. d9 t9 G) f; Z+ S6 e! ~
- Speed -= 5;1 W \: d6 F; M! E8 X
- moto(Speed,0); / i J' y0 c9 I1 \1 w1 T
- do9 J1 E, o: i1 k% _
- {+ E9 y) b7 ~& `/ P# l
- Err = getErr(); m, n) Y1 F7 k7 ]/ N
- }while((D1+D6) == 0);' z8 I S" y, l
- }" B' K, R0 d' N X3 ]7 ]# H
- }
) s5 ]8 A8 E* D& H9 Q7 n - else
5 {. @! x S( A+ A - {1 o+ k7 G' d, b. A
- if((Speed < 160) & (Err < 2)) Speed+=1;
3 {0 f6 X! I) Z/ T - if((Speed > 100) & (Err > 2)) Speed-=2; # w. K$ d$ U8 W
- integral = integral + Err;
) r7 Q8 v! ]& s) v5 a* G - derivative = Err - previous_error;
% X& L5 j- T& [2 h5 D - output = Kp*Err + Ki*integral + Kd*derivative;1 S" D! G/ v& ^ ?
- moto(int(Speed-output),int(Speed+output));5 `; Z' Y% S; R
- previous_error = Err;. |6 F, v% M! V0 t: W
- }7 P# K: M3 S; `# R; K
- }while(!((lightsensor_2.read()) < (10)));! Y- W# L$ W w
- moto(0,0);' ]0 v; P8 I# @! K" G
- delay(500);1 T0 Z \4 c, ~5 N
- buzzer.tone(262, 500);
% j. ^ M" [" k8 X9 x - }3 s! ]& K( u( `* C6 @/ O+ Q
- - r( M7 p5 D! M9 O8 X j7 n' Z1 A
- int getErr()6 q- I* g; |5 V- q6 h3 G9 p8 f4 _
- { 3 ?" p# ^/ p( `8 b( T
- sData = linefollower.getValue();
. W9 r8 C; n3 g( l( V' @& t - D1 = ~(sData>>0)&1;
+ r$ Y" r: j7 e5 f* i- { - D2 = ~(sData>>1)&1;
) b& w/ e3 F" E2 L - D3 = ~(sData>>2)&1;/ u- C1 B/ B: E* O4 d7 Q
- D4 = ~(sData>>3)&1;
! d; H) [6 Q5 c! `" ^4 _! n& K( K6 K - D5 = ~(sData>>4)&1;
$ \" J* ^# j, w9 c1 K1 B- d - D6 = ~(sData>>5)&1;
5 O4 U* @+ e4 Q9 f6 H$ ?1 J& \ - int downD = D1+D2+D3+D4+D5+D6;7 O" Y. X6 k4 p& w+ E) ^+ u
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);/ w1 O0 h9 R9 q& o; L+ X) |- F
- if(downD == 0)
3 @) f, F1 N5 f: H - {# e* E. k# P+ l. u- [. f G! ?- `! k
- return 99;* N$ ^, \4 q; {3 |
- }/ U" {+ T( L1 X5 N2 m& C2 s
- else
' h9 Z' Y( G7 b! Z6 ~ - {$ w# \$ a* f# o& O0 D
- return upD/downD;0 G& V# }" k3 g0 b, ?+ H4 J
- }
: r( O- w6 E' }$ N* B - }
; h( S: q8 u7 M6 h3 ?8 `- D" X6 A - 3 Y3 ]$ B: P% _
- void moto(int Speed_L,int Speed_R)
. D6 V' g/ ^8 O8 V: J! y - {
, h3 t: o; w0 i" T8 \; q - Encoder_2.setMotorPwm(Speed_L);
- W% n. I" u4 [6 ^ - Encoder_1.setMotorPwm(-Speed_R);+ H+ s# N' P+ [" |! ? m) Q
- }
複製代碼
$ z. ]( n1 [; r5 q: l- f dMeLineFollowerArray.cpp9 ^. u8 z) f/ P5 ?3 m- |$ z
- #include "MeLineFollowerArray.h"% v/ g& @/ k+ A j0 x7 x
1 W7 ?& K! w4 s9 U' x- #ifdef ME_PORT_DEFINED- l5 p8 _5 P1 A& H' Q. d1 N& g
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
+ v |! Z% p* g7 j" ?1 U4 D3 M D - {
+ e1 I i# C0 G* y6 f
. |$ e- W; R) u1 u- Y' W- }
, U/ o! J* {8 a- W; A - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
6 ]0 }2 s. W$ E# j - {! ?) B" C/ Z; F3 p. k
- _DataPin = mePort[port].s2;! F2 A9 _/ W C; ]% W3 O1 [
- pinMode(_DataPin, OUTPUT);
4 V( Q2 L& i& G. w z - digitalWrite(_DataPin, HIGH);
- q8 s' D7 d' W - }9 @# w3 K/ k0 k- n, o5 X3 q
- #else // ME_PORT_DEFINED- [0 x7 {: n. N
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
& ^' k. f: ^( w* g+ c& S - {
4 x% }( i$ M H7 d" Z - _DataPin = pin;
7 i$ o1 {# W& C. n! o - pinMode(_DataPin, OUTPUT);
, d/ _, W5 M! k, q; u - digitalWrite(_DataPin, HIGH);
$ H7 A6 A0 h/ w - }
& o4 q; p9 ^) N9 V5 g L# G - #endif // ME_PORT_DEFINED
9 o2 o0 y+ [. e+ v+ Q
\' U; |0 e& w. N
0 {, H; v/ g4 T* f- void MeLineFollowerArray::setpin(uint8_t pin)) N$ T, d* e! Y
- {
) i& }' }. Z& P9 Q- ]# y* k, ` - _DataPin = pin;2 \3 l- M6 k' M
- pinMode(_DataPin, OUTPUT); ) G- v! w w$ q8 v; V8 a z
- digitalWrite(_DataPin, HIGH);# p' v# e- @ |& L7 i1 I
- # A, M/ L0 }- M6 b8 Q3 m: h
- #ifdef ME_PORT_DEFINED: Y2 c; C7 u1 r n/ w0 N
- s2 = pin;
! v! k& ^( X, B! \4 ^ - #endif
* O& f9 c( H8 Q - }
; o' W: }# g/ I* w - 6 Q3 c' T* K9 e6 o
- uint8_t MeLineFollowerArray::getValue()& N( _3 g6 Q: H
- {
- S2 G4 ~$ L. k) O, _ - uint32_t LOW_level_read_time;
/ b9 n3 |; v* b) X, U6 X - uint32_t HIGH_level_read_time;2 N! n, r7 F5 i: \5 ?
- uint32_t time_out_flag;
2 T0 e7 E3 `/ H - uint8_t Sensor_Data[3];( j4 ^7 a+ X/ \
- static uint8_t old_data = 0xff;1 l W* {# ` ?1 [* S
- % C9 K6 c9 K) k- `
- pinMode(_DataPin, OUTPUT);! x0 f* ^8 n( `4 Q, I- G
- digitalWrite(_DataPin, LOW);
) Z" Y3 x# ~% t m - delayMicroseconds(980);2 z9 E3 E/ g; x* X! @8 X2 U
- digitalWrite(_DataPin, HIGH);
1 v0 t% ^, p. d6 z+ q$ i3 @ - 7 Q% |. U. I, j/ s
- pinMode(_DataPin, INPUT_PULLUP);! |, A' J& ]' u/ _6 \$ J" S
- delayMicroseconds(10);& C* T; o$ v _
) ?6 M* g6 d$ r, k, l- time_out_flag = millis();7 N8 }* \5 G1 n6 Z6 ]
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# p L: X/ l( i
. L! [ h; C* u1 u) g# S- LOW_level_read_time = micros();! w4 \5 b* f+ `7 e
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
' ^! y. J8 ^9 @ - {9 D; B. T7 r7 Q
- return 0xff;4 t% P0 J9 E% M+ D3 @
- }, S( ^2 q& R! N# l, b8 d/ \
- $ E p2 g* z, X1 t
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
* k; Y0 h- V+ h$ v3 @7 N/ i
- X7 v# l! n! A8 v! u+ e+ ? g- HIGH_level_read_time = micros();
! ^; I6 n4 m1 @; o0 [ - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level- X1 f0 Q1 T. d* }/ o) S
- * h. x5 S* k. G- E
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out8 W- c$ S. g( b9 v. ]2 ^3 G8 e
- {
! P8 y4 p! Y' d& h - return 0xff;3 Z. X& |, V$ ~0 k1 x
- }
. N$ S3 S X, W2 t- D
2 X; K% w, n% f5 {- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
$ K6 g: ]" ~' o v. l# k - {
# S1 ~; @3 c& r. [# z% \ - return 0xff;. g7 {4 D" t5 p$ N: U" z- s2 o$ K
- }; }9 z# A1 a0 o0 u& `$ |- |
- 8 k' D( P, c0 y% z; A- T
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 d& ^% g) q4 a0 h/ i8 [4 Z - LOW_level_read_time = micros();
+ B, M4 e* I+ X* F: Q - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level, X) P; e+ @: L# N% R; `- P
- 7 G- F5 ]; f9 v% F, C
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
( X( c p% U1 e' v' A - {9 H* U* _4 V) O7 V! z% h$ f8 j( ~
- return 0xff;( F# P: K" E" I# X- M0 P+ Z
- }
/ _ |! Z- |6 E- {( w
( m7 d/ k' Q1 w6 c) g$ r- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
4 d+ N: m4 a$ E3 Y2 j - {9 s. u4 C2 f- p0 U* G* O$ N
- return 0xff;/ ]" M( l6 j2 e$ R
- }2 U3 ?% y+ i2 }" F# X
- f1 K; h5 }+ ? ~
- for(uint8_t k=0; k<3; k++)
" N* A a+ d, @5 C t$ s - {
: v0 j$ S) A" D/ h9 H, | - Sensor_Data[k] = 0x00;
* S) B2 ^! N1 o8 O. d7 |7 d - ; ]9 {) X U$ z) @+ z
- for(uint8_t i=0;i<8;i++)! A; N3 j3 @+ A% g& X
- {- U. K7 J- q* T2 g
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
! e4 q" _' t6 q, F1 t - HIGH_level_read_time = micros();
- k+ K, ]& C% P* ?2 `% n5 m/ W - LOW_level_read_time = micros() - LOW_level_read_time;
' d# [* _0 f9 Q1 [$ f8 R - 6 c# _7 X/ o: n/ U; q" l' U
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )1 F1 |9 B$ P3 `! K9 H# e
- {/ | ^2 I2 [4 B r6 \# S1 ]
- return 0xff;
8 O& e6 b; Q" }# _. N - }: Q: }- h8 i' @0 k A3 f
& \, {0 R: l# s- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); p3 I' k& D% b/ b3 @' J. C( B' M
- LOW_level_read_time = micros();$ w* z, {! ~% j3 {3 N1 _" Z
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
1 G+ C; m" Q/ f" k
) R- ?( z0 l: p8 W. ]% I- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 15 H0 l9 J# g# h* i
- {
1 B! j: \4 s7 P, k5 n - Sensor_Data[k] |= (0x80 >> i);
. I; s6 D9 h4 L- [" g5 G& |) r - }
. s$ h5 f$ C- d, n! R8 k# Q% q - else if(HIGH_level_read_time >= 100): r7 [- r/ D5 u# s
- {+ N9 z' [2 J3 Z d- Q
- return 0xff;
# h3 W% j& L/ L/ m$ h( w2 H - }2 `. m0 ~- G5 V; Y
- & L/ a- `; A1 {' D$ r
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out); k$ i% F! ]: G$ [' q
- {9 w% ~! M! K8 K5 T" H
- return 0xff;4 C, R# I' f/ b8 G
- }
) V3 `( `- A2 ?! s) o - }
* s; y7 \, C0 R- _ - }9 C- H: }4 B; ^9 e& U: N( C
! R `! ]! b& T/ v- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level( [" K2 Z8 W; X% _# s7 m% _ O
- HIGH_level_read_time = micros();
; R9 s( I/ r. h# ~( g. q- o - LOW_level_read_time = micros() - LOW_level_read_time;/ ^7 R- a% s0 N7 o
- " h4 E0 X. G9 u5 v* |5 K5 P+ B- T
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
9 \2 Z7 n4 }# ~- S& R& Y - {
3 [2 C1 w0 u9 Z. c+ r, [+ `- D - return 0xff;
' Q3 l/ L5 n3 C - }+ N% d7 a7 V. d+ x2 c0 U$ W
- % P6 i a$ O. V
- pinMode(_DataPin, OUTPUT);
9 g5 u& J! h# |, |/ q O - digitalWrite(_DataPin, HIGH);( d: W' B7 h6 N. z8 L8 d
$ J. ]" L! n4 }9 ` h. B% _- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
- b1 x. O3 Q6 ]7 a- | - {* h+ U, e7 r3 j0 k6 z5 f7 p' m- q
- old_data = Sensor_Data[0];
3 ?& {$ E3 \+ S) J" v9 p7 U1 ~7 v - return Sensor_Data[0];* w# f8 G- [/ m9 B. |) i( X
- }5 x* V9 k4 Y) k! H
- else
2 Q1 S: O. j$ y8 Q - {+ i# u7 P8 F+ |6 y
- return old_data;6 {6 S. z7 ]7 r/ w; T
- }
6 ~2 y9 k# ]- e( h. ~! \6 j - }: \9 n1 l& l; R# Y: |1 k, V
複製代碼 . T6 t6 V) D! k
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
3 R& K2 u* e/ E$ T/ A: ?! Y9 x
. s9 _; J/ ~( r- p* N5 f |
|