|
" Z% Z8 l5 p) D2 K9 u7 \
- #include <Arduino.h>
: @4 h' P" P8 A4 X5 K# p - #include <MeAuriga.h>
7 e5 V# `& ]( t" k - #include "MeLineFollowerArray.h"
8 v8 W4 g6 x+ g/ m; b, t
# W3 _! I5 _7 Y- MeEncoderOnBoard Encoder_1(SLOT1);/ @3 H# M$ i3 j( s
- MeEncoderOnBoard Encoder_2(SLOT2);- W9 o+ \" ]+ A! ^: \
- MeLightSensor lightsensor_1(12);" P3 T% ^5 h3 ?0 P. s
- MeLightSensor lightsensor_2(11);4 n4 W& C/ K3 B( \5 F. j0 n
- MeBuzzer buzzer;6 t, b$ B* S6 J" d" `; E
- MeLineFollowerArray linefollower(PORT_6);' D' s+ z$ J' D& I' F z) B7 h' S
- r p. w' J3 n) _7 i2 A- #define Error1 1
! \' Z, d, {) t4 u# C) d - #define Error2 2
9 j6 J* g6 Y* H4 p9 F - #define Error3 3" u, T7 S7 L/ I: Z9 t
$ E5 c- {* P1 ?& ~- #define Kp 15& O, w+ s. i0 A9 H8 M
- #define Ki 0.15
( C) I( s$ x& O3 R: q - #define Kd 0.03
5 U$ }! p9 z+ q& f
' h# p% B3 X/ w' J }, z" ~- uint8_t sData;/ s8 w; v' [6 k0 \7 A
- uint8_t D1;
$ I7 Y/ N8 s; d' P3 I - uint8_t D2;' ]1 h/ O N* j1 r. @" o+ P* s
- uint8_t D3;
7 m2 Y4 y& ]: J* G - uint8_t D4;
1 G% j# l- j- `7 _; n - uint8_t D5;
+ D8 ]+ _* N0 c, X5 G& k; z - uint8_t D6;
* ~, v- v( k1 {6 G+ L+ U$ |' ~
& D, [' e- c+ @/ |' Z- float previous_error = 0;
& d. t! x" H# S5 ^9 u - float integral = 0;
; B/ D: q5 T2 c4 ~+ ]' ] - float derivative = 0;
2 n# j% o) T; l$ W8 V) y - int Speed = 160;( n5 M5 t( t, W, |
- float output;, Q, B6 |0 U4 e9 o1 `, o, T
- : E' k% |! z8 I- n6 h9 @+ M7 d3 v$ M3 E
- byte Left;7 M: L$ W3 Q! q" @
- 3 F9 U3 a, `& f- {
- void setup() - x, f3 @, Y5 e' e. ?
- {4 l& _5 y! T8 ?* z, ?* e) j
- //Set PWM 8KHz2 M( ^- u8 Z! l$ }
- TCCR1A = _BV(WGM10);
. ~. L$ L% @1 u+ f+ s& M1 w4 [ - TCCR1B = _BV(CS11) | _BV(WGM12);
1 I& ~' P X0 c C$ n - TCCR2A = _BV(WGM21) | _BV(WGM20);: j( k4 P- t N% N
- TCCR2B = _BV(CS21);
* u7 W: h, A/ R- r. k - Serial.begin(9600);
* y; H. w9 X+ b1 w# P( J9 [* q - buzzer.setpin(45);
' _1 X1 `1 c9 c3 ~% q& \6 a4 M - }9 M- E" A7 r' F. g+ k' o
- & U( I$ {) l* v% }5 A
- void loop()
. ^* ~* P6 t4 C# V1 d0 m8 r, x5 t - {
0 z ?( l: [7 M% @ - while(!((lightsensor_1.read()) < (10)));
6 x, D3 ~" Q) [) h; q - buzzer.tone(1047, 500);
( L0 L! H% p& w( j/ i - delay(500);
1 [0 H Z% s/ q0 J - do" w: ^! ^0 X( X8 B0 `; i
- { {' B0 r, W2 A/ S A+ l
- int Err = getErr();
+ m5 ~ M5 ]9 v/ A# s - if(D1 == 1)! a" `. Y, Y f/ f2 a0 q% c
- {6 ?) R9 |1 k! \6 N0 |
- Left = 1;
% Z* Z! Y( P; e# q - }
* Y8 v0 B# O) V- l( N - if(D6 == 1)
" w" o; K$ r X+ I$ N' J - {1 n0 z+ ]5 k* H6 H
- Left = 0;
- B( O. Z" Y& {' D8 H - }
1 J6 K: L N! K! V! ^- m( `2 F; ^ - if(Err == 99). f, e* T! A! f+ r! Q; Q
- {
5 b/ ~4 J& W$ l# [- I1 H! r, J - if(Left == 1)
' n4 I- e% H! e; P- B0 M }0 @6 j - {
, }+ b- l w/ A - Speed -= 5; F( x( @! k' Z0 Q& D; p/ z
- moto(0,Speed);4 ]6 [) M+ c2 @, x& X* v7 V
- do
* W& D, [& Q7 @5 D, p& M - {
1 L1 V% B, Q3 u9 E) {9 Q5 r y: ?# v% h - Err = getErr();/ g6 ]6 i/ l' g: c
- }while((D1+D6) == 0);2 C. n" }6 F- t0 M
- }
, s5 Z1 @0 k7 e4 h2 ? - else: W$ T! q8 o0 Q" K, v# Q
- {& w3 J9 l/ m$ t: @/ V# V: p1 H
- Speed -= 5;. `, Y* _" R& q* O2 W
- moto(Speed,0); 4 H5 u0 N- x; _ W A3 m
- do5 ~* z! `+ U, ^* w
- {9 ?3 J! Y& z0 Y6 a/ d7 L" V3 h$ S- p
- Err = getErr();
) Y G, |$ O; I' w3 ] - }while((D1+D6) == 0);" F- \" x# Q; u7 X" e
- }
, ^% i, e2 O" X. p0 } T - }
( u2 s7 v1 ~" Y% O - else
! o3 ` m2 I5 L' S: p. F9 ~8 o - {
- j/ }* c x: p: C7 [ - if((Speed < 160) & (Err < 2)) Speed+=1; - U% C9 E \! Y% k6 i
- if((Speed > 100) & (Err > 2)) Speed-=2; ( M( Y4 l4 N$ k
- integral = integral + Err;/ H: H$ ^; N' Z N4 U% A8 s
- derivative = Err - previous_error;
! C/ x- Q2 u+ u4 q. R8 b8 ^7 h2 V - output = Kp*Err + Ki*integral + Kd*derivative;
5 Z5 Z& S: F( q$ D - moto(int(Speed-output),int(Speed+output));& }0 g4 T, U/ K4 ~* f: R
- previous_error = Err;( h7 u" ?, s; ?; x1 \
- }
4 L/ w. a: y H ~' G8 R% c$ o; q - }while(!((lightsensor_2.read()) < (10)));
5 h# x/ t! V5 a5 S: L" w - moto(0,0);
% v5 n `4 v! x/ G; [ - delay(500);
% E: d+ C8 j2 y+ O5 E/ r - buzzer.tone(262, 500);
# o3 H! @! ~0 X% |5 O - }
6 V( J! q8 B9 E, R9 u+ E5 L8 H; n - ! c2 {- R# W1 b4 p& f8 p
- int getErr()- T5 c4 U9 J1 p0 h( g" x5 j) k
- { ( y( ^7 r: B' D( m, P
- sData = linefollower.getValue();
4 i" c _0 s2 K - D1 = ~(sData>>0)&1;
% I9 [; n7 T# t4 Y6 k" z( H - D2 = ~(sData>>1)&1;+ W0 p( X r7 c1 y* k: z7 f
- D3 = ~(sData>>2)&1;+ @! _& A( M4 x [1 |- J* U- G4 }( m
- D4 = ~(sData>>3)&1;) w3 p6 |- D- V0 x9 k4 |0 R J* G6 E
- D5 = ~(sData>>4)&1;
$ A5 J: c% @% o' x) [1 _8 b+ W+ X - D6 = ~(sData>>5)&1;, j8 o$ X R3 d9 E
- int downD = D1+D2+D3+D4+D5+D6;
, b' n5 j$ o+ V& n! f2 v4 K - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);. c1 p9 s$ [; @! @" }7 |2 P+ e" A
- if(downD == 0)
" G3 H' G) o4 r: T& Z - {6 _" n( T5 r) f8 b
- return 99;$ _) Q) e% z' E" d! F) N
- }
, S, c. {, `6 E3 |$ K/ |+ h - else* o8 W. ]/ G1 W6 M
- {
1 B# N4 t% n+ H% }% @& c* [. N - return upD/downD;
/ S5 ]) k% H5 U3 X2 A+ A' \, q - }
% x. M. v+ v ^5 {1 e0 I - }
# H9 z \% `/ F
$ s7 H1 l9 x6 n; M$ Q8 n J- void moto(int Speed_L,int Speed_R)
' {; Q- P8 c8 S - {/ m8 ? m4 ~# }# z: u
- Encoder_2.setMotorPwm(Speed_L);
/ b# N3 C" h* p' p' ] - Encoder_1.setMotorPwm(-Speed_R);: x) x4 k; k# c$ {( _
- }
複製代碼 8 P- V: A Y, L C' D( k
MeLineFollowerArray.cpp2 S' p1 [5 L# s3 E
- #include "MeLineFollowerArray.h"8 d5 r) A+ i- t; p H2 e
2 O0 U9 V$ s. G' l6 }- #ifdef ME_PORT_DEFINED* D1 r% O2 J' Q- s$ @, o
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)* e5 D; f" U$ `. V$ @4 |/ s) e8 y- J
- {: q. S0 e; O1 s/ O4 |
6 h5 P5 `5 I, a- o, R, J# ~ y- }; G/ }4 N# U5 b# D7 c( E
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
+ m6 W! a# T) p* B# O5 { - {2 J4 E. S3 [' _5 d
- _DataPin = mePort[port].s2;
: P5 t& J6 }0 F0 j) U - pinMode(_DataPin, OUTPUT);
6 D/ m, E, j/ |, D8 K6 v - digitalWrite(_DataPin, HIGH);: ~( w) L0 O0 W% d. H5 P- j
- }
5 w' A6 ` i4 |! B! q+ x' c - #else // ME_PORT_DEFINED3 M+ ~2 i4 Y( P" ]$ A p3 N3 m
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin). c% f/ X+ \8 n8 {
- {7 ^. U! y9 x. _' q
- _DataPin = pin;
7 v4 Z2 F5 E- a7 U - pinMode(_DataPin, OUTPUT); 4 _+ V$ c: `5 W- {
- digitalWrite(_DataPin, HIGH);9 M, s3 a# k+ H: M( P; P
- }
9 \; R/ G8 p* C. H; p4 o e3 { - #endif // ME_PORT_DEFINED' o1 |4 s y3 z4 g7 Z8 i$ H8 V6 e* V0 c0 y
6 A, ?. k1 [# ?1 k( K \4 [9 l! M- & ]+ e+ d- r: B6 i& T" ~
- void MeLineFollowerArray::setpin(uint8_t pin)) F+ b; R; \! t7 B
- {
5 y t U3 b& {0 M( I; s - _DataPin = pin;
; z: j4 {. C7 c& B+ X/ n2 m1 {- ` - pinMode(_DataPin, OUTPUT); - `/ q5 b( ^# z0 ?
- digitalWrite(_DataPin, HIGH);
- X& W6 H: D' H9 K* d: e0 b; _" S - 9 x! j+ f' [3 B- e. c* [
- #ifdef ME_PORT_DEFINED
: e5 |9 q- Z+ c) O - s2 = pin;
' u) o0 J8 N: i* H - #endif% s0 l! P1 f y# V! }+ Z
- }1 n0 y( S2 w% `. G( e
- 8 C) l3 D9 g, o8 i$ a% \/ x
- uint8_t MeLineFollowerArray::getValue()& i. ~2 e5 D1 p! `
- {5 m& \% }1 I- v2 b
- uint32_t LOW_level_read_time;5 ~8 d; q( N* |8 d2 [" r
- uint32_t HIGH_level_read_time;
x7 P/ X: `' @$ [ - uint32_t time_out_flag;
) j2 w+ Q; S1 r) X - uint8_t Sensor_Data[3];
~- C( q* R; d$ }0 v4 ] - static uint8_t old_data = 0xff;
" b( T; k4 W5 S4 q6 I; Q - : h3 I: P1 p" {$ a2 R0 M& n
- pinMode(_DataPin, OUTPUT);4 L% A5 z C, S5 ?3 M* V& d4 Z
- digitalWrite(_DataPin, LOW);& y, Q7 N$ B2 L& H
- delayMicroseconds(980);' G5 u0 ?. J* w0 |5 r4 H
- digitalWrite(_DataPin, HIGH);
2 A5 h+ e$ k8 M& B
; s: ?' {2 Y: }/ {. E- pinMode(_DataPin, INPUT_PULLUP);9 n6 B. \6 P/ p6 i& b1 {( P
- delayMicroseconds(10);& i/ T/ }4 g! y% h2 B3 q
1 q/ F5 x) l3 N3 L d S7 Y- time_out_flag = millis();! h2 b& ?. d5 n) C3 z7 W' i: j b( b
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 k2 ~7 [0 Y) l- ]
$ |; w- \4 E6 ~, A6 b% u- LOW_level_read_time = micros();
0 I* |, P1 `* n7 u - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out4 q+ h' Z* F9 ~$ [8 d* [2 g, D
- {' X, c5 l& g( E0 G- K6 C% J2 ^6 e4 P
- return 0xff; y* J) k, M, i1 S6 y! p
- }7 x1 {7 f/ R/ K% V# S
- 2 V2 i) v5 x! C9 G$ I
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- V; s7 u' d7 W' W$ o
- . t& e3 a1 t) j1 a9 W
- HIGH_level_read_time = micros();
* ~& a) k, l6 N0 V' D - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
% w" N( M3 t! D8 e4 _ - 1 J5 _, W& e/ Y9 D- \2 R
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 A* `; [0 R, D5 V7 m! C9 j8 }$ p- L - {. z! T3 W, d- ]" |) M
- return 0xff;
: p; C7 c# Y# P - }7 O( y5 n3 S, e8 u. R- u
2 n* T9 z& @9 M* q& Q- o6 i- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)), A' u7 [5 k( X; o
- {6 j; I& y! \! o! `7 V' V3 j' Y
- return 0xff; Q/ f2 @( ^+ P7 d# Z9 t
- }5 X/ P: `5 S# i2 p, \: r" `
1 x8 Z+ p- c& z: u- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );& Y: ^4 g# ]6 l- f, P: l( s- W
- LOW_level_read_time = micros();
1 g/ [$ `5 b5 q+ ^2 [ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level5 p9 O( ]) [1 M- t7 o6 W
6 o0 j& T. c5 x; p# ]& V' D! u# Y- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out# F, X" A: {4 \# j' C' A
- {
# v9 O/ t9 G& L: \9 Z8 u+ S - return 0xff;! o/ F1 {4 F4 j! T4 m
- }) H1 h# X( p* _9 z6 p F9 ]. ]7 b/ A# V# I
" \- @' L3 B: q0 R- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
. }% {( x' y" H - {9 T$ S( {7 W- L6 g! `; N
- return 0xff;7 i/ ]9 S7 R8 M! S# }6 Z* z A8 Q
- }% W( _: {4 E8 ?( c$ X* D/ u
]9 o! \7 `- }; b! s5 w7 ]- for(uint8_t k=0; k<3; k++)7 {8 v, V! o2 W* f# U* {4 s, @
- {/ q4 |% u6 x2 x; [1 \9 P
- Sensor_Data[k] = 0x00; h' L# G( P8 n; a! k/ F8 m# V
1 X3 N) M) d( W+ E- for(uint8_t i=0;i<8;i++) M, V6 }" O) E7 S: i) K B; Y
- {
3 ^; k% i, s9 i6 n( y. } - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level" T8 l( Z$ H# g' `( ?0 d3 q4 g& s
- HIGH_level_read_time = micros();+ z3 P, T: |+ g- T. M
- LOW_level_read_time = micros() - LOW_level_read_time;
, T: Q% N% f8 d$ K - : H* s; K4 {' c0 b! {
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )% g( i; h- R7 h! i
- {3 `9 S" e: \& z3 Q" R9 I
- return 0xff;. E6 G4 h f$ _5 Y. w
- }5 ~" h/ d8 x& i+ g; T1 B% j0 @
8 q7 Q& o& O; t- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
0 `+ _% m# j0 r) A" o0 `2 V5 n - LOW_level_read_time = micros();
) Q; m2 s$ \8 C l - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
( H$ t, l" D; _- E8 o' \9 c - 1 `3 X# X9 B8 ~4 I
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 17 k8 u( I6 h! v0 s
- {
2 G5 { U5 {; E1 x/ ]0 L; K - Sensor_Data[k] |= (0x80 >> i);
' g0 J& m' A4 ]) c3 D. z! S - }* L+ {9 J/ X7 Q9 d a' M
- else if(HIGH_level_read_time >= 100)7 ?/ R6 B" |" Z4 A% r2 |# a9 f3 t9 m
- {* Y- X( v$ O. k% b: ?. D) f2 P7 }4 h
- return 0xff;# B" u5 ~9 V: G Z3 G
- }* A0 l& v& `' R3 `
- : x8 m( I" P' Y& c4 q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
; R; h n J1 P. }' M' u8 W8 x6 u - {; E! f* Q# J P+ Y
- return 0xff;
- f/ G9 Z( U, \! v# m - }
# g; t& H# K; o/ w - }
1 C7 p4 Z* c' l: Y X0 \' V0 B - }
( Z/ f2 J7 ?3 o0 h- H9 N
* i0 K q4 y& I, {7 @4 I J3 d t- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
0 R- w5 u0 X' R$ h/ n - HIGH_level_read_time = micros();2 K* S- R& @6 \, r2 a. z0 o7 N
- LOW_level_read_time = micros() - LOW_level_read_time;
, U4 r6 W; c% g: C! [0 L - ( ]/ O _8 _0 Q
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )0 W9 T% c! |, H, m6 |# u2 y
- {
" l9 e2 @( ~1 E& u" [ - return 0xff;
: R: v$ ~; G5 o) E& \: f - }
7 u# S( ~ p8 o0 T( X' ~) r - ) S# m; U4 F% W7 G) w, S# h
- pinMode(_DataPin, OUTPUT);
. e% ?) c) n' C# Q - digitalWrite(_DataPin, HIGH);
5 W8 h; p) A0 j5 C) _7 C
& m3 ?* l8 s9 m- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
$ _- w" q% p0 | - {
6 a: J. X: I/ V6 b - old_data = Sensor_Data[0];- z; {& ~4 A2 m8 W
- return Sensor_Data[0];
' e5 ?% A- }; P2 B - }4 o2 e Z" E6 {/ |( u
- else
2 z: A" r- U1 e! U - {8 G- i2 H* i1 k. j3 c4 r# p' m v0 p
- return old_data;
2 b2 u6 p; {* F6 i - }7 x) x i: G- ^) r* F1 L
- }
: e& L; J. Z$ a% ^2 M/ ~3 W
複製代碼
- j& H# o7 O6 S( @+ E( {3 sMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
% ]: j j B! T8 l
0 |- n3 b- ~7 D' ^ P& L5 o5 X |
|