|
|
- c4 l5 m7 H, `+ K6 ?# h
- #include <Arduino.h>+ ^% d0 X: }9 }% L Q: Z1 M9 R- {: M
- #include <MeAuriga.h>
. M, |$ K: v) b! I - #include "MeLineFollowerArray.h"2 z, R. g4 l; Y) s
- , s/ F* w. s9 a8 j9 ~ q; U' p+ n
- MeEncoderOnBoard Encoder_1(SLOT1);- d4 x! C5 Z- e4 h# y) a; j- G
- MeEncoderOnBoard Encoder_2(SLOT2);( ^4 w* ^: |; h/ q
- MeLightSensor lightsensor_1(12);
, Z: J' v' \ d! ]/ P D: g" O1 G# b - MeLightSensor lightsensor_2(11);
$ L9 ~ H5 W4 M, G% a5 Q+ ] - MeBuzzer buzzer;
1 ^; a [, _6 d) b( }& N - MeLineFollowerArray linefollower(PORT_6);& |2 Y" _8 ~# C/ o
9 R* ^# W P+ G8 i* i+ Y; H- #define Error1 1
1 t& ^3 @8 K/ a - #define Error2 2; I3 a9 l/ `8 H' `
- #define Error3 3
; V0 l* b) C/ Y* u/ _) O7 } - : R! W- j+ ^3 c0 {- i* ^. ]
- #define Kp 15, l* N' m2 h% P1 J
- #define Ki 0.15
5 z6 T) }* U4 n5 z9 \4 v7 g - #define Kd 0.03- Y N0 g1 [9 U$ a
! i1 |, G5 A1 w" |- uint8_t sData;
" f9 x" V0 \, p+ r- L m2 ? - uint8_t D1;7 E% {) o" M# E0 K2 o
- uint8_t D2;6 i; |% W" F, x$ Y
- uint8_t D3;
7 E, S3 E" `2 E7 P8 J* x+ E$ c - uint8_t D4;
- s8 V+ T$ z7 r2 M/ @+ ?- O - uint8_t D5;
f8 y9 }4 U! m) |3 ` L - uint8_t D6;& d+ C3 U( a! v) U' `& n
- 8 n! `' N; E- ^' q+ j* Q% t
- float previous_error = 0;
: I" {3 ~' f' l2 p; a - float integral = 0;
' D$ g+ M# s5 d3 y - float derivative = 0;
: q6 L+ Z5 r( b' I - int Speed = 160;
* I+ }& T/ ]: i/ g& n - float output;
; i" @4 n/ T! e d - - x. V, u8 `5 E8 u B3 i6 t/ v
- byte Left;
& P/ s+ z; o' s$ E
: P9 o8 p9 L) m' p! e0 M- void setup()
1 P" B4 M' }9 h/ q) ~$ H - {1 W& L4 z; n! o" x5 ^6 E
- //Set PWM 8KHz
" Z8 C! C3 a+ u% L - TCCR1A = _BV(WGM10);
3 b: F) s1 r, v4 o9 U) s - TCCR1B = _BV(CS11) | _BV(WGM12); `& I2 i0 u6 x p0 g5 _6 _2 s/ r; u
- TCCR2A = _BV(WGM21) | _BV(WGM20);2 [& c$ L9 [8 i* r1 z
- TCCR2B = _BV(CS21);
: U+ k2 Y A! S& ? - Serial.begin(9600);6 W( ^ E9 O% _! q; _: P
- buzzer.setpin(45);* f: Y4 W! N3 e$ }: C K
- }
: S% j" v- O2 d. K+ c) v, D - ; r7 X6 r3 Q5 } _9 s
- void loop()
4 G9 B& ~( ^. z2 `, r - {
, r4 f) w! D9 G - while(!((lightsensor_1.read()) < (10)));
6 } |' ~# } O% x- s3 S* t2 \. m - buzzer.tone(1047, 500);) d7 l- [& J+ S m7 i
- delay(500);* B( B* L9 W( V6 S9 f
- do* E7 p9 A3 v5 U, O; z" q
- {
; E# |* l0 t! j* q7 Q - int Err = getErr();8 g4 p" b. S3 \( L$ ~2 P
- if(D1 == 1)/ ]9 J$ Q$ o: X- A( J c
- {& c* O: p5 t9 q v. i2 _1 F
- Left = 1;. ?3 g# n0 z# i% G
- }
s( W" q! W U, w* X) Y* K - if(D6 == 1)- G! b& q j4 y; t" D' t E( N
- {6 ]- T2 {9 E) B. b# {4 H5 ^
- Left = 0;
0 m- y( G; z% ^# J3 J - }* r; x8 F3 D0 t/ R) X, \. B
- if(Err == 99)
s. T. n) T3 C5 M - {
0 F7 X2 b) j/ W" E6 p - if(Left == 1)
4 k2 {8 C/ `: `8 |+ j# J! o4 o" B - {4 p+ W3 a/ y4 r9 ]! H
- Speed -= 5;$ V9 A9 b( \ T$ d0 ^& F# p
- moto(0,Speed);
1 z7 B2 {; k) ?0 R% Y - do; s1 t7 p- H5 F9 y% r0 [+ s- ]' Y2 Z
- {
* o3 V$ t8 M5 q+ o' a - Err = getErr();
4 x- O$ v8 ?8 k9 v4 d n: } - }while((D1+D6) == 0);2 W# Z- U! v, d, O8 }
- }
2 F: F/ e! c: R" `1 Y - else
/ I4 y/ G' g. | R" h - {
" p% o# ?6 q8 h4 G6 M- z - Speed -= 5;- c% c! X3 N* u4 W% j8 J
- moto(Speed,0);
3 \0 o& _+ z4 b5 F - do, {1 V; n, T3 W) d
- {$ B$ x5 `+ P4 g7 q: u5 a6 [
- Err = getErr();1 T- b$ M) j- E) u. P
- }while((D1+D6) == 0);- G/ g3 \; M( E, W4 G; Z w; `# `
- }
f$ S- o2 x+ p( ~4 D9 N - }0 c- Y1 G! f# r& K5 m E/ u' P
- else4 m2 `- [1 M: X: Y$ ~
- {) d; z7 n) h/ Z3 l
- if((Speed < 160) & (Err < 2)) Speed+=1; : R7 x: {+ Y" m5 o$ t. H# B
- if((Speed > 100) & (Err > 2)) Speed-=2; . d9 ] O7 q6 s# K$ S5 S" S* F
- integral = integral + Err;0 o2 c$ b6 y' L5 r8 u! Z$ R
- derivative = Err - previous_error;
5 C1 V, }" P0 A( E( e" O* r - output = Kp*Err + Ki*integral + Kd*derivative;
* w- Y# Z: S- _. K$ g - moto(int(Speed-output),int(Speed+output));
2 x( Q+ n2 y9 C0 W4 y: W - previous_error = Err;
$ k" d) j) Y8 a! m, ~/ |. ?2 Y - }
$ R# G4 _5 {- f. \# u& m - }while(!((lightsensor_2.read()) < (10)));
6 r* T; t+ [0 j) P2 R - moto(0,0);5 O. c2 q( ]- [$ Y9 K! Y
- delay(500);
6 w# q4 ?0 R _8 d0 j6 X - buzzer.tone(262, 500);
+ Q: J4 R5 H- W+ G& E- e - }2 R9 [9 L& q, P4 C
3 Z c. w+ R9 B- int getErr()
% T. D) K0 e8 i( m5 a( _ - { * G% G# G2 g; R0 l4 O% V3 Y0 L
- sData = linefollower.getValue();8 P& `" t7 `$ y8 T4 o" Z# V+ e
- D1 = ~(sData>>0)&1;# n) t3 j; K$ i2 s0 v
- D2 = ~(sData>>1)&1;
, W0 N4 ]$ W) {$ b. K+ w* z - D3 = ~(sData>>2)&1;
( S: r! ]: ]7 b - D4 = ~(sData>>3)&1;
" |% `" E+ P7 s6 N - D5 = ~(sData>>4)&1;
2 X6 [% s, I3 \# h1 {' |+ F - D6 = ~(sData>>5)&1;1 S+ W2 n9 \4 x1 l1 q& T2 Q9 O
- int downD = D1+D2+D3+D4+D5+D6;
9 h* i( o7 E( t: B% q - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
~8 b# ?. H1 j, z3 W - if(downD == 0)0 j$ d! Y$ U0 e
- {! }# R2 [ ^: I, ~5 U4 F
- return 99;4 ]& [$ b3 p% Y- l$ w
- }& @; m! v* b* q) o
- else' ~ H/ I' i3 i; d
- {2 r$ G) d# e3 n9 p$ K. G& k
- return upD/downD;
" h! F+ K% Z8 Y$ y0 s% B2 ] - }( y. ?+ d1 g) z) x( F5 @
- }! D" r, m( W( ^2 s( m: Y
/ V7 P& a0 a+ S+ f& t) I) N- void moto(int Speed_L,int Speed_R)4 q# a3 O3 u4 O4 _& H+ X% a+ h3 V1 C
- {
I: ?( Z; f( o; c3 ? - Encoder_2.setMotorPwm(Speed_L);
; I, Y- ^0 ^/ q$ u- S - Encoder_1.setMotorPwm(-Speed_R);
; z5 [/ C7 q& \+ Y3 J, R - }
複製代碼
3 `6 t* s7 Y7 R4 HMeLineFollowerArray.cpp
: f/ _; T; u8 [ N2 Q" y, e7 i- #include "MeLineFollowerArray.h"' [4 R- C6 \3 I/ R% T! M b
- " j- \9 k. T4 i9 U @# c0 s/ a/ R' q
- #ifdef ME_PORT_DEFINED1 f' Q9 z9 T' u' @. ^3 U \
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)7 b* N# I% I7 P" D- a
- {
% k) |) J1 D0 R9 @8 |
3 q. T" s! V0 {- }% l9 s) @2 N( Z* p
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)2 E" n8 `( W+ i* k; M& }7 Y2 J% r
- {- H/ }$ z* ?6 V- k! B p0 R, c7 D
- _DataPin = mePort[port].s2;4 ]) P0 b) M& j* i
- pinMode(_DataPin, OUTPUT);
1 Q& \( t6 ^, f4 W" E - digitalWrite(_DataPin, HIGH);
2 T L d8 I/ f" n% d* v0 ^ - }) x3 n5 e0 d* v" q/ S
- #else // ME_PORT_DEFINED
: j1 d$ b! J9 {1 h) o: L' T - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)( l. D* M8 f) D$ Z6 l# l h# T
- {
) X' n+ x3 w- X/ r; i3 d. q - _DataPin = pin;2 t* ^1 e; b( y$ R9 u; Y* h" [
- pinMode(_DataPin, OUTPUT); 5 z9 L8 a7 n/ L" h8 [" ?
- digitalWrite(_DataPin, HIGH);
% l% P: R7 M: Q5 A( l4 [8 o - }8 p8 n7 F' f& F8 I
- #endif // ME_PORT_DEFINED5 c' c! R8 J$ J1 U
- / V6 m$ w" x: j3 B2 ~* ^
1 W7 v5 \' u0 K+ a- void MeLineFollowerArray::setpin(uint8_t pin)
* ^. W9 M0 H" b2 ?4 m5 n7 j0 m5 j - {) g9 Z. o* A& {) o; s
- _DataPin = pin;
: K" q- B+ D" @6 } l - pinMode(_DataPin, OUTPUT); + W0 l% M1 C. @& H2 v0 ^. I& ?$ P
- digitalWrite(_DataPin, HIGH);; C) }6 x6 x+ P, x+ g1 q- E6 y
H( {* T& U5 C. a) s; j- #ifdef ME_PORT_DEFINED
% R& g7 V1 _) n0 f9 U: ] - s2 = pin;
% L2 b9 i' o4 X' F - #endif- D2 e) R* z# h! a2 y( U
- }
) ?# E0 I2 H% s ? - 4 S. J; d! Y v0 h5 O6 K) W! z8 q
- uint8_t MeLineFollowerArray::getValue()
% j$ W3 ^' ^0 `! R o2 o9 ?6 b - {% l# T' E" F" \ l" n
- uint32_t LOW_level_read_time;) a0 F n+ b1 F# K. v- H1 X( ~
- uint32_t HIGH_level_read_time;7 C/ c' X1 u- o$ P, g
- uint32_t time_out_flag;( @! H; o- m% z
- uint8_t Sensor_Data[3];" u6 B0 I7 G8 h+ b m" c
- static uint8_t old_data = 0xff;
, o$ z+ B" S8 e) [5 G - % F) V7 c/ v- `5 [+ G0 o
- pinMode(_DataPin, OUTPUT);
8 g# ]7 ^- m3 Q - digitalWrite(_DataPin, LOW);, `' l$ ^6 V* F5 s- M( f2 o" h% ^
- delayMicroseconds(980);
" D: y# y% H& {9 G - digitalWrite(_DataPin, HIGH);9 v8 j# V$ X6 @
- : p- ?* `: }* v& o8 m% T
- pinMode(_DataPin, INPUT_PULLUP);; V- m i& U* m6 }$ j) Q' B
- delayMicroseconds(10);
1 ?0 w, R. s5 b5 l: Q - : |$ Y- C; |" A1 L5 v
- time_out_flag = millis();! [) A' u1 |$ d, v6 u( h
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- K% F! v* |# D' n; e - 3 y* R6 u4 b2 }6 U5 w* c: s
- LOW_level_read_time = micros();
; |" t$ C6 r: m - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out$ V Z/ T0 X7 f" w
- {+ x( @! T; X+ _6 x! j e# Z
- return 0xff;! K5 t& |* \: V8 G! J* `
- }
2 S' _5 j8 |& Q - - d( Q1 d/ \/ {- }, i0 v. s
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' q8 G9 H+ o; K) t8 n/ w( ]9 o* V# R: q
8 {" q A" ?: g* J* Q; Q# ~% V) m2 d- HIGH_level_read_time = micros();2 d. c, k, ^; |
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
+ l% u; ~2 j( X - 2 e }8 X/ @) q% K0 i2 O" ^$ A* R6 m
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
* x. A$ J4 J8 B- [5 O - {, A7 s4 n, q6 s- X9 H0 b
- return 0xff;' A7 I9 g; U Q: T: Z) \
- }
6 V* D7 r! ?0 \9 } - . D* f: y7 N' e( u; E
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))# _5 l- s/ l( [/ V3 v
- {
0 M' y- {( x! W - return 0xff;
8 d8 ? B* |3 T* U' p* {" k0 U - }
4 u* y6 d- b. J& ]! f& \' Z - , V6 }, f- V$ F; F
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. D( u' L5 K4 U - LOW_level_read_time = micros();1 {; ?. S# u( H
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
/ ?4 ^* H& E% ]% t1 [ O
5 B5 i6 s1 ]# }7 Q& b- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out+ m8 Q" W5 k+ O. d% Q
- {
- u: Y2 f$ e2 g - return 0xff;0 N# d- k8 I+ ?) I V6 E
- }$ }; o& P6 L- m/ W
- & u- r4 h; A+ l+ x
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))) u. u8 V. L0 _) @1 Y: [% O- n- `" {
- {6 d% V1 |4 w$ L
- return 0xff;
& U/ W/ F7 p$ a& R; q - }
0 i6 e# _) x/ v - ' U6 W1 @2 g- U; D' ~
- for(uint8_t k=0; k<3; k++)) l& l/ C$ {# ?. d6 ^: Z/ A; }# [- y5 g
- {( ?# n( I* F- W8 A$ \# ?% o
- Sensor_Data[k] = 0x00;1 [- v+ `% ^9 E, e
- 5 Y8 e# Z- I& b& l4 |. R' N; N
- for(uint8_t i=0;i<8;i++): G; |0 i) {* Z- @
- {" ^- ^0 T3 K* _& j" \
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level: i+ h8 u2 y8 l# J
- HIGH_level_read_time = micros();. {6 W( D: o p' v6 f# l
- LOW_level_read_time = micros() - LOW_level_read_time;
# D: l" U2 {& b( h
8 P4 I7 k4 e# L3 z+ ?2 t- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )9 S8 m& f m3 q1 o
- {4 C6 U/ k- D; m% Z
- return 0xff;* E- v+ N* g4 P0 K3 Q
- }
3 A+ `) i5 L- k; C9 H$ P: B
: o- l; d# f4 l. O/ |5 v- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: W1 }4 a9 Q8 z9 v3 X - LOW_level_read_time = micros();
# i6 `5 b) T0 ]+ A6 J" {9 K - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level% C& u8 X/ ?0 Q2 n# n5 j! f+ Z
- 1 J F5 j* v* l6 ]2 g& c
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1& e2 e# S' r, u% T* D
- {7 }+ R4 m/ a) |
- Sensor_Data[k] |= (0x80 >> i);
6 ]1 d" V0 m5 [2 }. j- M2 e - }
8 |+ I! \0 b4 ~ P' ?" e& k3 J - else if(HIGH_level_read_time >= 100)
2 c: y* D0 A1 p0 w; B% J - {$ b$ a& `, ]% E3 q) a
- return 0xff;
+ y* X# n7 H" g+ O: u/ {! S - }0 w4 C/ ]) g' Q
7 }8 u8 K4 @. A: G9 x- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
. u3 f" m H; Y7 y1 l6 A - {0 `- g2 E$ [6 \9 N# Z; y3 _! J
- return 0xff;' K. i% }. x F2 o
- }4 G( ^6 j3 E* o8 L' S
- }
x* N% }. C2 Z9 c V5 g - }
1 ^4 A! A" d- N" H) C1 C. i; W - " Y0 H1 S s" Q4 Z8 y) I) f" S/ e
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
0 }3 e% l0 W' M) C$ n! x( x; M - HIGH_level_read_time = micros();
# ?' u1 D) X: v - LOW_level_read_time = micros() - LOW_level_read_time;
, v5 S3 ?' d" |) u" p/ T# _ - # X$ [/ @# S6 e7 h7 i, \
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
8 B+ {9 K, O+ [9 { - {: h: F- {" s; }0 X Q% O: |, w
- return 0xff;
$ l' K3 Q! K4 B9 ` - }
, p% P7 o% q) ]+ M" N4 Q/ x - % d; I% @9 ]" @; X# |5 d4 A* \
- pinMode(_DataPin, OUTPUT);
3 e- {0 C2 C, d5 `/ J: k - digitalWrite(_DataPin, HIGH);1 \3 r4 D5 Y" b; Y) a: ]4 t
, @4 b D6 @4 u) @0 H- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
" w" y6 R& {* G - {) j- t' K4 r! Y- M" H
- old_data = Sensor_Data[0];7 z" X# K7 S* }5 R, L! @- g
- return Sensor_Data[0];- K* |& \1 s- H7 |( R
- }
1 Z& x8 y1 G: I. s8 [9 p' l - else9 U* S7 o8 K9 w( Z. E
- {
) Z- m0 U- n8 V+ W* g: I$ Q - return old_data;
1 l& ]/ S2 c w6 G - }
2 t" X5 z! R) q) K! T - }1 T" |+ I$ g6 Q% D
複製代碼
Z6 O* _3 j( A6 }& ?0 tMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
' Z2 W; t" l. m' B# O& |! D" G) }2 X- t- z2 ^. s
|
|