|
% Z# h4 i: e Z9 y6 B% ]
- #include <Arduino.h>
. {9 F ]% D1 ?+ F7 F) [ - #include <MeAuriga.h>* U& r1 n6 h% ?9 V/ L4 t6 s" E9 J$ h9 v
- #include "MeLineFollowerArray.h"
( |0 ~& d9 T# c8 y" \1 `6 a
2 d$ K, L3 @, j5 v5 L- MeEncoderOnBoard Encoder_1(SLOT1);9 K4 G/ ]# U# C
- MeEncoderOnBoard Encoder_2(SLOT2);
: P ^& J. u* E% [* E7 ? u - MeLightSensor lightsensor_1(12);# D( i( C" D+ p# b' D
- MeLightSensor lightsensor_2(11);$ N" J5 c! i* Q, _ w
- MeBuzzer buzzer;
8 t( @7 E! V! C3 p! l4 n. t - MeLineFollowerArray linefollower(PORT_6);
6 q+ |- n" c) _1 R
$ y7 }: J o, Q" r2 Q& \" c$ F- #define Error1 1, J4 Q: w) a' m, O. w$ g
- #define Error2 2
" t! T. Q9 M+ L* V8 {$ j/ s" c - #define Error3 3
6 L! `- Q/ R4 o9 D - ) o5 p( K- k6 [& L
- #define Kp 158 n* _6 d. J6 G: H" E
- #define Ki 0.151 b5 ]2 q! U# ^- _0 D y3 d0 a
- #define Kd 0.03, F( K: Q- ]' d
- D, z0 A% j5 z: s1 B: w/ x0 ^- d- uint8_t sData;
7 L% y9 [& N7 n+ r! ~/ U$ z9 ]1 d - uint8_t D1;
& |* c& p+ u, [ - uint8_t D2;# t' y; I( m$ W
- uint8_t D3;
: A; y2 w, l7 P+ o! c+ t7 M2 } - uint8_t D4;7 z9 }# f$ F# W. s1 U- o1 e
- uint8_t D5;
8 d+ m. S# z1 p8 B/ ~/ _6 Z. ~ - uint8_t D6;, X; d& Y4 X: x# _' P
9 D& u0 t8 d" m; B, g Y3 w8 J" d2 G- float previous_error = 0;% ^2 ^+ Z8 O; f2 Y. I8 O( l& ?: j' V
- float integral = 0;- b; z2 e7 c# Y! `) w& I. p1 i
- float derivative = 0;
9 Q' {: O! r V6 F - int Speed = 160;
& J7 @0 y2 v' x2 ?1 G7 [ - float output;
`+ A% ~% S+ U+ f+ z0 [
2 T- a# ?3 t8 }! L$ o) W9 G- byte Left;+ v! i/ p2 O/ T' @( n6 E" P
- 6 m8 Q, [, A0 ~$ ]8 z
- void setup() / U, g+ E* D4 m) _
- {' [+ F' L0 G9 k' D' V8 ?( A
- //Set PWM 8KHz, A) c, G) N0 M5 e
- TCCR1A = _BV(WGM10);( G0 G: y% t' |3 [' K* d i: z$ g
- TCCR1B = _BV(CS11) | _BV(WGM12);& ^3 e4 l0 j6 n4 d4 l$ ]7 y2 F
- TCCR2A = _BV(WGM21) | _BV(WGM20); |$ r4 n! X; h/ v) J/ I% o( \' ~+ l
- TCCR2B = _BV(CS21);7 S4 j, k! [! n! S2 _" {
- Serial.begin(9600);
- k4 v4 @5 P; u2 N7 `+ t5 u5 P) g - buzzer.setpin(45);8 h& `8 d/ U2 l" ~+ {( x0 B$ R1 A( k
- }% [+ V) Y! Q+ Z) M
- : \+ D. T% \6 n/ `* j) b2 d, C; Q
- void loop()
: P% z1 e0 A6 o" _( r# `% x6 c. B - {
; _8 N5 Z. k0 z9 v' X - while(!((lightsensor_1.read()) < (10)));% F& F. j; [0 X
- buzzer.tone(1047, 500);
0 G3 n2 g+ {. R - delay(500);
# A/ ^1 ~' c- Y7 ^0 x) C - do7 s# R4 z* ?$ o3 z& ~ h" u5 I: J
- {* R; R( Q$ n& ^' h2 z& k
- int Err = getErr();5 f Q6 y& G1 E3 Q. j- [+ |( q
- if(D1 == 1)
# q/ X9 O. H% b6 t# t$ l5 h+ S - {) Q& K0 W* S: H0 S7 W- |
- Left = 1;0 B' W- K+ N8 q" ^0 Z
- }
$ p( q" }, q, _+ j# J - if(D6 == 1)+ g- i1 ?" ?0 X0 j( F/ h/ j
- {
2 Q+ F- a' U& n8 |+ i - Left = 0;
6 E s- c- ~! ?" m0 r, u - }4 Q4 z! W2 _6 I r, W$ Z
- if(Err == 99)6 v/ N& U2 m" ]
- {
5 V8 C+ R# s3 C - if(Left == 1)
4 S8 x2 n& T( G* M. U+ P# Q - {0 N" j: }$ D, A g
- Speed -= 5;
) N9 O# y& p8 r, L1 O - moto(0,Speed);
- ?$ q9 K) X( T: S/ v - do
/ T% i9 [ ~0 b" k) [0 C7 a5 ~ - {
/ B6 [* w3 v; W2 Z+ _1 K& U- H - Err = getErr();
3 Y% A. ~) s* i& {: ]: h - }while((D1+D6) == 0);
9 M+ Y* L, _4 H7 L! Q. \ - }
6 t/ H! b( B& c* Q: f - else3 e+ j/ f- o* e% h! \6 u' H; E
- {
8 h, n0 k" v- B - Speed -= 5;
b j" X9 a6 i: C& u+ d' ^ - moto(Speed,0); 1 f8 q6 ?8 }5 j7 E+ ~2 H8 P
- do# z5 r# K% N( g* F( d( N6 R
- {
% v! r' }! g& _) {% P# t% C' v - Err = getErr();
' P$ ?6 A6 ?, g5 n' Q - }while((D1+D6) == 0);
. Q+ S$ R0 q3 Y, h9 i& O$ C - }
Q5 J/ S* p. b# y - }2 W: J3 N, G: H8 Y6 H5 H& ?8 c
- else
/ B/ g/ G. m; s, |- A" l% R - {
( v% t- p' `% q; Y0 R* w/ G: J - if((Speed < 160) & (Err < 2)) Speed+=1;
+ [' Y% N% Z( I( W$ a - if((Speed > 100) & (Err > 2)) Speed-=2;
! x k1 Q" k5 F* x6 w: A1 a - integral = integral + Err;
4 U' B4 i5 R; l' u' W3 F/ Y' g - derivative = Err - previous_error;
! b) s! @/ r1 S! r& N - output = Kp*Err + Ki*integral + Kd*derivative;
6 h* d1 _$ X. U9 z - moto(int(Speed-output),int(Speed+output));
4 Z5 a3 ^4 {% n9 s8 X - previous_error = Err;' _8 ~6 X3 w. ?# X6 h. n1 O
- }
7 V8 L& p' Z# ^1 F3 V - }while(!((lightsensor_2.read()) < (10)));
' S6 {1 K0 O8 V. w' }1 f - moto(0,0);) P. {6 v. G; ` w- p& I
- delay(500);
; ]7 b( }% I! r6 V- e4 Y! L4 ^3 m, M( Q - buzzer.tone(262, 500);
- I g# v% y6 y! e$ r, @% Z) Z - }
0 q1 Q0 x- v% _7 @, k. r
$ N7 f- S0 B2 `- x- int getErr()
/ g" l: i- V' x/ W8 I: j2 Y! x - {
* B8 V/ J1 D/ Z+ R - sData = linefollower.getValue();2 }+ u- @" j7 k) Q! ^9 P- h
- D1 = ~(sData>>0)&1;
* Z- O- X/ g$ m9 r4 y; J0 M# \ - D2 = ~(sData>>1)&1;
0 p4 ]/ S* [4 ]$ F( G8 N' Y9 m$ _ - D3 = ~(sData>>2)&1;- W, ]# K! B6 P3 b5 I4 B( a
- D4 = ~(sData>>3)&1;
6 L. y) A! o9 p4 \& u1 d - D5 = ~(sData>>4)&1;) P6 e! J$ w5 g
- D6 = ~(sData>>5)&1;
* h( U0 H& o) R6 }; b6 c# { - int downD = D1+D2+D3+D4+D5+D6;
4 |( `9 ~1 E% [5 V. [# X - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
. j( y$ X7 T0 m ]1 f/ p - if(downD == 0): _' n \- |5 i) @, d2 D
- {+ h+ A) i9 a# G2 m2 K m; \/ W
- return 99;: i5 ~% b( p; {( q
- }3 f, h- ] l8 Z9 K8 e6 r2 N
- else
* ]2 K; r& X9 V - {. P6 N7 s y! {/ M C+ s. I% o
- return upD/downD;
) A+ B- j. F$ L3 K - }) @6 C3 H* B3 O& ^
- }$ S8 B, I3 i4 }+ j) [( ^
- 5 G- t4 V2 K) i. p
- void moto(int Speed_L,int Speed_R): _2 U! A* {1 X5 h# G
- {0 z1 Q. C, l& t ?
- Encoder_2.setMotorPwm(Speed_L);
5 D! D3 c+ b+ s7 c1 J - Encoder_1.setMotorPwm(-Speed_R);
4 X# j6 M0 [& h3 S* T6 z - }
複製代碼
$ y) \5 o1 g# k% M+ FMeLineFollowerArray.cpp
5 ~( @* V# [% L# D1 a- #include "MeLineFollowerArray.h"" r/ Y4 K4 U- M$ Y0 a/ \
! D( f' U' M3 a6 D( ?- #ifdef ME_PORT_DEFINED
7 L# m1 }# l* b - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
- K5 l$ j. l7 K, k3 i - {
6 c$ Z* V4 L7 @* L5 R9 [) R8 \/ N - ) \- O9 g7 {8 C1 |( Z+ n: @. ~8 E3 O
- }& J/ t# `/ P* M9 {( W' e
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)3 f7 N7 e5 e& n( \
- {( W1 A8 E# y9 @& C1 Q
- _DataPin = mePort[port].s2;
0 G8 s4 y5 D. m9 o; H - pinMode(_DataPin, OUTPUT); - {& f6 Z) `7 ]# z
- digitalWrite(_DataPin, HIGH);
+ q1 \$ r) q6 x& L9 z3 w' R9 o% @0 g3 ~ - }9 @ G W3 r0 F/ O: |7 M8 l
- #else // ME_PORT_DEFINED5 {8 N. B! N, g6 G
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin); _0 F% c n( P- ~$ u' H7 G T, y
- {
- h$ R; @5 p r6 z - _DataPin = pin;
( D& u& K4 L7 ]& ] - pinMode(_DataPin, OUTPUT);
- G4 x9 h. j# J( [* K8 Y - digitalWrite(_DataPin, HIGH);
# j' X5 @% v4 }3 w - }8 X1 \6 ]9 M6 C9 `' y- H- T5 M
- #endif // ME_PORT_DEFINED1 x/ m# f, K! T) ^5 ?! {' x9 }: i
! M; x s( H0 r, ?# @& i) L- 1 ~4 L) K' O- [+ V
- void MeLineFollowerArray::setpin(uint8_t pin)8 B! U+ v( F6 D" D
- {
, u& x8 _# x$ Y* z% ` - _DataPin = pin;% M8 j& V+ ^5 o( h- e+ D
- pinMode(_DataPin, OUTPUT); 6 L. X# M6 r, ~/ g( h# B$ z# G
- digitalWrite(_DataPin, HIGH);
1 I$ n: p* ^: J. y3 G - . R4 _8 k3 |3 E) L
- #ifdef ME_PORT_DEFINED
; L1 b8 W8 _* @1 y - s2 = pin;
. |; t; i9 P& X% }. Y; O - #endif
; u; L0 J% Z2 } _3 ? - }; y' y8 a# {1 e( s4 r* X0 R2 O# v
! O5 W- n. v! Q; h5 t- uint8_t MeLineFollowerArray::getValue()
5 h R" K3 A( B( F u- Y! D5 ~ - {
* T/ q8 G8 m! k - uint32_t LOW_level_read_time;
! |8 K1 ]( ^: m' W8 \% e - uint32_t HIGH_level_read_time;3 N$ T; R0 r& q" X
- uint32_t time_out_flag;1 t, i; R: m$ g5 r w
- uint8_t Sensor_Data[3];; }' O. z: D& l0 t
- static uint8_t old_data = 0xff; y: v" L( K0 K! v$ B+ w8 x' Z
" \! [" V# ~0 h& F- P/ F5 O, E- pinMode(_DataPin, OUTPUT);
8 ^1 Q/ N$ L! [4 [5 v9 |2 ^ - digitalWrite(_DataPin, LOW);. W' f) ^' R6 E9 I( u9 D! U
- delayMicroseconds(980);
! Y3 m0 `0 u9 u$ p( H3 H - digitalWrite(_DataPin, HIGH);
" D3 o! P5 m) [7 x" c
3 H. L- ~) p% V& \- pinMode(_DataPin, INPUT_PULLUP);
" r! B; J" p& N - delayMicroseconds(10);
2 }% S) H2 {9 G1 o& k9 }% [
' \- C0 K, l7 n) n4 E* J& J- time_out_flag = millis();
- V3 o7 A3 m( y# |0 C - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
1 r* y, R5 F$ }* q
+ ^6 i% D- R1 |* r. r9 u- LOW_level_read_time = micros();
. ^8 |# c+ d7 i8 [3 [ - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out; V9 `. ]6 D3 I, h7 P/ S6 }
- {5 f5 @6 y* K. k& |# c. j p
- return 0xff;: N" c8 k0 T# v' E
- }
3 i9 x9 ?7 ]% Y8 x5 J ?" {' u
' O" `' S9 j% ^- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );# a; x9 B! K* k9 x
- 1 N5 Z/ x+ w$ ^3 a3 b
- HIGH_level_read_time = micros(); |* t6 D9 M1 C3 V( Y) O1 O
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
& }( G8 }# w: T% I0 u - : @5 G7 s" D; v* G3 l5 z4 K' U+ D4 Q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out) c5 O- I; B- Z3 c8 p3 f2 ?
- {: ^6 {2 ?6 d# q. B
- return 0xff;
+ [1 i; @( O$ D& Z% [" @ - }0 f: J& X! g# b ?6 k. |
- " U( e: k' d' W' |; d; j4 q% A- L
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))+ Z- o7 @. t4 k
- {& i: W! b& O) ^" n- n& k0 k, P
- return 0xff;
2 L+ p" G# i: [) o+ c' Z8 j8 F - }: o. T7 K; Q& ~0 l1 y, y
- + H/ m. G9 Y4 L
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. K* I# Y4 R* q# S$ O0 p/ _) u# _
- LOW_level_read_time = micros();( M6 |, Z- l! \4 x2 N3 ^
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
3 z& Z; X; O& w
% {/ U5 f2 m! j8 B+ v- A5 Z- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out$ w+ S3 o* Y/ N- g$ ^
- {
! f+ r I( A K \ - return 0xff;
Q) H0 `3 d4 `& m% \ - }( m4 ?- A% y0 S6 t9 b7 H
- ( _4 t! h1 ^4 D; z7 q
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))9 Z# T2 P1 q) S4 q/ S, ?, P
- {
( m9 N' V7 J# P - return 0xff;0 I) B$ A! }( s, A v
- }
9 A& h9 }3 w3 s* u- s2 ]1 o# U& N
$ S* p6 N0 Z' j6 b- for(uint8_t k=0; k<3; k++)" O8 D4 v5 u" |! c" c; E
- {
) Y$ c% A# Q! j - Sensor_Data[k] = 0x00;& d, b& L2 a6 A+ ^# r4 Q- e8 B
- ; Q3 _ c7 f% |6 C
- for(uint8_t i=0;i<8;i++)% @# H& ?! F4 W3 b5 o, T
- {; t" \7 z+ Q$ d1 v R
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level9 p% s6 H* j/ U9 e( v$ w5 P7 \
- HIGH_level_read_time = micros();
9 P7 x3 x. L7 m( d; C0 c+ } - LOW_level_read_time = micros() - LOW_level_read_time;
/ @7 @$ G" k- B; p x - ; P) P! X) ^# s
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )+ m( i1 H8 R: B) G7 u6 g9 e, i
- {
$ I9 n: {4 Y1 W$ B& v" n+ Q3 z - return 0xff;
' N. Y" t- v* k: S6 a - }4 a- W1 k) s( B A
- - M7 t9 Q2 @/ k& Y
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );4 t/ B1 Y, u: |& n0 b1 z
- LOW_level_read_time = micros();
8 G* A# P1 M/ r/ ~! t3 C - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
/ f/ h- i5 x& Q$ V& [# ^, k
, l, h/ W5 x7 e' b% U! c) a) d& m- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
3 T2 j( a" x3 Q$ `# X p - {
, ^" z! s/ h- ]2 @# |& Z7 l - Sensor_Data[k] |= (0x80 >> i);
2 |; s* G) p' u- @4 e* P - }- R5 k! B5 y s( O& K8 K* b$ }
- else if(HIGH_level_read_time >= 100)
& [- N6 i1 e+ o, H2 w( X: N* X - {
, v {- e0 F$ q' u5 d, e4 W: j - return 0xff;
! j0 d* v2 F# C: H' S0 p9 G6 K4 p2 M - }
% D, A) e- |& L2 i2 _' G! V. b: g - # ^# U7 G' v3 R! r' m* J1 @
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)+ V0 Y. w6 t0 P* q$ H) d+ x, n
- {
9 ]9 O: E! ` \' Q - return 0xff;: f6 y7 z2 D3 A+ O7 z% S
- }/ c( }" c! k* [9 ]- h6 W
- } S2 e; G# [3 F+ K/ Z
- }8 U" `7 b1 Z1 V3 [6 E! _
- 3 m9 x o3 e! f- ?9 c# x+ J
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level3 n& M) K6 ?: `
- HIGH_level_read_time = micros();
- o# }% `/ X2 i8 I" m6 s! _' P7 C - LOW_level_read_time = micros() - LOW_level_read_time;3 [1 ]( A1 {# c" X- M- e. N
- " Y1 n! }& M+ \3 D! I" e! V
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
* U: e# y& Y4 L$ i2 w& s - {
# g6 z( V9 I. h" j2 V1 n - return 0xff;1 L, q* o3 B8 T. y; e
- }% U/ m1 F1 F6 ?: z0 C
/ D4 }' v( a( P$ b- pinMode(_DataPin, OUTPUT);
6 s! N5 |) r9 T, y - digitalWrite(_DataPin, HIGH);
) W8 \1 J0 |" @1 H8 S8 T# w - 8 M4 H) L$ g! q+ ~
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
( o' s! `7 P, U4 } - {3 m; q6 \ y3 S
- old_data = Sensor_Data[0];
7 e5 y2 @5 W7 Z# M" t& K0 s - return Sensor_Data[0];
/ K9 P* O' C& E2 X - }0 j! C0 _1 z/ U+ T: B: a
- else
2 ~ P, ]3 N* `& V0 |5 Z - {* p" O' I, o7 [) o( F2 [7 r# b
- return old_data;7 N( D0 x0 {+ p* i- I9 H
- }
/ X) b# ]& t4 O( h+ B) | - }& x4 g, Y; K% Z. m# ]) l/ Y4 _6 q1 X% g
複製代碼 5 r) z' [2 b8 Q, D; b) J
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
+ G6 H1 j/ ?6 y C% P& g6 [' I& |- C- x9 X+ I. O9 _; l
|
|