|
|
! s& Z9 C6 W1 I8 r; t( j0 @, m$ j
- #include <Arduino.h>% ~; P# J* q, ^6 P
- #include <MeAuriga.h>
2 I' {# S9 y6 G/ f% w - #include "MeLineFollowerArray.h"
9 i4 q; L$ B+ s1 D* k: p2 l - ; I$ E! B1 B( `* r9 \
- MeEncoderOnBoard Encoder_1(SLOT1);
0 W/ }% R7 }% c/ g5 |5 G - MeEncoderOnBoard Encoder_2(SLOT2);
& g% l- z& r6 D - MeLightSensor lightsensor_1(12);; J# S4 Q% L2 Q. P$ v
- MeLightSensor lightsensor_2(11);" S4 C+ ?' m8 f: a9 s
- MeBuzzer buzzer;
% h. g; f3 y+ H L; E! W& M - MeLineFollowerArray linefollower(PORT_6);
: j1 I# C, v! _: ^1 ~4 {( ] - * I) I7 J5 _5 p) f
- #define Error1 1 j9 j# }. Q7 e' d! y A; v8 |
- #define Error2 2, \; ?& g' f3 I( s3 i
- #define Error3 3( A. [2 c+ X) k! W1 {* h
* B) ?* W& h, P* Y% N- x2 s0 I: v- #define Kp 15+ S+ r, m9 y8 G: H5 A
- #define Ki 0.15
7 Q( c' {* Z8 O0 t, y6 a! m; T - #define Kd 0.03
3 y5 {- O w5 h
0 q; \# U2 |* G' y/ D- uint8_t sData;
1 ^3 x) b% h& V' R5 c! r0 g - uint8_t D1;
* C- ?- Y" @ K) l: n - uint8_t D2;# R2 n/ Q& g! B. r$ L& I* I6 L i
- uint8_t D3;
- X3 E' H. w3 i" ? - uint8_t D4;
7 j' O8 g; j) s6 F+ \6 \0 @ - uint8_t D5;
: N, U! d, \! P7 V: X) n) P7 d - uint8_t D6;4 _" J& {' _6 z* Z7 P
& B# A; I4 t' H: Z" b- float previous_error = 0;
- j4 N! K4 |# Y% O - float integral = 0;
, P6 @0 ]9 ^ y; l6 B - float derivative = 0;" Q* o, ?5 ?: h% ~
- int Speed = 160;
# [5 C1 F/ [0 S2 H - float output;
2 B+ Q& z2 |! [+ x# g
5 F; K5 [+ B. {. w- Q1 g- byte Left;) B+ f; w1 W* X6 `2 {1 ?# C- @4 j
) H4 a @: K$ k \- void setup() $ K7 E7 ^! m$ f2 q X
- {
9 G) t1 D8 d& l6 R3 {& ~8 ` - //Set PWM 8KHz8 K& U" F U. n, D H
- TCCR1A = _BV(WGM10);- }/ {" @# w; O
- TCCR1B = _BV(CS11) | _BV(WGM12);
4 P/ U; E+ {! S% u& E0 E1 c - TCCR2A = _BV(WGM21) | _BV(WGM20);
/ g b, `* i9 x5 b7 R - TCCR2B = _BV(CS21);
; L5 n4 h! ~+ G# T; q, @) L! n - Serial.begin(9600);
1 Q' h. f' e/ K9 N% b% Y5 v# W ?$ Y - buzzer.setpin(45);
) ]; u4 e Z9 O- m - }* x: z q+ F0 N3 N, X
- : D6 i7 G8 I5 @: ~$ R" q: @! B
- void loop()1 J: Q3 G. ?8 S6 H9 ?( q
- {
- j1 e+ ?0 h( I# _! z - while(!((lightsensor_1.read()) < (10)));0 I3 |. e: ?2 x" O: h0 x
- buzzer.tone(1047, 500);6 E, q1 I& {$ ?% n: v
- delay(500);
1 N/ h7 o* A* @9 s0 U( Y - do7 i0 ]. }( I" z
- {
7 ^4 p5 y$ P0 g( Y - int Err = getErr();% x w# C! w) b- \' q
- if(D1 == 1)' a8 d9 Q* N! I# r
- {( r+ s6 k" F3 i# A( D% R# k7 n! F
- Left = 1;. s: n- ?+ M) ~/ d" q
- }
2 n( g) C, q+ A - if(D6 == 1)+ r7 A3 A$ K; |9 u* e4 R
- {
# m* P& ~0 ]7 N3 h1 g - Left = 0;
, i# o/ [! _$ \! O - }: r1 }* `3 y. V! s/ V
- if(Err == 99)
$ E! t3 b( S. X - {! p, ?( i7 C4 ?
- if(Left == 1)
: ~9 `+ a6 t9 i - {
1 y2 w) j9 L" I6 W - Speed -= 5;
7 m% i4 |7 o: B6 l" o7 Y( G - moto(0,Speed);% b% a" [( V. i! m1 q
- do( M7 M3 ^0 p( H: O7 O
- {. d `3 i. G8 @! ~: g
- Err = getErr();
3 t4 S& t' s" ^% I, ~* G3 v C - }while((D1+D6) == 0);
/ g) r* `6 n/ n$ `* r - }
" ]. k: F$ P. E+ B$ F6 j - else
: g5 I/ g# k! {$ Y - {* D9 x# `' _. r' ]# B2 n9 l; D
- Speed -= 5;
9 ~5 }$ t9 z: v8 w. W; }+ ^6 _3 @. B - moto(Speed,0);
/ l4 @* ~* j! s1 A( N - do
/ L' V* Z" `( x - {5 n# c5 Z8 r7 x- i
- Err = getErr();
& u" m x0 t, _; | - }while((D1+D6) == 0);
$ f! r, F( p1 i - }: @1 T( M/ Z k7 y' A1 r" u' q% h
- }5 | V7 Q3 x5 h( n" \5 @
- else
" N+ l3 E- d' q: Q4 ~) s - {
. Y8 d, [# F- x. \1 x - if((Speed < 160) & (Err < 2)) Speed+=1; # F" a8 x6 f3 ]
- if((Speed > 100) & (Err > 2)) Speed-=2;
p4 s" o# N6 }7 C+ f5 R4 j$ r$ j - integral = integral + Err;. M4 ~2 G) _7 u9 Z) A+ x( [
- derivative = Err - previous_error;" F) S; s8 L- L8 k( U' ~% L
- output = Kp*Err + Ki*integral + Kd*derivative;7 m: ^# A, a/ Z9 z) q1 R G X
- moto(int(Speed-output),int(Speed+output));
: J2 i+ }, n- q - previous_error = Err;' \4 ]+ `5 a5 |" H2 @' \9 P
- }8 H% ~9 r+ Z3 {. ?2 ^! _- n
- }while(!((lightsensor_2.read()) < (10)));
# d" s+ ~4 \* m1 ^ G - moto(0,0);
2 B. \1 z, c' ~% Q0 w( P/ e - delay(500);/ h: P- l2 h' g
- buzzer.tone(262, 500);/ h' k/ M {8 B) K- W
- }
* b- f8 p, T1 N3 e7 m
+ A# J" i0 o; E' U# S- int getErr()- |( @: i2 {% K- ~. [/ }
- {
+ m" Q9 W/ l. h1 x - sData = linefollower.getValue();
2 ~$ g. [5 e& t2 l$ G3 i - D1 = ~(sData>>0)&1;
+ e+ D( a) ~- y9 D( H) r1 ]! e - D2 = ~(sData>>1)&1;3 m- r. P c, F+ o
- D3 = ~(sData>>2)&1;& J8 s" m4 ^0 r
- D4 = ~(sData>>3)&1;5 m, H. E: {% E: ?: w
- D5 = ~(sData>>4)&1;
8 [7 Z# ^5 J* V - D6 = ~(sData>>5)&1;
2 z9 L) |1 O" ~ - int downD = D1+D2+D3+D4+D5+D6;& C( W, V" L d& S
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);: L+ |+ J1 {/ C+ b
- if(downD == 0)
& W1 B$ O7 N+ l6 E+ N7 ^3 n - {+ z; n, W2 p+ d3 d3 w
- return 99;2 s- i0 e. W7 {7 v
- }* @3 o+ x% f& e
- else' w) J. v4 E# B7 h( m
- {
1 Z ~7 m3 ]& @4 x# O/ f - return upD/downD;- `: ^! y" ?0 B- S% q1 h
- }. o% ^8 i4 w" F7 |
- }) f$ C w6 ^0 @0 ?
+ t$ |+ }, _ K+ d- q- void moto(int Speed_L,int Speed_R)5 f. \4 F9 k `6 T
- {
+ }& t0 H' G$ e+ O* b - Encoder_2.setMotorPwm(Speed_L);% S+ r6 a0 d, @; @
- Encoder_1.setMotorPwm(-Speed_R);
$ U& j' j. ~( c! A+ I - }
複製代碼
* y) N0 V5 S9 Q% B- SMeLineFollowerArray.cpp
+ V$ J' s+ O, x- #include "MeLineFollowerArray.h"5 P R, R: t* }2 a9 R+ Y, l) ^
, y" j9 K+ y8 O+ X$ ]) C- #ifdef ME_PORT_DEFINED
5 y: c# R1 T. z8 E! `. [; Z3 v - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)( R- ]0 i/ b5 t
- {7 p2 s9 B8 n# G
( N3 a @1 L6 W1 z6 a' T3 q- }
' k& c" x/ L% B' o% w' x5 G) | - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port): e1 u; l9 ? L
- {% O4 I; S' W: _0 q2 F
- _DataPin = mePort[port].s2;7 C* w# p) }) x) w, ^& }7 [
- pinMode(_DataPin, OUTPUT); / W* N8 o' @ s3 J3 M* ~2 x! Y3 s' a
- digitalWrite(_DataPin, HIGH);2 I3 c5 |" S% n% @: ]/ R
- }, a' m; J9 Y1 @4 S) i! J; {
- #else // ME_PORT_DEFINED9 I2 f1 C8 T* W) v5 k$ Z6 n7 o
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
* v/ G% T5 l \ - {
+ ]! f- ]! k) m. M6 n B1 ~ - _DataPin = pin;- Q, U- n# ?" Y% K3 ~% O# M; \ n
- pinMode(_DataPin, OUTPUT); : _) ?, H9 k2 S7 q# e
- digitalWrite(_DataPin, HIGH);: m- P l) W8 ]4 Z# l* F# b
- }8 o5 l9 X4 m, |/ c% X* s5 W
- #endif // ME_PORT_DEFINED5 o' i( @( ]3 q* r
- / p. O( `# c: A' J1 ~% }
2 y% D( F- V( ]; M% }- void MeLineFollowerArray::setpin(uint8_t pin)
% }' i6 Y" c- D1 m7 x/ O' L - {
9 }1 b$ J A0 L8 D; u7 Y, ` - _DataPin = pin;6 l/ ^% a5 c1 }" g" E
- pinMode(_DataPin, OUTPUT);
& M$ L9 f, M* w$ m( N2 r - digitalWrite(_DataPin, HIGH);* I9 C$ c8 K$ k
- % G, i9 y5 n! h& `# g! j
- #ifdef ME_PORT_DEFINED
2 R" D, ^; y7 N0 Z" Z - s2 = pin;
& i& A" |% T$ R& Z, K - #endif
1 m( J4 c+ [( q: K5 ~& V: S - }
2 w2 ]- y; H: O1 H - , d( I2 r0 K( w8 h
- uint8_t MeLineFollowerArray::getValue()
+ C' r# T$ D) A1 C8 B: } - {
* l3 U# A8 h' i5 i) `" `; G - uint32_t LOW_level_read_time;
3 e5 t& A; y$ r2 M% ^/ Z - uint32_t HIGH_level_read_time;1 V# ^9 S2 ?- A0 R% u2 v" w- t
- uint32_t time_out_flag;
- Q5 ?& v% F8 |/ L - uint8_t Sensor_Data[3];
, W) y( V* y& l6 Y7 c6 n$ g. W7 c - static uint8_t old_data = 0xff;; V) x4 a1 O' W& M l
/ }: ?5 Q. _( U- pinMode(_DataPin, OUTPUT);7 ?1 x9 [( A K0 Z/ H- G0 I: B4 L f0 Y
- digitalWrite(_DataPin, LOW);
V0 @' F. D* q - delayMicroseconds(980);$ r* W0 a$ K# |3 A+ k0 j
- digitalWrite(_DataPin, HIGH);
4 a$ B6 `$ |& k# R* E& D6 _- z - 4 x2 E1 y% t; j. \, u: r
- pinMode(_DataPin, INPUT_PULLUP);
6 S. d8 B _: b+ x5 C" i - delayMicroseconds(10);) ~% Z) S1 ]' t! r
- w$ P0 z" O& l) g! h4 m
- time_out_flag = millis();# B3 l6 ^( h6 J
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 k8 T& \+ x8 z/ l+ W) s8 N
& f- R0 [- W" w! ^4 }1 t& ^- LOW_level_read_time = micros();
1 Z' Z$ d/ T: L - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out, B6 L& ?7 L2 j( H6 x0 e
- {
+ w% ]( n) M5 u - return 0xff;
/ E! V9 u* U9 }' J9 s - }! E, Y- C+ p; p# { K# Q2 k- @* @
- % S- m" Z+ X' F+ g$ Y+ ]
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
N; i, \2 S' u+ H+ h! `1 q - # u$ y7 Y$ I( q n% A+ U- |( D
- HIGH_level_read_time = micros();1 I2 t; X: Y& m' o6 V2 k, E
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
$ U" o5 T6 c/ ^6 E# Y. O Y' o - 0 j9 q+ {0 ~8 B
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out) `) Q% v) |6 i4 C+ h0 d5 i
- {- T$ ^( ~+ j }" E
- return 0xff;3 m4 X) Z4 N5 f; N1 k8 g. h
- }% ]# s& z$ s2 B( s
- . H& h! D q9 d1 ^# Z( P
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))7 u2 Y( d" v& _2 J: c/ L
- {2 c6 O1 t, W/ x9 N" F, @
- return 0xff;
9 m( _! x& J8 c5 G) s: ?& g q - }
, b" Z7 Q) I( d( a
g/ w, i! E- j$ T& @/ ?& @- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
1 K. s5 y0 n o" A- r b8 `: a - LOW_level_read_time = micros();
+ V+ J, N2 k+ f( f1 F( W2 M - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level* f6 F1 d$ C! Q5 c
( q8 C" I, i& Q; f! Q& |# t- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
( e+ U, ^5 A, g* h9 p, I3 m - {
3 L }! }8 Q- T) y, k - return 0xff;8 m8 B% X4 Z: A5 D: n
- }& U6 [: E) v1 _- [
- ' i7 O$ _: F- ~# g
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
1 g/ d( u# J, } - {
4 `" F" o' R8 h - return 0xff;
# J8 I7 L5 [6 O2 v6 L - }
, G) _% C7 _/ A5 q% A% @ - , k4 z7 F1 o3 O, U5 e A0 H
- for(uint8_t k=0; k<3; k++)
5 K8 k, e! N) }1 x; H9 L' E - {7 i/ |7 f% v# P7 L% b/ Z" u
- Sensor_Data[k] = 0x00;
6 A8 d6 K$ O# Y1 L! b9 k) Y0 Q6 x - 5 L- F2 K+ X( e
- for(uint8_t i=0;i<8;i++) P4 a% P1 W9 @ i
- {: y" O/ N. [6 W% Y- ?
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level {- d1 i+ O" r. e
- HIGH_level_read_time = micros();& u4 C) Q' H; n* Y
- LOW_level_read_time = micros() - LOW_level_read_time;# W! L" J8 @/ m7 e( G! [: x; D+ E4 q
- : ~2 ?+ O: g2 W: P0 X1 G0 Z
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) Q* w/ G6 m( U; Y/ G$ J3 V& \- _
- {: r4 T; |1 o) W8 [/ o. f& s
- return 0xff;
. k& s) I; m+ N8 A# e# T - }
" O! I* l6 ?# E/ c - 3 C- i. o V6 u; ~' ?; A
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" ?. A: i% G8 K# D! N1 O+ W/ R
- LOW_level_read_time = micros();+ b; ~) H. z) P2 ~- s, Z$ m
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
5 D7 K: R$ Q8 t7 q. I+ l - * U- y3 }5 [5 X
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1; j2 V s* M7 S# h/ f! `% J
- {
; a. K: P! u/ n3 C( e - Sensor_Data[k] |= (0x80 >> i);
1 F& y9 Q+ r+ L* U4 q2 q - }0 `- T) y5 l) b
- else if(HIGH_level_read_time >= 100)
T2 [9 e: E9 @' V: F; m - {
% I ^ h, [2 c# N1 I - return 0xff;
' W5 l2 W" Q+ C( ]* |, E' F; @4 M4 Z - }& [. s- S# L$ V: V
' x( a' o; Q3 t9 d" @$ k% |- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
, v7 g" k! U& }3 o4 `0 V' U" P - {
: h- A5 x3 b! x) @5 b - return 0xff;1 p4 |3 ^7 X: l& f8 T
- }7 X4 J4 M- l( v
- }/ ?- X1 I/ c* j7 C* `: R3 d% s( k
- }. s8 `. }$ [1 d w. P
. x; C( K: A& N! |- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
$ p, h8 Y( ]: r9 F0 r - HIGH_level_read_time = micros();
( c; k. |5 A7 K! h - LOW_level_read_time = micros() - LOW_level_read_time;3 Y/ |6 ]: x' E s, f
- # X+ n0 {3 _9 ` L: \ j$ D
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
7 ~/ s# M7 u9 m& Y y - {% s4 ?$ c @( g4 i7 A" v: s7 P, h
- return 0xff;
# p4 n$ [* q. Y/ q- o* Z' S - }* M' q# u- a5 f
- ) M* p. Q6 L& d0 i9 d1 J1 H$ S! s
- pinMode(_DataPin, OUTPUT);
( S f& i3 N# s' f4 r - digitalWrite(_DataPin, HIGH);0 |3 z5 l; G) {
. h# G3 ~, J% Y$ L- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))2 G: W. @+ R1 x7 i; a5 w& M8 N
- {
+ t Z$ ?5 y, D' B7 E# e4 b1 x, G1 l - old_data = Sensor_Data[0];) z5 v( a" I; s4 ~( d/ m) T, z. t
- return Sensor_Data[0];
/ t. K) f e! m" k; V - }
7 b! ~ q1 @5 P, S5 E6 }$ h P, p - else7 u7 k3 ?9 y3 s# T
- {
# w/ f4 r1 H; w2 W6 J$ S! o - return old_data;4 A% ^" _6 n4 D3 E0 b A
- }3 L) h( o6 D4 a8 x5 G0 I
- }" o) E5 R0 y6 h( F
複製代碼 1 n( B; @1 { G) S0 j( G
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
9 l7 {& V$ _1 A! A/ L
2 C/ U: b) Q( D6 s9 N |
|