|
1 a) p# M U8 c8 g4 h7 u$ y
- #include <Arduino.h>
( U5 m4 l; y7 n9 @& g - #include <MeAuriga.h>% ]# V$ v) z; Z
- #include "MeLineFollowerArray.h"4 e, I% S" o7 a5 i" v" V
& t! f, \; V; b8 _2 S6 m/ I- MeEncoderOnBoard Encoder_1(SLOT1);
3 L1 v& c* X( Z! A - MeEncoderOnBoard Encoder_2(SLOT2);
# _. L a* `# \ j, G g) v% _ - MeLightSensor lightsensor_1(12);
. [8 c7 V+ B! k2 y, W - MeLightSensor lightsensor_2(11);7 d: m- B% s2 |1 W" z& Y
- MeBuzzer buzzer;
4 A2 I7 \5 k+ Q6 X8 F0 t - MeLineFollowerArray linefollower(PORT_6);. ]6 u' G, C$ e* C1 `0 @
- 7 g& z4 Z# @7 A7 i
- #define Error1 1
" n5 d0 j X. \0 y# @ x/ _ - #define Error2 2
) T$ ~5 S: i& ] - #define Error3 3, C( G2 B: H" ~( v' ?8 b
- ' G* G3 f- i6 V% h* v% k
- #define Kp 15
: @2 F" v7 d y# ?# Y* l - #define Ki 0.15* Y+ y/ ?) V+ K3 h
- #define Kd 0.03
6 ]- W9 f4 X, d# n! r+ j - % F! i' N- `( A0 g
- uint8_t sData;) R* c0 d& Q; l" I5 v5 z
- uint8_t D1;9 ~. [- y+ k; w3 j j( \# l9 ~# z
- uint8_t D2;
5 `" [6 Q6 F X3 J: ?* k - uint8_t D3;+ L3 N0 }4 _8 a- R9 [2 c- v3 U! f
- uint8_t D4;) C; k5 q* o4 s, S0 J
- uint8_t D5;" r" |+ w6 s, }1 }
- uint8_t D6;0 E6 t. ]. Z+ p7 q c% h! |
- " ]: F! q2 Z. d
- float previous_error = 0;
) a$ D' p! Z2 m7 b* H7 \5 q% l - float integral = 0;4 c& l3 b* Y# K4 r6 U
- float derivative = 0;
) J4 G- S. E: e& k! @5 v - int Speed = 160;. W8 o7 o, S4 d' O# E2 o8 G
- float output;0 R( ~3 Y4 q% a* K' X$ j
5 \! o3 p* R ]6 i r- byte Left;
! T v% p5 ^# n; _# ~
: k7 d: C" ~7 v2 D- void setup() - C* ?$ e* ~9 A' o4 \; b
- {& d% k7 R' }0 |! ^3 Y; L2 G
- //Set PWM 8KHz
3 N0 n- A: }" Q0 K! Z2 j7 i7 k - TCCR1A = _BV(WGM10);# E7 Q9 ]" F" n" J1 N8 |- W
- TCCR1B = _BV(CS11) | _BV(WGM12);6 f' c* U6 Q% p7 c
- TCCR2A = _BV(WGM21) | _BV(WGM20);0 f( F4 {7 o: k9 e0 s* h$ N) z
- TCCR2B = _BV(CS21);8 @0 i- X0 o- l0 b% U3 V
- Serial.begin(9600);; T. a9 o7 u% }
- buzzer.setpin(45);
' x3 q6 H4 k; \3 ]: H - }
; y% y( `0 a& F# k8 C
& j* L5 g* {( Q" X# t: g/ U& y' w. ^- void loop(): D4 r) T! T) ^
- {
% U; @+ _/ x, A - while(!((lightsensor_1.read()) < (10)));* h( p @3 P$ {( y$ t
- buzzer.tone(1047, 500);( m4 @( D) d: ]2 `5 Z% R
- delay(500);+ t. S( R' \, C- I0 S% K0 }4 F
- do
9 l% W- k# x$ ^+ e2 ?# ] - {5 L: `; C4 u7 S
- int Err = getErr();
6 @* f4 G, n& \1 F - if(D1 == 1)& g/ o' D) X; S
- {
" w' j) N" p/ v. p* p+ n - Left = 1;
; \2 Y; Z& N$ ^) u, k) z( J - }
3 F1 h/ a1 k8 f% Y) B, s( e - if(D6 == 1)% C! q0 A3 g9 H+ O% l
- {
9 C2 \2 |1 h% ?7 u, a( L! n - Left = 0;
7 G' i! ?+ j" J A( D. K - }
/ J/ d2 u7 l4 z4 J+ ` - if(Err == 99)
( z3 E; Y' b! ^5 y8 |0 d8 e4 r" [4 c - {. Q7 {# I! p: M4 h8 Z0 n
- if(Left == 1)
& f# h. \( S" o% p - {1 n3 o* a, V. T2 w# J, B# ^
- Speed -= 5;
6 Z% h4 K& f! u) ]( n8 k6 u - moto(0,Speed);
, j2 X( n9 @5 `1 t( [. O0 |/ P v) x - do# ^* a$ R( |: d* T. k
- {( ?0 U3 \4 V6 _9 p3 q
- Err = getErr();4 }! {! m a, o
- }while((D1+D6) == 0);
& |1 T4 N `! e7 y/ y% T - }
6 x+ p; U* ]$ ~. c/ t - else
; k7 V( H1 h# A( S9 P. y) H* W) l. | - {# I9 b* X+ b+ L- V: ~
- Speed -= 5;
# @+ d9 ~7 @4 g, Z - moto(Speed,0);
( F/ f6 m: B. \: \2 L+ Z - do4 ?0 E; T( q s
- {
. q6 Q/ V" T' Q3 \0 G/ R% f - Err = getErr();* s s6 I0 k5 g+ y: g0 j5 S
- }while((D1+D6) == 0);
# o, [& L3 d1 E6 |3 b# w- ` - }
; E4 b' }% G* p' R! d1 V0 f) E4 P - }/ l' T2 g$ _% v2 ], A7 e% p
- else
1 W( n# J4 R: o5 T9 |+ |/ c - {
% p" t# ?. v: H) g, O+ ^0 ^/ u7 T - if((Speed < 160) & (Err < 2)) Speed+=1;
9 Y0 T; Q, Q S# E - if((Speed > 100) & (Err > 2)) Speed-=2; , b1 x! g' C( g" d) k( ]2 v
- integral = integral + Err;
( z# Y9 j$ J% H( W, L+ r - derivative = Err - previous_error;
$ u' |7 K2 L6 r& t - output = Kp*Err + Ki*integral + Kd*derivative;
- k0 f0 M' _3 w& R - moto(int(Speed-output),int(Speed+output));# n. b4 \9 F( _) r% Q
- previous_error = Err;
/ p/ k8 a1 d3 n5 T - }
* w& G7 D7 x! l( \/ o - }while(!((lightsensor_2.read()) < (10)));
' T) s& C3 ]5 l$ X' O6 [ - moto(0,0);- h8 u8 U8 F, I9 y6 a; \/ n9 r7 t
- delay(500);$ R3 z, z! ^: O3 I
- buzzer.tone(262, 500);* c" V! X- P8 E8 T5 h( `: C& ^
- }6 W- A& r8 e- s/ Z
" i0 ~+ I# E* A$ @+ x- int getErr()5 b& y; W; s+ R; L3 ]5 n
- { - V( T1 r1 y& t1 @' Y+ k
- sData = linefollower.getValue();# f0 j. Z: g+ K0 }
- D1 = ~(sData>>0)&1;
L1 e3 t7 [" _1 L - D2 = ~(sData>>1)&1;0 B0 t, E7 ]: f- q
- D3 = ~(sData>>2)&1;
, Y2 K* `$ W) \6 ~, e - D4 = ~(sData>>3)&1;" D# w5 l: L2 i. ^/ Z
- D5 = ~(sData>>4)&1;
6 Y, P2 v5 H& i, m4 I% c: C1 ? - D6 = ~(sData>>5)&1;- z" {% U9 }# B5 i$ u$ z2 M
- int downD = D1+D2+D3+D4+D5+D6;
/ Y0 {0 p& M0 _$ }$ v - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);" F- V4 X$ n& d
- if(downD == 0): Z( a) t( X- x( s) u e
- {" l( B8 a( E2 L
- return 99;
" M( g9 p; \( m1 D - }/ z2 Y; M T9 i& Y1 r9 P
- else
- N1 |8 e/ g: q. \ - {
7 j6 x# }* x- d: y. u ]2 ]% F - return upD/downD;6 @6 y0 ^! a' q! q# p6 w3 D3 L; V
- }
$ D9 u9 b( I2 O, K0 j' O; ^ - }! v6 x( t5 ~: s; V! f7 [) [
8 J% b4 E! \. V, t- void moto(int Speed_L,int Speed_R)
7 C( r2 v l' ^/ G( L! c% S - {" R$ C- ?! y3 p' B
- Encoder_2.setMotorPwm(Speed_L);, h: ^- W0 [- N6 p' n
- Encoder_1.setMotorPwm(-Speed_R);
, c1 ^! W& X% `0 |$ v - }
複製代碼 $ }4 S4 l" g* Y/ r; r4 k' |
MeLineFollowerArray.cpp( N, S/ J9 o5 `* H3 x0 T+ C" Y: ]
- #include "MeLineFollowerArray.h"/ J: v, c7 `+ y# W8 |; m
- 7 V7 L5 V! k) G5 }& C C! o
- #ifdef ME_PORT_DEFINED
+ ^- N. e1 O- b, _8 _ - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
# v$ y1 {5 R8 N. P! E" `5 S - {5 t( d' O5 d* R( \; C
/ o' P$ u+ H* T+ L6 h& v- }
# O. W- u" m5 G# J0 h- B5 [ - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)4 C7 E; N! P, h6 U
- {
9 a5 B2 q; V$ _9 U% z- y+ S - _DataPin = mePort[port].s2;
5 G* R6 ]( X: R- q - pinMode(_DataPin, OUTPUT); / L+ c4 _5 ^/ @) c B
- digitalWrite(_DataPin, HIGH);
3 R* R6 b% F- N) Q% i - }* x7 K ]3 Y& u+ K# ~1 e, g
- #else // ME_PORT_DEFINED
$ t" i* r. F% X( }( _ - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)/ N, X0 `! T2 [( N' z
- {7 X" F D5 l% m) J8 _2 F9 d- e
- _DataPin = pin;5 s" q# _) u1 L4 S$ D5 |
- pinMode(_DataPin, OUTPUT); : x( N* ]5 Q/ ^! g# [
- digitalWrite(_DataPin, HIGH);
5 W8 [* A$ o: }" N# a' y3 l - }: a, v1 i, d- w# Z
- #endif // ME_PORT_DEFINED
6 y0 B/ r& c! A4 \" B3 m - . d* n& B% u& c5 m2 x- _- K) r7 P
" a. w1 u& _% V# w* l6 y- void MeLineFollowerArray::setpin(uint8_t pin)+ S7 z+ }) V2 w! V8 h$ t1 R
- {( b/ o& c( o6 u" v
- _DataPin = pin;+ @3 U( `' q" P/ b2 X. ^
- pinMode(_DataPin, OUTPUT); & ?5 D& g! a4 w$ L K& ?9 f% _ j Z
- digitalWrite(_DataPin, HIGH);
& j7 e5 u% y" ~' K0 H8 X - ; }/ A9 p9 F/ {& y f$ a4 h0 k d
- #ifdef ME_PORT_DEFINED
( \- Q) }- a5 q+ c, V; { - s2 = pin;' e. j) e3 \4 s; c' a, w
- #endif
0 z |" R+ G3 | - }
# ~6 l, P, h2 M( l4 H
7 w6 }% {4 n: ^! E- n: w) b+ [- uint8_t MeLineFollowerArray::getValue()1 O4 T* `/ H" T9 K" B# u
- {, ~' W+ l6 ]0 c0 f i
- uint32_t LOW_level_read_time;. w6 u9 X O* Y
- uint32_t HIGH_level_read_time;
- B$ R* G; m# b3 s - uint32_t time_out_flag;2 J9 W+ ~! T( U7 }
- uint8_t Sensor_Data[3];* Z1 l* |" Q3 E1 z
- static uint8_t old_data = 0xff;3 f! O/ T9 G- k) b! i W
3 v; U. S3 K/ w& {9 S* p7 ^- pinMode(_DataPin, OUTPUT);; }% G0 F+ p7 N# F( ?8 B
- digitalWrite(_DataPin, LOW);6 M/ W* q' t. i, s$ o
- delayMicroseconds(980);. f* `$ F( W5 I, g. S7 c
- digitalWrite(_DataPin, HIGH);
9 u% T; [ h. i/ w" N' h7 l+ k
{. n( H$ H7 o- pinMode(_DataPin, INPUT_PULLUP);5 w. ~4 }3 { W4 l
- delayMicroseconds(10);
" Q* s5 |7 R$ M) Z9 u
* e. U, J. @9 k0 m B( \% y% Q- time_out_flag = millis();
5 b" n4 z/ p4 Z ], k2 H5 B3 [ s* M - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 p1 h5 M( y9 t - , `5 E9 L/ ], o! J' M$ G6 y) N+ S
- LOW_level_read_time = micros();/ H" ]5 g' n8 P4 R/ i. h8 C
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out0 o2 D/ o8 V9 z( ^: A6 Q/ c- K$ z- v! o
- {9 o! l* F) k( `
- return 0xff;* {8 {) W8 g2 v$ V2 K9 j$ I
- }
: R- o E- D/ t( K; w: ^$ E: G
1 g8 q- M* G& Y; ?- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 K% L# @& S l, a+ v
- 3 W1 T: |) f; o4 P. ~, F c
- HIGH_level_read_time = micros();
7 Q9 H2 Y7 \' K3 n - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
8 f3 d0 O- K5 m6 @4 m. }( \7 q - 2 z0 M/ }3 g. c% l: y7 e' m
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 r7 Z8 E* }# S2 t - {
~& t. p7 W1 W4 |2 j# c - return 0xff;
# H" p# I, w3 _* o- o, ` - }* a/ U0 S# `. r6 |' Z. J. C& Y! ?
$ u( s( s( [% t* p- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
1 ]% \0 B6 M# k, G - {
" K0 \/ W- C) `' |. K/ a - return 0xff;0 S4 z) O4 _# ~7 r
- }
' Y1 E$ a! K# |5 j! n5 F - 6 v0 A h5 E3 _2 K$ V# G: I, w
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ t2 P5 u; M0 {) d; s/ M' t3 H - LOW_level_read_time = micros();
8 Z p) P6 ~, @+ } - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
1 [6 T: v; S# P! V4 Y' @1 q* l - # a K" B$ T: Y/ k
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 ^0 N5 U" Q& `/ W - {
@- }% c- ~1 m7 h) B" T - return 0xff;
9 R' b) L2 v* A3 l5 B! T: b( C - }
J6 q6 }9 J8 D1 ~3 s; G+ \, d, D - / g4 l; t, x: f- v
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
9 Z$ z& g8 r% S/ x" U: j% s4 X - {
& r6 |. e; M; ]( i; W1 z - return 0xff;) ?- P4 _& c1 f3 J6 v' ~
- }/ ^9 A2 b! j: [( a: u4 p
- ! X, P+ R, N1 |9 C
- for(uint8_t k=0; k<3; k++)
* R; x$ m5 @0 n k6 I% c - {
, Q1 q6 Q/ V, j3 g6 X - Sensor_Data[k] = 0x00;
: }2 [& P4 o: ], O7 J7 U
# u: A/ }! d5 l( Q- for(uint8_t i=0;i<8;i++)7 g$ g4 `* L% [& A9 L1 ?* t/ e
- {
1 ]$ D9 z- s% ]6 S - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
. d& ?! n, p3 c% t1 f+ c - HIGH_level_read_time = micros();0 w0 q1 F$ ~- K3 y3 x5 S
- LOW_level_read_time = micros() - LOW_level_read_time;& x' Z& S2 Z2 K+ m
! r4 s2 {9 U5 U2 u( C, K- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )- D" I$ t, h+ R) c4 _/ p
- {
8 z* B3 {9 S3 M2 M& [ - return 0xff;
' h1 G7 f- o2 G' Z: K9 ` - }
1 a8 G2 g6 X- V y - 3 t' |5 L. H/ J6 j! X
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
' \. ^1 H4 N8 D- c, a7 {6 K3 U - LOW_level_read_time = micros();( A, M) P7 p/ A0 t5 O' h! \
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level1 f; t( t: r! n4 g. l+ b
! N6 Q& P+ m, O+ T+ w3 Y- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1 s) S! A% h9 ?8 i- k
- {7 C2 ^' _, X% P1 P' r) x7 O
- Sensor_Data[k] |= (0x80 >> i);; ?1 L& W N, [" Y& G) E" `3 A
- }- _) U. @9 ~4 R7 e* b! o6 u, c9 D8 e7 O
- else if(HIGH_level_read_time >= 100)! q7 p) k; Q/ X
- {( H3 e, ?9 Z+ f& M6 F: y
- return 0xff;
, @! X& R! k: S% ~" z' \$ L - }, d0 y; Q+ l- m, u
/ D& o6 g$ G2 n- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
: w9 h9 [( H. S0 M; d" ` - {
' e) G) w3 c( p q3 d/ E - return 0xff;& ^% e+ Y- h) b3 z D
- }4 D n6 U1 K$ C. a- y+ F4 ]" J3 E
- }
: [' j0 B ^, l5 w. D9 i; s - }" u6 o2 g6 N1 u3 M
$ r) Z) e$ d6 z! M& s- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
7 l& N9 X5 W' M+ B - HIGH_level_read_time = micros();* S8 U# H4 ~, ^: K
- LOW_level_read_time = micros() - LOW_level_read_time;4 `$ p7 C: d6 @
& g4 E4 e$ n. o2 w- L! h- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )6 V5 _3 F, \( M; K* [
- {
/ N; I; N, F' i7 Q6 s/ \, h& | - return 0xff;5 G% ]( S5 [; `- k/ ~# V
- }8 X \" R: I9 |; J6 c- M
+ U# p D5 x' g3 Y! r- pinMode(_DataPin, OUTPUT);
. k3 O4 l$ d' i - digitalWrite(_DataPin, HIGH);1 O) s; F |1 B r" i( W" _
- 3 N; ?; Q7 V% k( L" V6 h2 r& {
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
+ }1 }3 }- x0 C9 L - {
* t6 o5 Z( s% Q- w - old_data = Sensor_Data[0];; _( n( R6 D3 B: _7 `- ~3 @3 ^
- return Sensor_Data[0];
/ j7 m6 M: j' Z5 p) G' m - }1 W" w7 b$ {, ~3 }) ?
- else: C6 L0 q7 y! c/ q! C1 m
- {3 ?' K* C9 A% L4 T
- return old_data;
- E% _, k0 I* p9 r" W - }7 f; u3 j. b. v. ?: t
- }4 O. m& y4 {% e- v F3 ]
複製代碼
6 K! T8 s3 [; L9 n" Q3 `MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
: J' J- N- s& K6 b" D
* c3 c ^4 m. i0 p J, G+ ^
|
|