|
|
2 v' G- A0 a9 l3 w+ ~ @5 `' `& i- #include <Arduino.h>
, I: m7 \& H% w - #include <MeAuriga.h>
; K# r# g% k6 L i& d u - #include "MeLineFollowerArray.h"
6 J0 o2 y$ R# g9 a" |
8 R0 N* h1 L4 O5 S; A, R6 m [- MeEncoderOnBoard Encoder_1(SLOT1);* \4 j; ~5 Z' @
- MeEncoderOnBoard Encoder_2(SLOT2);: l( e$ I# T5 P
- MeLightSensor lightsensor_1(12);7 {0 J- I. W( P X( L S8 n M
- MeLightSensor lightsensor_2(11);
# A6 B( v/ F7 _* i - MeBuzzer buzzer;
6 W/ w8 `7 k9 E* Y3 f3 `5 P/ q - MeLineFollowerArray linefollower(PORT_6);4 r. i8 T" [' ?( _9 [/ P# }$ m
- * B6 k0 y: L( [" U# b. E
- #define Error1 1" D2 w, r9 H5 n; ~. R; b- o- K
- #define Error2 2
% U1 Y$ a( n5 ]4 h7 ^( T! n - #define Error3 3; L r% f! [/ h, M
7 p6 r6 `* Y' y) x& n- #define Kp 15( a* o6 n! t3 _ p# g. C" M* @
- #define Ki 0.15
4 S- {$ |6 b! s6 ?( U - #define Kd 0.03
7 ]! ]3 r/ f: g0 s! |* e ? - + p4 |0 R6 Z7 D+ i6 a* D3 J
- uint8_t sData;
5 ^( y( N/ J" a1 w; c$ U# Q( Y - uint8_t D1;3 m6 T8 W; b$ R5 x6 x2 R
- uint8_t D2;
9 k9 W: }& s6 D. w; | |, { - uint8_t D3;
; g7 B- ^' q X* t$ `' ^ - uint8_t D4;" r- ?) ?% v4 D' a/ S @+ E5 Y' M
- uint8_t D5;, Y, Y: h% T3 R, q, D
- uint8_t D6;6 \& B1 J/ D$ \: Z: l h$ R1 S
- 2 s. q& ]) a8 W- x m8 [7 Z" |
- float previous_error = 0;
& ?6 ^8 Z. @# d$ j" `: l, C - float integral = 0;
0 {/ U4 N& N( _9 p, y; j$ C - float derivative = 0;
) i: P% T$ O d0 ]) u7 X4 ]( R - int Speed = 160;9 u9 s; n) ~# G( d) R4 V
- float output;" [! R6 E2 t. Z1 k8 A; E5 g& N
2 J, U2 T& y8 R' N1 [& m! n- byte Left;
* V! @7 \ c7 Q" _# } - 2 {, Y/ s0 l5 z8 o9 S
- void setup()
& \8 a, @) u4 x& f - {
( X- c' x( A( s; e3 T5 } - //Set PWM 8KHz! m0 \! M: @ K. r
- TCCR1A = _BV(WGM10);
# Q7 w4 n1 r8 W6 P% Z i - TCCR1B = _BV(CS11) | _BV(WGM12);/ d. B& f: k: u# y2 D; z
- TCCR2A = _BV(WGM21) | _BV(WGM20);5 A5 K( ?5 G# M
- TCCR2B = _BV(CS21);( a5 q5 ]) H1 U6 Y
- Serial.begin(9600);
! E" ?* P7 r2 E; q0 S - buzzer.setpin(45);$ I" c, M4 Z7 v1 T8 f
- }
4 h$ ?+ P! y9 G; ? - ' j0 L, T, [ }5 L5 E" J/ G" J
- void loop()
& ~$ Y. K6 t0 i - {) H5 ^# U" u' V. x0 i" H6 p6 U
- while(!((lightsensor_1.read()) < (10)));
. a2 ~9 s' ~/ D% a% ~8 h1 }% v - buzzer.tone(1047, 500);7 v6 N1 Y" S4 K
- delay(500);1 [) g6 ?( y2 a2 z/ a6 Z
- do5 {. P0 ]+ z9 R* ~ R4 n2 W
- {& q% u, ~* m2 ?$ q# v5 F9 Y
- int Err = getErr();
6 e: Z) h: j2 [# S- ~7 h - if(D1 == 1)
1 O+ ^+ j* w# T& C - {, T7 Q I1 B$ F( i. {& G0 n) `
- Left = 1;$ f' U; I) H7 }9 G; j6 N# U
- }
' x6 O/ f" P9 d$ |) g - if(D6 == 1)
" [' J8 E- f! p0 e$ X* I1 z( B1 Q - {, v6 e* W' |+ c) v! c# v5 J% v8 L
- Left = 0;, f7 C: H; u, s5 G
- }
- v. `& x6 f5 u; T+ b3 K( \; h - if(Err == 99)- C9 p, V: r1 I
- {) J7 g, a* D8 }8 w' ~; Z
- if(Left == 1), f4 }' h1 I9 w* U! {
- {9 ^* M' `, ~ e9 [/ |4 \# e
- Speed -= 5;9 }% U" \! s, v
- moto(0,Speed);4 x- D' S* `& X1 r
- do
" f' p8 a5 d( V5 [1 ? - {
8 ^" K2 ]* k3 {7 T, Y6 i - Err = getErr();) s* ]2 g! @1 d0 l" U2 |
- }while((D1+D6) == 0);
# r7 I4 w+ D0 N$ I6 l - }
- G- _+ q ~. O - else: B* r' v4 |( y; K5 y: p8 B
- {; V( O; o1 x6 B
- Speed -= 5;
" v( O) A6 S( n, u' a - moto(Speed,0); 4 B. d9 s- |, }7 \2 \" N' Y
- do, k/ H- u# X" A+ t |& T- R" H
- {
! j/ f( R% c5 Y4 @- P - Err = getErr();
, J7 V! B6 T& j& G$ I, x1 ^9 \ - }while((D1+D6) == 0);
( U- I" q. E2 W/ q. W - }% Y2 G0 G* M2 F' S) v( ?0 G
- }. t. `5 R" H5 z) {
- else X$ b; }9 G* q( ?$ y* m6 d( G1 O
- {
p) @, e1 M1 W* M) N8 W - if((Speed < 160) & (Err < 2)) Speed+=1; 2 c# c2 q j, ~0 d- k6 Y7 ^+ o# b
- if((Speed > 100) & (Err > 2)) Speed-=2;
6 Z4 ^9 q* u" B1 h - integral = integral + Err;
% X1 ^1 I" P7 g: p - derivative = Err - previous_error;8 }- y' ~- H5 K% o. N. a
- output = Kp*Err + Ki*integral + Kd*derivative;
& R* {5 S$ [4 N" ` - moto(int(Speed-output),int(Speed+output));
9 }! @& c$ @7 O7 g5 r - previous_error = Err;
4 k; [: M0 K- W. H4 z# d - }% U( b6 n3 C p# \# e; j
- }while(!((lightsensor_2.read()) < (10)));0 t' z1 b% U0 h4 Q$ g6 \( ?# |7 ?
- moto(0,0);
. b5 ?& \6 s& c# r/ j0 J/ e2 F! R - delay(500);
+ C, [# g/ x" m4 g& v - buzzer.tone(262, 500);
_) x2 K1 s, ?$ d/ k' T& o - }
% S- r. _$ X) }2 l/ b* J# w - 9 N2 D' m7 r* n% o, A/ b1 P$ S5 f
- int getErr()
6 s9 x- H$ Q+ i' A - {
( [4 x1 b1 ~. d' ~5 I: M* l - sData = linefollower.getValue();
' {: U4 E$ c0 o, W% L, Q - D1 = ~(sData>>0)&1;- A" j- R8 ` t# x* B" @& b6 i$ L' |
- D2 = ~(sData>>1)&1;
0 H b) m5 C' Z4 a$ ?9 L& d2 f - D3 = ~(sData>>2)&1;' j4 c3 U# p9 x- w* Z# I1 i9 R
- D4 = ~(sData>>3)&1; Y9 z! D c# I. Y* @
- D5 = ~(sData>>4)&1;6 n* R h% [7 }- P2 b/ R$ {
- D6 = ~(sData>>5)&1;) L& U6 V& t8 u
- int downD = D1+D2+D3+D4+D5+D6;
( I( ~9 }) K/ Q" b1 \8 d# z9 E g - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);: x& K$ M+ e( R( u# i4 R8 ^
- if(downD == 0)- U2 f5 |( v( N6 e% B& n0 z( Q" V
- {( r# E8 p9 K" k, W* _# w$ v
- return 99;
' t; |0 l; v/ r8 S8 v - }
# o& i- d; M. O1 b7 k) ~' Q9 |' H - else
; F( m3 k# r3 j/ e; Q& B - {
9 B3 k$ {4 `0 |, ^ - return upD/downD;* v/ ~1 l- M N/ E/ c1 \
- }( }+ W# J# r' v W
- }+ W* y8 X+ N0 a; |
" K( s. [- E( G" F, w- void moto(int Speed_L,int Speed_R)
* H; \% l/ Q8 U3 M# I- f- } - {
" m) a8 R; Q8 S - Encoder_2.setMotorPwm(Speed_L);
3 q. _) _3 R+ \' T - Encoder_1.setMotorPwm(-Speed_R);6 M6 `4 @5 z d8 D0 ^
- }
複製代碼 " @! ~! Y1 ^- _" v3 U; ^! |0 z# u
MeLineFollowerArray.cpp, e* j1 `7 c( y Y6 j" \1 q% y+ u$ F
- #include "MeLineFollowerArray.h": ^- G! {( H: Z. A, G5 ~# L6 a' g
9 O' s8 I9 @" n, q+ g3 ^- #ifdef ME_PORT_DEFINED2 t* N5 z$ ?' W i" C1 l- G( k; V! `
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
; ?( `2 F+ p9 e) Q9 p5 ?9 F! b8 X. u7 H - {9 x8 ]: P: c9 W3 i% }! I# G8 \
- 5 Y& D0 O; M2 b9 m: ?
- }
2 T; y' P- H! u: @7 H" P5 J) j, r - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
1 T" U4 D h) i - {, Z0 }# E# Z" K Y
- _DataPin = mePort[port].s2;+ n' u' o# k2 F k% b3 w% _
- pinMode(_DataPin, OUTPUT);
' D! f# v5 k* F - digitalWrite(_DataPin, HIGH);
+ ?8 t, X4 L I0 o: ~ - }
3 k9 Y( D2 N; D& d/ [ - #else // ME_PORT_DEFINED
% c; g. l' o8 ? - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
# T }5 G+ ~* k6 H: b - {4 s* |/ j, a& c/ y
- _DataPin = pin;
8 } i% z" ?1 n' G) ~' B6 X - pinMode(_DataPin, OUTPUT);
! N, U! K; E0 q% y# n+ Q4 G - digitalWrite(_DataPin, HIGH);
* c/ a6 ^0 o, ]9 G. L) `9 {% P7 ` - }
. {2 p; z0 x/ p3 d* L - #endif // ME_PORT_DEFINED
, a0 Y% d# g- P8 M
% O. ^* c2 n% Z7 W3 V- , i; M# g+ q9 c( ]1 U; \ T
- void MeLineFollowerArray::setpin(uint8_t pin)2 i$ u8 [- u5 S# \
- {' f/ i/ b# `8 Z! v/ X
- _DataPin = pin;# \5 d; }$ h2 \0 h% K
- pinMode(_DataPin, OUTPUT); ! j* B4 L& O. |/ k# L
- digitalWrite(_DataPin, HIGH);
; ]: I0 D G b% A: L0 G - . Q8 @2 {3 M0 F8 w+ ?- g5 s
- #ifdef ME_PORT_DEFINED
6 s' p: z; h7 T! M& G, y+ G - s2 = pin;
; [# s6 s* z: u$ \3 x - #endif
" j+ `2 F5 I7 [! x" G - }
# b2 P. ^& `$ {7 Q - ( _& X( X7 o+ l+ X. t) L8 B2 w( R
- uint8_t MeLineFollowerArray::getValue(); ~% p {& J i
- {3 H( t% W' e% M& N% }4 N, A
- uint32_t LOW_level_read_time;
' H1 c$ Z5 J% M1 @- q - uint32_t HIGH_level_read_time;
# I( h" Y5 @0 X6 A# d, _ U- u4 } - uint32_t time_out_flag;+ ?2 N, ?$ A! D2 ^- ~% t: J( O& u. p
- uint8_t Sensor_Data[3];* u. C0 t1 @6 i' U5 F2 ?
- static uint8_t old_data = 0xff;
& @2 |, b9 T. b$ L$ c8 Q5 k - * |- N( P% X6 h; H
- pinMode(_DataPin, OUTPUT);4 T. r3 M5 ~! }( ^
- digitalWrite(_DataPin, LOW);
$ d) o9 U) d* S6 M/ {# q* U - delayMicroseconds(980);- i5 g. E4 r- F( i* `; V$ `/ h
- digitalWrite(_DataPin, HIGH);
; H0 N: A& x0 F3 B8 N - . u* R& o$ D2 g
- pinMode(_DataPin, INPUT_PULLUP);& J( k) q4 u& Q
- delayMicroseconds(10);
6 M, c% ^7 n' n# I9 q3 ~+ \! @ - % \0 }7 \* i u+ [( P+ V
- time_out_flag = millis();
9 s [( c3 H, h0 Z - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( g4 |' v: x" D I8 l6 j$ e
# C1 e G4 f- x! ?( u) ^) c- LOW_level_read_time = micros();: A6 B. o5 g$ v1 t8 a! M5 F) y
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out. n9 w' K0 S+ t, x/ O
- {
. f. p9 j7 p. E; \" C- \ C% l - return 0xff;
$ N) S e! F- E% } - }6 T# @% G) N! {! J* Z @5 ]" n
: p* L4 L8 u3 S/ k1 w; D: Y- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, {. z, A# E" c& _/ B" l
; ^7 u% P9 z. K, a! w6 s- HIGH_level_read_time = micros();3 J/ h+ q: Q5 D0 Q6 e+ n. P
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
$ J5 c9 i+ ?& {6 c - # c) \( C6 V" t( r# K* w
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 z, X. K9 w+ C# o - {, ]4 g3 p. R9 J) G
- return 0xff;
; s4 Y5 ?2 K ]1 c/ E, `: m0 @ - }: b* m) G9 [3 C. t' ~! g: n
|5 o5 P, y* t7 m3 [' d" y- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))/ \) _' a* x0 c6 g: ~
- {
$ [% {" v0 d% ] - return 0xff;
+ l) `1 @! n. P' i$ m - }
, G( q! J6 J' N - - Y9 @3 o% X. f* Q
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; L5 [4 a% H2 H: O$ \ - LOW_level_read_time = micros();
+ u' d; S) d# C, d, a; E6 D; n7 Q5 f - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level. {1 i N9 c$ \0 |5 W
- 2 u6 H( t1 u9 H! D7 l
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out4 X+ l( x( ^9 \; l4 N/ m) H7 _
- {
+ S% _' d( K3 h* ~ - return 0xff;
# M" _2 c4 _: ~$ L - }+ [& p7 C {5 ^3 J& C# `
- 3 h- d/ i; M. m3 D2 _) C
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))( {8 E% L. r" p( Y: D. u0 x+ s
- {
6 U- {& d9 G+ D" i& h - return 0xff;4 ^$ J* `! b4 t& S- n; k
- }0 I) T* o& v0 N$ i2 F! b
) u$ l( W4 F# x* \% e3 M- for(uint8_t k=0; k<3; k++)2 f$ R. q0 P2 c3 L# q
- {2 _8 s4 E' e4 t
- Sensor_Data[k] = 0x00;
3 [- ?/ M9 O7 y; v" i5 [8 R - 6 H4 F" W( z t6 c$ N% \7 M
- for(uint8_t i=0;i<8;i++)1 v. q b- G9 M o8 X0 D2 U& `
- {! \1 g0 T0 e+ H( a9 T+ K9 m# d
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
# ]) U7 g2 G' s# g* ?8 T' [( o8 Z - HIGH_level_read_time = micros();% b$ {: e3 @( i- ~# Z5 [$ t9 V9 e7 R
- LOW_level_read_time = micros() - LOW_level_read_time;* ~% t7 o* ?! d& W8 V0 P
- 4 P. J# r& z- N, d; _
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )$ {0 r. H% b1 x; a/ v7 G, X0 d6 }
- {
3 g6 S& c, @9 G) i6 v8 C# g - return 0xff;
; ^" d1 X" E/ N - }4 M5 R J2 ?8 L" [
9 }7 r1 w) u+ o* \6 `8 `6 U- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );7 N7 h- B6 N: X- n
- LOW_level_read_time = micros();8 I7 Q! j1 A* I* |
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
$ h/ f' M& i. n9 G3 H
$ A9 f# h5 f+ e$ L' s5 B1 V- x* Y- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 16 ^1 x& B! }# h" [7 X: q! ?" v# k
- {" ^! u' A6 `. o! z$ [
- Sensor_Data[k] |= (0x80 >> i);$ K7 w5 R/ U! T' a
- }: I0 J/ k, X5 A: V; _
- else if(HIGH_level_read_time >= 100)+ i2 X/ d4 X" Q, F6 T3 f
- {
) P* h3 K" T/ F+ g. L - return 0xff;6 B+ v( N8 e% E1 \1 t" s# I$ L9 B6 Q
- }
5 W9 A) e+ M* L/ w+ R
! Y0 x! [0 L4 n" L& ?8 J- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)- z% I0 H4 V% _ z1 R, F/ w
- {
1 h* D' L9 @/ [$ T( [ - return 0xff; S8 y0 d: }1 H+ F t- a
- }$ M3 c+ G! m" b( {) b3 G+ l
- }% B& X8 e# ~1 \, y* ~; R- |& n* p
- }
6 `% P5 r B" W, [0 \3 _ - 5 n1 L" {1 R. @ P
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level! @. d" ~$ p$ R5 ~2 J0 ?3 N0 ]9 K
- HIGH_level_read_time = micros();
" P6 n+ k7 H7 H9 @5 J - LOW_level_read_time = micros() - LOW_level_read_time;0 C- P' y( f) \' R, }
- 1 y' `* C) m1 C* F/ S# ^
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
7 C9 m5 P! B7 ^/ \ - {
9 V+ R# }7 d1 L+ A - return 0xff;6 i6 k) t' s4 Q0 w
- }! l& l6 }: ^0 j8 n0 _. U9 X0 m
$ b: B7 U/ \$ Y8 s; g2 N4 |& R- pinMode(_DataPin, OUTPUT);. m u9 e. q4 h
- digitalWrite(_DataPin, HIGH);
% B$ y4 ?/ H# A- `
" {% H/ ^4 n$ h- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))6 J. J3 ^! i. Q5 a. B
- {
/ P) ] I! k' C; O - old_data = Sensor_Data[0];
; F9 y) C9 m" i. _, e+ H2 T - return Sensor_Data[0];
/ G, x) I& c* @5 p - }
! G* V. p U9 T0 e" ?( U/ {( @8 J4 r0 t - else( a: P4 X8 C# j5 g x6 ~
- {
& d& Y; v8 `2 j* k$ B - return old_data;
1 Y6 Z3 O" y( r# L7 I& j# g7 t4 y$ O - }
1 `% e0 @# o; e4 X9 @+ v) W2 h$ u - }) A. r+ b; Q# V
複製代碼
" p7 ?4 r0 t! iMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
2 F3 @& C+ N- k' j
' Q! b+ h0 R0 e$ O" X6 @+ A8 C8 o# }' m, ` |
|