|
|
9 J) M9 J( P* l
- #include <Arduino.h>4 _- R: @. c* p1 |3 G( W6 D; b2 d
- #include <MeAuriga.h>
. P, i2 E2 F% L - #include "MeLineFollowerArray.h"
, B6 y; g0 G3 s$ r0 T/ y T - $ X' s2 h; I% s/ K
- MeEncoderOnBoard Encoder_1(SLOT1);2 f4 a f0 T- P9 A1 c
- MeEncoderOnBoard Encoder_2(SLOT2);0 ?- l; @5 h. K
- MeLightSensor lightsensor_1(12);: k v# G& k( f- Q, ^, P8 k; a
- MeLightSensor lightsensor_2(11);
0 e7 a8 ]) d6 c% P3 x - MeBuzzer buzzer;7 {7 j9 f o. ~6 h$ o8 _
- MeLineFollowerArray linefollower(PORT_6);% F$ I* s$ }5 @
, L7 S& L; v8 F& R6 j7 p- #define Error1 1
( i; p5 \8 _3 P% I - #define Error2 2$ S7 _ V, F% S
- #define Error3 3
$ j. C# r; O( M9 e
# d! E/ u3 K% M$ `& Z; u% l3 p7 _$ }- #define Kp 15
/ a; `! U; q' q2 T c. ~ - #define Ki 0.15
# y( Z4 `2 {* S - #define Kd 0.03
; H2 A% |& o) k! c3 C `; D
+ p5 m- `! N9 g& w1 ?3 ?( X- uint8_t sData;8 Z4 w* |9 e. \( G) s6 a
- uint8_t D1;
/ J9 g: ?: ^1 |2 k. p2 A9 F' x - uint8_t D2;
. _' S, d3 j3 \( k - uint8_t D3;7 g! v; ?9 ]( S# s
- uint8_t D4;' n+ W1 V$ u1 b+ j% L5 r
- uint8_t D5;
7 L4 L2 r% d! o2 B - uint8_t D6;. y, F% A) a) b* r. m# R
; ^! `* _2 D/ J c( U7 ]: f4 q- float previous_error = 0;5 y( j4 U& R' y$ D1 l
- float integral = 0;
5 Y9 B2 s/ o: I& f) s4 C - float derivative = 0;2 y8 D5 b7 a0 i7 I; y2 z
- int Speed = 160;! W; J& k4 G1 ]
- float output;7 N& V9 g" E. I) d9 {' B
" w m* G5 ~) a2 a# v- byte Left;
0 Z1 u, p1 q, G( [0 f) {+ Q
; a6 ^6 X' m4 @% Q$ _# E+ O7 b- void setup() ) ?0 X* w7 ~9 g/ c. w4 }* r2 d
- {2 V& V; ~# I- Y
- //Set PWM 8KHz! L9 \# B& L" o: ?4 A3 N- S. {
- TCCR1A = _BV(WGM10);9 n8 ?( `; l f+ H) P& W( n1 d# n! z
- TCCR1B = _BV(CS11) | _BV(WGM12);
3 C& H- U9 P2 B/ h3 D) Y( a - TCCR2A = _BV(WGM21) | _BV(WGM20);- A/ L3 c! q) M J. O
- TCCR2B = _BV(CS21);& y7 r7 x8 o' g S# C/ B# g
- Serial.begin(9600);+ y! C( d% \ m1 X) s X* F1 ~
- buzzer.setpin(45);# j6 q5 ?! c0 h% C0 u' _4 K
- }/ O9 U" C5 Y* `
7 k+ g2 p2 T) o! q) E q, q" x- void loop()
9 o4 M* R0 Y& Q( U - {- s3 U% D2 m" m
- while(!((lightsensor_1.read()) < (10)));+ I' a: k& i+ X
- buzzer.tone(1047, 500);! Y& S( Y) @" D M
- delay(500);3 o* ?8 V* U, o
- do1 D) ?# } z/ }
- {
5 u( Y' o9 _; c- k - int Err = getErr();
. j9 T! J9 Q) W0 l) T" ~ - if(D1 == 1)2 r/ r8 A* k( k
- { A* V {6 L5 L7 E
- Left = 1;+ t! R) L$ S3 a
- }* }3 U& U$ w; x7 a
- if(D6 == 1)& I5 ?( c5 |) ^" a5 S' X
- {" u( e6 n& v W+ t2 p7 v3 M6 }
- Left = 0;0 M `3 e" T" l
- }6 |+ d4 G8 g8 X
- if(Err == 99)
2 U! K X5 f# ?- e2 W: } - {9 y8 j4 [+ N" e4 j
- if(Left == 1)
4 k4 D$ |& J, |' L/ Z1 y - {
. c: q' H3 d0 B - Speed -= 5;
+ y" R+ l+ x3 I# J - moto(0,Speed);8 @8 \" e3 b2 L" ]% { C, P
- do
: P9 o$ C2 i$ V - {& U" g) ?4 N/ {7 s [& H
- Err = getErr();
/ z( ?2 i: s7 { j - }while((D1+D6) == 0);
& D. F' q+ D# Y9 m ~) ^ - }
# X+ o; g! Y4 I, C0 W2 n) S; S - else( @: S9 z" X# ~' V* Q
- {$ ]: p3 n' K/ l' E
- Speed -= 5;# d0 _& O0 \+ z0 O e% [' B' w
- moto(Speed,0);
; X! m) q3 T# A, R( k9 a - do, F- J! m% F1 e V6 F. j( }8 z0 a% Q
- {! w& A1 ]* A( R
- Err = getErr();, e0 x5 Z/ g; Z, V+ Z
- }while((D1+D6) == 0);
. S& f7 W, F7 ]4 e! q - }% Q1 F. u) ]; S- d2 I4 m& n
- }8 c- n' M _, w. Y
- else
5 S5 f% I8 x6 M/ ?; Z& U - {. P' b( }% q$ x# q8 O0 a
- if((Speed < 160) & (Err < 2)) Speed+=1;
3 E- W# {% i4 z7 d8 d. }$ z6 D - if((Speed > 100) & (Err > 2)) Speed-=2;
- Z+ D4 u* v& \6 i* t: u1 i - integral = integral + Err;
3 w9 e0 a& t6 q9 K8 ] - derivative = Err - previous_error;5 A) |6 @( Y [9 Y
- output = Kp*Err + Ki*integral + Kd*derivative;
+ y3 w; G$ u. N6 D - moto(int(Speed-output),int(Speed+output));
: c& G; v$ ?- N1 { - previous_error = Err;
. ~/ n/ ~1 p, V2 r. ` - }# s' K/ d6 w9 G
- }while(!((lightsensor_2.read()) < (10)));2 V8 _& D+ G9 _3 k- j+ C
- moto(0,0);
2 b# C' G% \3 b2 V/ v3 l- i r* t - delay(500);
8 \, Z6 _- r, Q4 d" E - buzzer.tone(262, 500);5 i% G$ [1 S* C; b9 U! }' b
- }$ K3 u( B! R# s
- * h4 C# P0 X, g+ E
- int getErr()
; c. H6 v5 g' z3 A# [8 P: r - {
( l( T6 L3 \/ P3 g. X( z7 s2 y6 s - sData = linefollower.getValue();
% q$ D+ g$ X B - D1 = ~(sData>>0)&1;
8 T0 [% X6 R/ u+ Y, z - D2 = ~(sData>>1)&1;
5 F& V+ Y% v% p& _+ b$ A' k - D3 = ~(sData>>2)&1;1 Q9 s/ ` N/ ~) @; k
- D4 = ~(sData>>3)&1;
& y) e# x; i& l: u7 M - D5 = ~(sData>>4)&1;2 h# B3 ?# n0 b' U3 q: x
- D6 = ~(sData>>5)&1;% ?3 Y! y, j0 I+ H& Y
- int downD = D1+D2+D3+D4+D5+D6;
. K1 B- Y$ a/ A# z8 H( R - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);3 k" @7 [6 a4 b1 n$ S5 U7 V0 s% x" R
- if(downD == 0)
+ T: o/ d0 w! k - {
* ^* D( Z9 Z7 o( |6 ~ _" B - return 99;& Z$ s6 I8 t u; p6 R$ i
- }
! x$ c) L3 C; D" b, { - else
" ^5 P* _, Q9 g8 c# l/ q- N) D) ` - {
8 E9 c8 x% e$ E! S! _ - return upD/downD;
% s% ?2 ]; O: ]/ ?2 d - }
* v- R" b0 i& u' S+ {1 d - }. g4 l5 I a5 @- ]4 d. e
2 j$ X$ m* p6 A& o* V1 W- void moto(int Speed_L,int Speed_R)
& K& R \: U: K - {9 B- I$ d. |; K4 Q/ h0 e+ c
- Encoder_2.setMotorPwm(Speed_L);. c* L5 {, c; V& T
- Encoder_1.setMotorPwm(-Speed_R);8 i( E8 b+ \" k) v. k# O
- }
複製代碼
3 M% G, R+ f' Q1 H$ e* S- DMeLineFollowerArray.cpp9 X2 T' N6 P0 @( m$ B4 Q0 R* A
- #include "MeLineFollowerArray.h"
2 o9 A0 _2 z* W. x6 @ - . ?# d: V- m* Z; H4 g0 W" w8 g
- #ifdef ME_PORT_DEFINED7 R8 W# H/ M3 t
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0): D6 p- y4 k W2 y- ?
- {
( |5 t2 I' g. ? - 0 a8 S, A- f/ B! ]3 ]
- }
: S3 n/ j7 Z, P; V4 @) |: m% G - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port), \$ j- l3 T/ w
- {( @7 t/ J1 j7 o9 m; f
- _DataPin = mePort[port].s2;! q9 _$ `) ~4 [1 E: J
- pinMode(_DataPin, OUTPUT);
+ ~' u# @0 \! ^& i L. R7 x - digitalWrite(_DataPin, HIGH);; H6 D- d$ @8 A6 w; E/ n; l
- }+ S5 {) k6 P& {6 j. A, s" F
- #else // ME_PORT_DEFINED5 A+ h# J5 X5 j6 D
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
* \" y6 L3 ^4 M/ i# P0 U+ i. F - {
. k; n. h' y2 z - _DataPin = pin;
* H5 s& Y0 z$ ]$ r - pinMode(_DataPin, OUTPUT); , s, b; ?" [. p
- digitalWrite(_DataPin, HIGH);2 s! ~7 I f0 A
- }
4 G7 {7 U6 h# r7 m/ j - #endif // ME_PORT_DEFINED
( ~1 ^$ T. t* |* n5 T - : K4 o; S' h) K
# ?/ ^2 c& N- C, ?- ^; N- void MeLineFollowerArray::setpin(uint8_t pin)
' F- @5 j" T5 G0 @ - {* r' ^2 z3 U7 C6 k n
- _DataPin = pin;
1 N5 y2 E1 Q6 l' S: M2 W; l$ A - pinMode(_DataPin, OUTPUT); & }) R& R( a3 `! _, |7 @" Z
- digitalWrite(_DataPin, HIGH);1 k; m! R2 Z" S2 v, x+ j
) V5 c" P! ~5 w# h; x- #ifdef ME_PORT_DEFINED i/ I5 c% B: L" M( A0 {
- s2 = pin;
3 f7 @; r) D- {' Y' [/ Y - #endif$ a# m: U: z9 J7 B+ x
- }
! Q1 {- g& m) S; X' L8 b3 Q - 6 S D* q* y1 d/ f( V% E
- uint8_t MeLineFollowerArray::getValue()
8 R6 M8 q6 y$ b/ {5 {/ c - {
9 c6 ]8 f0 x, V - uint32_t LOW_level_read_time;' R5 X* g# S X8 n3 N9 h5 h8 ]
- uint32_t HIGH_level_read_time;0 D0 y2 t" \2 Z# j2 G" M, P
- uint32_t time_out_flag;* ~8 q! ^7 [- p! \3 j1 @
- uint8_t Sensor_Data[3];
* Q( H' _ ^2 s - static uint8_t old_data = 0xff;
8 U3 y% R+ ]/ Q7 M2 ] - 8 K( u/ t3 Q7 W* K* _# M7 Y
- pinMode(_DataPin, OUTPUT);
" g+ \( ?5 V" z8 w+ p! w - digitalWrite(_DataPin, LOW);
: P5 r& L7 a6 C. N: o - delayMicroseconds(980);- F# [: o1 W. d7 C
- digitalWrite(_DataPin, HIGH);& S: c) V& b7 K
- + T$ ^+ @' O9 P9 v' v0 c
- pinMode(_DataPin, INPUT_PULLUP);
1 ~5 m5 A. X& x2 l( x: R' T' d - delayMicroseconds(10);
: E3 P# Y' S: u5 Y# p - 3 X+ N# V; `" C
- time_out_flag = millis();
* @( r$ q5 R; u; x/ ]$ c+ \( C7 i - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );! K7 J- E1 b. u
- 9 C5 P- U( L1 j5 o; `0 a' l0 Z8 P
- LOW_level_read_time = micros();8 ?, \, g) }" F4 M' X. t
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out. b; @/ D/ X5 \
- {, @' L) ?+ N& V8 r; r
- return 0xff;
, B) c& |5 {- L' O9 [! b - }
# ^ I3 R' B( m - @* t9 N% w5 w0 f3 x/ ?5 Y
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- A6 W: ~8 ^9 K J# N g# A9 E9 f
- ( F; h' h: \: N3 ]: r9 Y |
- HIGH_level_read_time = micros();. m- p1 K/ [ N& _7 \ H
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
' h5 Z( A4 D2 P$ Y8 A - 6 v' S( k% f% e) j x
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
" ?) h1 L, V- z$ v3 B - {
3 w9 T! _/ Q# Y9 g( Q6 A c - return 0xff;& r* Q) m. x1 i! w
- }% i% M; h! T, U$ R. q
0 z% |, w0 m, ~. D; s; v3 c. y; D) @- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
6 {* {: V: Z" w; M( X - {
( B0 \4 S) T( ]! Q - return 0xff;
) M9 U5 w2 M+ G# o# q- R - }
{* F' S8 u+ w' d5 j' f
" t4 y( E! o* ^' X! ^! n! f- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
0 `' e4 v) s" I9 b# P# w7 a - LOW_level_read_time = micros();
7 O& y0 `, @- U* b1 C7 [ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level) I. w9 N& V4 f) s7 y
1 C! t; Z$ `: y U- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 F6 N. g1 o4 S/ ~4 o$ I. ?9 a - {) A9 ~4 |# H2 Z* p& q- \8 W" n3 E
- return 0xff;
! j5 d i7 u. a% v4 R8 @ - }2 \' N5 ?, J$ y' Y* R# |
- ! e# H' _& I ]" E4 o
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
+ i# `9 r* u8 ]9 ]) y' L - {$ I1 ?& I& q1 C) A( |' r
- return 0xff;, d$ P& v: ^+ p6 y5 z2 L2 \5 r w
- }
- c: [+ L2 }, c' V; G( {) g - 8 g" d- y( P) d% Q* |
- for(uint8_t k=0; k<3; k++)
7 ?3 i$ J% \; x8 y5 W) h - {8 D% _. z' M: Y; u* K1 h# w& o; w
- Sensor_Data[k] = 0x00;
: P& \: x+ `2 h8 t8 i# K
* L% d+ Y. M2 C! r8 l1 D( e- for(uint8_t i=0;i<8;i++)
2 \2 e3 i5 B8 d8 J - {
2 j* F7 w8 e6 @1 s& O, o. P0 s - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
/ }7 v5 i! v2 |# y( w ~ - HIGH_level_read_time = micros();8 Z6 `* P- a% s9 m, _
- LOW_level_read_time = micros() - LOW_level_read_time;( d+ j1 E- q: }3 B
- + r, I7 Y, ^8 j4 c
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
# U6 `" B Y: U - {( X+ n3 [& w9 R0 U2 I
- return 0xff;; s0 f! m2 W: J; b9 D# ^: }
- }' {# ?" a: D' f; O
- / b2 l0 M" x1 Q' S
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
" z& C9 H3 F' m4 [2 j* m - LOW_level_read_time = micros();1 C" G7 s9 j# ?/ w6 s9 q
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
7 `$ e# i e a2 v - - J( `$ w& g0 C A
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1% U7 j. D* X' [* k$ ^# l) V/ P
- {1 z1 T5 @$ h2 i' Z1 r
- Sensor_Data[k] |= (0x80 >> i);
; G. r& k- ^0 A$ B7 | - }
; r( n2 G; U% k z. Y& Y' Q- T - else if(HIGH_level_read_time >= 100)6 \7 l) A4 o' a) l' W7 R& @
- {# i& b% g& r2 i3 A3 J5 k7 Y+ g
- return 0xff;4 m! ?2 i+ G+ C4 L& w
- }$ _! H( \5 e3 D8 a% C
; A/ Z, u8 h; v+ V; G( ~- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out). ?, x- t4 Y @: @3 ^8 P
- {9 M( _$ ~% { E6 k' Y% i5 W+ F
- return 0xff;7 X f: l9 o. X2 c5 C
- }( V4 E/ f7 `4 E( q1 v; c
- }8 Y' k1 C) p; R4 Z4 s& c5 W2 [$ j
- }
$ P. c) m1 F: T
: ^+ a/ s/ ^" U- }8 B- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level* m4 l! M. k) J. d
- HIGH_level_read_time = micros();+ E% v$ C* X# w4 V7 t
- LOW_level_read_time = micros() - LOW_level_read_time;( ]6 j( E" p: y( n% B# L
- 4 _: i: b+ }6 b: ]5 Z
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )/ {2 o* Y3 P3 o8 N7 c; f/ J- y
- {
) e( ]; @1 y; o+ E' g- ]6 }" c# L - return 0xff;% r" V4 V2 f, L5 t+ j# q& i
- }
& O& s9 o0 q% L! P2 o9 X. ~# o1 S
- y5 }7 b5 F6 U% w- pinMode(_DataPin, OUTPUT);' q2 ?, X# R1 F# Q5 j6 R3 O& i* A
- digitalWrite(_DataPin, HIGH);
, B* B* m, L" c4 F$ {1 O, U, n - * C6 R, `! i( m' p$ f0 S# M
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))/ b! z3 P$ U8 U# P6 m8 j8 m/ i
- {
; D& [" Y6 J8 _# D& X; ^1 Y - old_data = Sensor_Data[0];
& R. t6 h3 `8 X0 n* o; w - return Sensor_Data[0];
9 h6 |" v" ^ E( B - } K9 Q M: E) H. N: v& Y" Z# R
- else+ E. e4 d5 e2 o4 ^, D5 l
- {
* o5 e* d! P: s0 z/ s" K+ I - return old_data;$ E6 \0 _3 V4 G- Z! E: [ ?
- }* H, ]( X( H$ O0 t. H* N0 |
- }' S) H! @4 r9 e: _, ?0 e
複製代碼 3 n5 Z9 p* H8 Z, r$ x+ ?& N
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
2 f2 ^0 X( l# }7 p% _3 J
+ y, f- F7 r+ Y% I
|
|