|
|
% `$ p) ?4 X; C5 W [- #include <Arduino.h>; d. g- E+ S; G* X, [2 l5 X
- #include <MeAuriga.h># q( @+ L0 B4 y: K4 A
- #include "MeLineFollowerArray.h": a( G; M( p) {5 B' ~ N
( Z! u; e, T1 H9 I5 b, T- MeEncoderOnBoard Encoder_1(SLOT1);
9 G) p$ h' X8 p9 }5 t# V - MeEncoderOnBoard Encoder_2(SLOT2);- g4 ^; C5 G8 p1 |# `4 |
- MeLightSensor lightsensor_1(12);% S @2 Y- t1 l+ C1 @4 K# `7 n: t
- MeLightSensor lightsensor_2(11);5 s. X1 t" \; H! `8 U9 Z
- MeBuzzer buzzer;
z9 B$ R# S: z) p6 J - MeLineFollowerArray linefollower(PORT_6);
2 l3 t! ?9 c$ p1 b# r - 1 x! d7 h5 \8 h6 T# H5 _+ V9 U
- #define Error1 1
' j; a& l* z: S3 }) e* w - #define Error2 21 U v4 v! i( j5 Y4 @
- #define Error3 3
* h# I$ W" m6 H A( ?& k' Q9 O
, W) Y1 i+ V7 _5 z( {- #define Kp 15- [: W1 q5 l+ }% M" h' l
- #define Ki 0.15
" j9 x5 E( z8 _$ b/ V9 r - #define Kd 0.03
- P- u6 W" u* q( F" k
5 J" a2 S: b; S) W1 v5 C/ u- uint8_t sData;( z8 C1 c2 m# `1 ]2 g% o
- uint8_t D1;- M% K5 E- G4 _3 G
- uint8_t D2;
9 k5 o; L* q5 S! Z1 V - uint8_t D3;4 l9 Z% `, c7 }5 v5 ^
- uint8_t D4;, k `5 u4 |9 f8 Q/ v, e) D
- uint8_t D5;- I, Z* t) q; @% `' K1 n- B) p
- uint8_t D6;
9 p5 X9 V) ^6 \, \* g7 K1 [) L: @3 [ - * X3 o7 r" S* b
- float previous_error = 0;
0 s' z7 x: W. @( R$ S - float integral = 0;
- g: X- n# G2 S - float derivative = 0;
# N4 U# T/ ?" {9 C7 g8 Q - int Speed = 160;
: p' _. u7 Y' J0 d4 j - float output;+ ?8 `; I* ~9 S x* {7 `
/ t1 d- F. U( P0 _8 N- byte Left;- s- y+ ]$ U* z6 l3 E
- ; k1 H* J3 d# ?4 U$ A& S
- void setup() - Z- _/ t$ u- ]% U" D' v4 z: ^, J
- {6 N: |# U* [- H( Z7 Q% _- p
- //Set PWM 8KHz- v8 A7 X" P* Q X" }& r
- TCCR1A = _BV(WGM10);. }: ^ b8 l2 H" y1 m, Q5 B
- TCCR1B = _BV(CS11) | _BV(WGM12);6 Z9 j, U' y" p7 s
- TCCR2A = _BV(WGM21) | _BV(WGM20);
6 v: |0 z1 r/ I6 I6 { W - TCCR2B = _BV(CS21);
& f( S. M$ K2 { - Serial.begin(9600);
; W* S+ e) m8 P5 t2 ?" O - buzzer.setpin(45);5 ^: N$ K2 c5 r' x8 u) t3 `! A0 l
- }" e. p& B/ a$ O( q9 I4 G
! r4 t# J4 V; ?" p8 Y1 O- void loop()3 B# l; Y- B: U9 s+ B
- {
3 l3 S/ a7 S* N* R8 J - while(!((lightsensor_1.read()) < (10)));/ \6 L) u& i3 S& q
- buzzer.tone(1047, 500);5 x. B: G& I% B/ _- c1 f
- delay(500);
3 s& m% |$ b5 T6 Q - do
3 L+ M1 t$ P, H N! }+ h9 S - {/ h% x% j5 a! i* D
- int Err = getErr();
( T2 O1 x3 i/ S/ W7 K7 R - if(D1 == 1)7 |* I/ F5 W$ I/ |* z- Y: w; f" E
- {
, ~* `* z0 F1 a - Left = 1;5 @: }: j3 P* g/ A
- }
- i: j; b6 s# n8 _3 O, K3 b0 q - if(D6 == 1)
5 U* y9 [8 |! m. Z; O) _% A - {$ p6 z% [6 U! \, K5 a( U1 J! ^
- Left = 0;
& v3 }3 l6 R# b7 w1 C - }
! E9 l5 b, k* R' u - if(Err == 99)6 w, J$ ?1 V3 P. l" U: s5 Z( { `
- {
9 R$ F0 A( Q: `4 ^" s - if(Left == 1)' S) P& l/ ~! b( K1 U: G4 u/ W0 a- v
- {9 G& q( ~3 g9 z) T, d+ `! a( t+ q
- Speed -= 5;" [0 t$ G, G0 V: h* @
- moto(0,Speed);
8 L; G! C. z$ C; W. l0 O+ \) @# t - do1 k+ t3 o; i) c5 d' [: G; @! A
- {% J4 |& z$ ^' W, W& M& |5 R
- Err = getErr();9 }0 ] Z0 C: T: H0 T/ c3 r, M, Z% L- r
- }while((D1+D6) == 0);; l; r* w/ k2 e, `5 L! v: P
- }
* o( ?* q; s# D+ S: i q - else
) Q: z. g+ o! L0 ?8 O - {
O1 C: b: w( ?& A% L- s - Speed -= 5;
9 Y: M5 V# }! R' K' y$ o - moto(Speed,0); ' n9 @7 U8 _% u! {
- do
3 l6 b0 {4 z2 }( T: k( G2 ], I - {9 R% A! E2 V* D
- Err = getErr();
W; ^; g( j* `, n- b. N - }while((D1+D6) == 0);# G* T8 _5 f6 W9 ]0 t& r# d/ z
- }
+ h, e. V" l+ x' j! }& U2 D - }5 d! ?5 l* B; O t* ?% ]% m) u# Q9 S
- else
# x! |$ f5 t* l( c - {
- u* ] y7 y, N+ V' V3 b - if((Speed < 160) & (Err < 2)) Speed+=1;
4 U3 i) e- N z; z B4 G f8 J - if((Speed > 100) & (Err > 2)) Speed-=2;
: {9 Y0 P8 ]+ w2 k: V' X - integral = integral + Err;
3 W& `& {7 ~7 w% [ d* z - derivative = Err - previous_error;
8 A* `( b) P; J - output = Kp*Err + Ki*integral + Kd*derivative;1 I; a& B- m: g$ v6 K0 H: U4 c5 n
- moto(int(Speed-output),int(Speed+output));
3 m- v+ |. J, h# G* R% Q& W - previous_error = Err;
/ X9 f/ ~7 m$ z. C - }8 B5 F. `1 W1 G: n' K6 \2 V! \
- }while(!((lightsensor_2.read()) < (10)));
: R) R* e7 A6 D% M) P) n4 |5 O7 ` - moto(0,0);
6 s. J8 H6 W {, P- E% ~8 j5 e$ Y - delay(500);
3 Q! T/ z' n' I4 v. d' E - buzzer.tone(262, 500);
/ c2 F- q2 s, y( c; [ - }. k! W5 |* Z3 x. j7 U
) N4 q3 Q" Z/ m$ G0 A5 u- int getErr()
8 O& e# q# \6 v1 v2 ~0 ?/ T; g1 Q - {
: K1 [7 y7 ?" U: C - sData = linefollower.getValue();- g/ G M+ t! C% F
- D1 = ~(sData>>0)&1;& s; D% F; x7 Y# s# ^, J# e
- D2 = ~(sData>>1)&1;
- ]/ k. j0 M/ l' l( W; i5 n - D3 = ~(sData>>2)&1;
6 R" `0 k* C% F _ - D4 = ~(sData>>3)&1;7 w3 a5 B9 x0 K% A0 }6 \
- D5 = ~(sData>>4)&1;
4 \$ o, a _; A7 p1 f1 ~1 @ - D6 = ~(sData>>5)&1;
! Q5 [; L& a+ B$ j - int downD = D1+D2+D3+D4+D5+D6;8 `5 y, M2 P( o4 ]3 q9 P6 O5 o
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
6 K# S; z/ A* `5 y9 }+ p6 `6 M; w - if(downD == 0)
/ x7 _/ x, s! r: b+ F+ e - {
, |: r1 S- Y; \8 b7 b2 V - return 99;
; B1 c+ A3 E+ J9 v - }& I3 I& X& n Q: ~
- else* w" Q8 v* i2 @4 y; R- f2 B2 I' v
- {$ |; y8 _: V* r% C( D" G8 ]
- return upD/downD;4 T! p% h6 W7 G D& O6 G( v" U
- }* r! Z% p9 R: B4 ]9 V* L( {
- }6 Z; X( I2 \7 n7 [5 f9 Q
( a: j% _9 l+ }6 V0 d5 b6 J2 @ K5 M- void moto(int Speed_L,int Speed_R)
5 q6 o- L+ O, P. H - {
+ {5 l- D. {8 g% E9 q2 R s, G - Encoder_2.setMotorPwm(Speed_L);; e' i, w$ N. F9 b9 Z
- Encoder_1.setMotorPwm(-Speed_R);8 `: ]) F g1 x9 E: x7 q
- }
複製代碼 * d) K% Z- z0 [2 N! J
MeLineFollowerArray.cpp# [! ?4 a% N7 p2 p4 ^2 M, X+ D
- #include "MeLineFollowerArray.h"
$ w% c/ N/ o4 F2 S1 Y8 @9 A; s
% ~9 B4 W+ W1 M' R) j) v" T- #ifdef ME_PORT_DEFINED
4 r5 a8 F: ]7 H* H7 X5 \ - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)' p/ f$ t! k$ ~* j3 r: A
- {
4 W3 A) ]0 n$ w% x+ k
1 o0 o2 w( e& Q6 o- }+ E3 A0 |4 J# z, \% V" w- q9 \
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
$ I/ l6 L5 j" a% x' L: K2 G; t - {0 x6 P& u2 d, t2 q
- _DataPin = mePort[port].s2;
* {/ ^9 ^8 s: _4 O) C - pinMode(_DataPin, OUTPUT); . e% h! R9 g4 J- a5 z
- digitalWrite(_DataPin, HIGH);+ N/ D, z5 x- j0 ~0 Q
- }% r: {, T4 A5 V
- #else // ME_PORT_DEFINED
- m9 f# {( P2 L, R2 i8 A7 D - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
/ G' N4 Q I- N& ^1 K+ t - {' ^4 P: y8 O5 M0 R/ I" D* s
- _DataPin = pin;
+ s6 J1 R! g7 d4 R0 K/ ^ - pinMode(_DataPin, OUTPUT);
2 B7 b) b& A! s - digitalWrite(_DataPin, HIGH);
8 K# ]/ Z& w* h - }
! _' s" d4 V" }+ C - #endif // ME_PORT_DEFINED
- Y# C/ a( f2 o; I
; l! y! J9 k7 q8 z' k
' k5 O8 I9 C' ^* i8 P P- void MeLineFollowerArray::setpin(uint8_t pin)! `/ |1 G( e2 Z% b( {7 C2 P
- {
# ~ ?+ t( O0 z - _DataPin = pin;
( M5 Z, ^% R2 l - pinMode(_DataPin, OUTPUT);
" r) V# ]/ h( E8 E - digitalWrite(_DataPin, HIGH);6 ]2 ~" J9 Y& U$ _; o
- o; H! d U7 l+ B( v
- #ifdef ME_PORT_DEFINED% E/ f! O5 F' Q# k/ |+ {8 Q. D
- s2 = pin;
% I9 U% `! Z- y, c7 M - #endif
- |( ?* }3 D+ T/ _6 M - }
, L9 {; h* b& e8 R0 f5 S) C - . A" X& i. y! h M' [1 l
- uint8_t MeLineFollowerArray::getValue()
4 B* {4 J" S1 r3 v4 m: k7 i - {
. d+ E2 Y* h% R' O - uint32_t LOW_level_read_time;! J# L2 T$ V _' V* C
- uint32_t HIGH_level_read_time;
6 o' F% o* p2 e6 ^; j - uint32_t time_out_flag;& b2 p. i0 G8 R/ b: I# v
- uint8_t Sensor_Data[3];- ]" `! C% d- p) ^# j. o( B s
- static uint8_t old_data = 0xff;
9 O5 A* w% V1 x - ! h4 W# o' V/ d. l5 L, I% u' X, I
- pinMode(_DataPin, OUTPUT);
* g7 g- P/ v6 X3 M - digitalWrite(_DataPin, LOW);9 ]7 C2 Q" p" L; K" M
- delayMicroseconds(980);( m6 K0 i" E$ V% [
- digitalWrite(_DataPin, HIGH);$ O, ]. Z9 }7 j3 G9 w, C$ u6 V
- ( ]# g* J5 c( R" w
- pinMode(_DataPin, INPUT_PULLUP);
3 J0 H9 u" {1 J' l - delayMicroseconds(10);: i- ?9 g6 b" N! d- n
' Q4 U; \9 n# l' j- time_out_flag = millis();
& x& F; w$ i, `# d& C, b - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 A# m# z0 u. |5 x5 O
. P- L5 I$ F+ }5 x. |6 G2 M' v- LOW_level_read_time = micros();
0 f# D/ D5 \$ a0 g6 M1 b' K \( K5 {" d - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 O9 t+ U! C3 V$ T) ? W! v$ o - {
! e4 N" T4 o. ]% }. m6 P4 X - return 0xff;& [8 g6 _# I# x3 R/ t( |
- }4 l/ y0 O8 C0 A
- r) z/ L; i; v/ C% Q% W- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
1 `% p: r1 C6 |) V# |" P) l+ i8 [
/ F' D; s6 \$ J& e7 p V- HIGH_level_read_time = micros();
4 J. a3 f' E& b6 d. Q; [3 V - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
. h" @- z; J+ D4 R b
9 D' Y# F6 i; D9 I- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
% R& m$ S' j/ e. E6 v$ ]1 `( b' S - {
& |& o" D; h+ w; r5 t* ^ - return 0xff;9 q8 B& w5 I5 k' Q8 G
- }
& i7 H: T& O. @ Q; M: n. h
! ^8 Z# e7 \; ]1 O. n- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))" d- g r8 e4 r
- {1 _, \/ A& o3 }9 t" k9 q
- return 0xff;1 {1 }7 \* W% c1 @& w; C
- }
! [: i0 c. M8 L7 D. u& o - y! R! o1 N8 c# \
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 d2 z5 a k) V7 k: _# w - LOW_level_read_time = micros();
3 F. b9 Z! B; N8 O8 { - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
9 m) g9 }# j1 {1 L5 i3 f - / s/ P3 g8 l2 x6 ?! ] |7 j
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
* |& T+ J8 ~: ^( R$ }/ [ - {! n$ _3 A& ^8 x9 V4 Q
- return 0xff;! K# K% j) A: \: q" R
- }
. [ i/ j3 F( p& b! b$ ]
+ f) R' B$ {( _" j" f- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
9 m) W" B. Z* l( n; h2 k& T+ o - { k& ^% G$ o5 a& ^* t/ f, f: i
- return 0xff;
: ~1 `1 C; a, J - }1 D$ W; h8 \6 F
# d# |/ g! Q* W$ `7 ], `- for(uint8_t k=0; k<3; k++)
9 x9 s- Q$ r, Z8 V% f - {7 O; F8 i- Q+ l) V& `
- Sensor_Data[k] = 0x00;
O- y% Q. C! M
: I' ~8 g$ B8 R4 ]- for(uint8_t i=0;i<8;i++)! d3 N6 M; x% T- s% G1 S
- {6 Y% D$ X5 z( i% i! @
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level3 v7 D- M3 N3 c2 X# V0 u: m4 a
- HIGH_level_read_time = micros();
) G j) L& H4 A6 S9 T - LOW_level_read_time = micros() - LOW_level_read_time;
; Z. B9 G, N _% ~! ~5 K
g0 m: G6 Y: [, @; }5 e" v ?- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )* q$ u" H! u9 R7 e2 q8 N" ?+ o
- {6 Q2 v8 g* k3 z; |
- return 0xff;% T# ?% s/ p/ X; J
- }
, y: u$ l5 Z% E' R9 t% t - 8 z3 Y1 L" l* l0 q/ Y- u5 v
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( S8 ]& W+ g& L& p. q# o1 Z
- LOW_level_read_time = micros();
* m; U; z W% G; R3 u - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
H" Q3 o+ G& A
7 C3 d$ j/ E& v/ b5 f0 ^- p- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
6 U% h! A* o* q6 F" v! c* Q - {5 _, o( Q, N% [
- Sensor_Data[k] |= (0x80 >> i);) J5 t# w8 F0 n( m6 _" m1 i
- }, Q0 z$ v( h; W, c3 p+ _. D: q
- else if(HIGH_level_read_time >= 100)
4 \/ z S+ j" i& [" l - {$ E. B8 u) C0 _+ C
- return 0xff;
: ?( Y7 s7 h2 h0 ^" r1 z* C3 o6 V - }3 ?( ?( D3 w" l4 e8 d
5 u% n' [$ l$ I1 f- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)/ _, r9 t# k7 ?
- {
, ^% w4 f* I* k% v8 R - return 0xff;
( Y9 D; x4 f& e' U# d+ C$ n4 W0 a - }
! Q: C5 q: w! T/ ?7 { - }
7 J. l" ?# `$ n4 C - }1 s# K* `8 Y2 }% A* o+ S
- 5 B; v0 E n+ m0 @# W- {, D
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level( G5 b/ u3 u3 I- I9 }$ w" w9 a8 e
- HIGH_level_read_time = micros();2 D! n* r+ k+ f. X. W7 X2 o
- LOW_level_read_time = micros() - LOW_level_read_time;% [& ]+ `/ \7 ~. O
- 0 c8 {" r" p1 y& ]7 w. K' a
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )6 m* D7 |( Y8 A- r; s
- {- D @; O/ ~$ r7 H5 B7 V6 |
- return 0xff;
) f0 B/ s6 h: w0 { }4 P* P, I - }& ?) m( k% m7 j& |, A |
' x: {3 I! |2 V- pinMode(_DataPin, OUTPUT);
) Y/ R( U' A& `/ S, A9 K - digitalWrite(_DataPin, HIGH);' ]5 C t0 q# X9 E, ]) g
. c/ I* [* l# t6 u0 y* H- i q- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
% S6 M% I) q' U8 K1 g - {" a+ g7 ] a9 H7 \7 O
- old_data = Sensor_Data[0];
( p. E, q3 ~, j% [) E - return Sensor_Data[0];
/ G8 t& v! w7 s; I9 [7 @) V9 ~ - }
, L' a& T5 _! m) @1 y - else9 |7 W# q# @7 u
- {0 y% T# L" {/ G2 J/ N: \
- return old_data;
6 ~2 j6 A" F8 N- O" }& Y% M+ t }0 Z - }+ N& n! N! {0 s$ N( |6 o
- }- k& B- I. F. S& z; u
複製代碼
5 L8 n2 X( ~& z3 m0 q2 F8 K5 NMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
5 C+ D0 g3 i# H' S3 n# n" H5 \$ x6 F: D& H7 i! o* [3 G2 Y8 x
|
|