|
|
4 B' u$ P ^ r$ Z3 G- #include <Arduino.h>: W- X2 S+ ?; S" J; A: }4 s" ]+ J$ K
- #include <MeAuriga.h>
! N, A$ S* q3 N+ s# B - #include "MeLineFollowerArray.h"5 ^6 C% r" B6 V% A
; }: K6 G; z- W: H: l; t- MeEncoderOnBoard Encoder_1(SLOT1);9 A9 ]) c& h' @* o# D: N5 ?( x
- MeEncoderOnBoard Encoder_2(SLOT2);
" a2 i4 s: O. n0 ]4 K2 N - MeLightSensor lightsensor_1(12);
6 K6 d9 y4 d2 |$ U5 O m$ I - MeLightSensor lightsensor_2(11);5 S- b6 S9 x" F" ?7 A& L" ?+ m" c* m, A
- MeBuzzer buzzer;
3 d; C* m5 S# x7 ]! L$ [) g. Y3 C( U - MeLineFollowerArray linefollower(PORT_6);0 N5 U, ]$ E9 J' q. z, c4 s! F
- * p! |6 _6 u+ x h
- #define Error1 1. F6 p2 D! V' q w% Y" m
- #define Error2 2) E2 U* ?& M4 G# \* v1 T: e
- #define Error3 3: @' U; U4 O8 ^+ Z% r8 w2 @5 {
, } s0 Z3 d3 W3 L( s- #define Kp 15
) {: n! u# Z5 r# e, @' k$ ` - #define Ki 0.15, P t5 J+ w2 [, K: b
- #define Kd 0.03" \, S/ g# V+ r# s' V# }
x8 z0 l- S [# U/ c* m# \- uint8_t sData;
3 ^$ O& V! M# J. [ - uint8_t D1;1 b* f u3 S, _7 S! i) ?
- uint8_t D2;
1 Y, @8 n% J8 | - uint8_t D3;
5 v! x- `# p: o$ s/ h* h - uint8_t D4;
% T- W: F, J: [7 s* f - uint8_t D5;
$ z* g8 v. {7 I: A" t0 X - uint8_t D6;
4 P: X4 b, A# d2 w1 l- G* l
* T, [7 c( ?) L. j- float previous_error = 0;5 w$ t% J2 j h5 v" x- D
- float integral = 0;
$ E. [$ @; z, X/ F) c - float derivative = 0;
5 j& _' }+ F$ r* p3 F' ^7 N - int Speed = 160;
, }% J' A( y, _- p& U+ n: j - float output;8 n* F7 c( v" v$ \8 m
# E2 E- N5 N' m& |( ~' ]& A3 S' S7 r- byte Left;
+ z0 f @; o, X+ h5 {
6 \+ o# ^! X, j; h; f1 d# L4 S$ r( @- void setup() # M& K( P4 ?) Z8 I3 Z
- {
/ G, o4 D* i/ \4 ?$ e - //Set PWM 8KHz$ T' C& ^/ j d4 h/ k& C
- TCCR1A = _BV(WGM10);8 U5 ]- D6 H: f( x! \: U4 l+ ^
- TCCR1B = _BV(CS11) | _BV(WGM12);
' G d3 Z& x* m" B+ b3 j - TCCR2A = _BV(WGM21) | _BV(WGM20);
' c s' K. U! v( D" E+ n# P0 A4 d - TCCR2B = _BV(CS21);8 H/ ?2 y% m5 H' y; I
- Serial.begin(9600);1 |0 D& ~6 Q" A# ~/ @) w
- buzzer.setpin(45);) s; z5 R ~0 f( ~6 e) L7 W2 t
- }
0 ]% W/ m4 ~2 V, G5 j: }& W - 0 P& J! w+ ]% \' r
- void loop()
9 p+ z: C& Y x, ?+ A" P+ Z% z - {- Y. Z$ a) N" `* f9 S
- while(!((lightsensor_1.read()) < (10)));( G, Y( [- i. L
- buzzer.tone(1047, 500);7 o. p7 K' F4 a# {5 }' l
- delay(500);( i) T) O, h! x8 h- b; r
- do
0 Z- G! Y! |7 C& Y" e% x+ d - {+ z6 M5 F0 n% W4 d: K
- int Err = getErr();8 I% c& @! k3 J# L6 C
- if(D1 == 1)
# o' {5 D8 |8 j1 P - {8 c# ?# `) X D2 E$ W8 E
- Left = 1;% u3 v. U, U; N4 r, D; [7 z% m. x$ c
- }
4 d3 `1 [! t* f+ o: R' J% c, G - if(D6 == 1)3 W. S& u- p) X% {
- {
4 j2 S! o' z0 q5 P+ u! Z3 l4 c - Left = 0;
; E9 S' |/ j" W9 c9 t - }1 o: l4 _& E+ y- F- x
- if(Err == 99)5 r0 j: ^+ G9 F+ j. o8 U
- {
9 v2 Q: G. F8 c2 C. G, `7 @* x# D - if(Left == 1)1 h0 p" _1 T7 {
- {. U& p3 H7 b z! h- a0 U# d
- Speed -= 5;
4 @& d# }# |# v8 F. k1 s7 } - moto(0,Speed);
& `) X1 j" p- s0 `4 e7 g - do- N. P2 \! p) f( H& K% M: G. x
- {; S' V$ h$ N5 W6 t+ T& k' P. t: n& A
- Err = getErr();
, u2 p* \2 N6 d' X. _ - }while((D1+D6) == 0);/ { f- k/ ?# `
- }( ]6 E3 R6 ~! I5 ?+ N/ Y/ J- y
- else" G Z" X0 \) d7 S$ S0 G- J3 F
- {
* w5 P8 v$ h3 A' \- J9 j; R5 a - Speed -= 5;
! d# S* I0 o7 l! K" s" [6 f$ y5 d( E - moto(Speed,0);
/ n( t$ c9 P9 S5 _ - do# n0 v9 x: R( I- ?
- {
, a7 o9 ?4 s1 P# _ - Err = getErr();
7 d! }3 Z n- W - }while((D1+D6) == 0);) x; k6 B* }+ {2 M0 O7 E @
- }
. S( M" Y t. Q7 |6 y - }- b- F6 q2 F) i, E0 x7 I
- else
% v& e$ W# x0 u8 m7 G6 t# [ - {" l5 P+ J2 g! ~+ p/ S( T4 K
- if((Speed < 160) & (Err < 2)) Speed+=1; : o- [5 |. V1 J' j
- if((Speed > 100) & (Err > 2)) Speed-=2; # s+ \2 q5 E0 h! N2 i( V: {* S
- integral = integral + Err;
) y( S, ~0 ^+ x2 y3 o - derivative = Err - previous_error;
* f5 ^% m+ K2 ~) K$ z+ z+ U! Z - output = Kp*Err + Ki*integral + Kd*derivative;2 @+ W1 U: [1 V0 S
- moto(int(Speed-output),int(Speed+output));# C. V2 p6 R3 X n$ W$ N6 y
- previous_error = Err;
8 `4 @5 K# z( @% L - }* y' x2 ^9 c q% `/ U$ q
- }while(!((lightsensor_2.read()) < (10)));
. g# @ y3 W. Y - moto(0,0);
! H# O9 u8 p) R - delay(500);1 k. x6 }7 S9 N: ^) e6 K- e5 r
- buzzer.tone(262, 500);5 `9 F# B! W/ o4 W3 h1 k1 h
- }% ?. B [' w+ S5 {) {
7 Q$ `$ i5 z* ^; p, E" j) V- int getErr()8 T$ `- I8 V+ u8 _
- { 7 X6 o# x+ t* @: k2 v
- sData = linefollower.getValue();1 j3 H* k8 P! C6 I: h$ n
- D1 = ~(sData>>0)&1;
- Y q V7 e, U - D2 = ~(sData>>1)&1;( l: ?2 \3 ~# ]$ g9 s
- D3 = ~(sData>>2)&1;; T) f- h! s- K# u
- D4 = ~(sData>>3)&1;9 b9 Y7 a4 ~- U! O6 I! Q+ a
- D5 = ~(sData>>4)&1;3 P: S3 s' j4 k6 \
- D6 = ~(sData>>5)&1;2 a( d( {! X$ R- o
- int downD = D1+D2+D3+D4+D5+D6;
8 H( J* @' b7 ~) k; T% G M - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);; q( ^! r' x( M- U9 u
- if(downD == 0)
1 b9 F Q) ~2 W$ u; n - {9 \" o: }" N; @4 b3 u2 n
- return 99;
0 B0 l( {% [7 ^- X5 }7 i- I - }- m- V) \: Z! l& ]4 P* Q ]# H
- else
$ L1 s; Y7 V, c1 i+ ~ - {
3 h7 w3 X; p1 y6 r1 p; A - return upD/downD;
, t: H6 u! s" |/ G8 A3 u - }
1 ?! d/ V1 T7 ^" I( i" K - }/ y6 D+ U' F5 c% K
- 4 t! Q* T2 b. ~" D* `' X- [" @: @6 Y
- void moto(int Speed_L,int Speed_R)
3 h: b6 W) I3 [% z! [7 c - {
, b* E6 t6 n. M* q6 s% k - Encoder_2.setMotorPwm(Speed_L);3 D8 t0 B" b9 {
- Encoder_1.setMotorPwm(-Speed_R);5 g- C1 \) R& z) z: Q
- }
複製代碼 9 ]' Q. l/ [1 M
MeLineFollowerArray.cpp1 q- D9 z' E( L j/ J2 a$ j
- #include "MeLineFollowerArray.h") U3 ^& F7 y/ T( o0 P
- ) L2 M! {- y/ c* M' k5 A
- #ifdef ME_PORT_DEFINED% R6 K. ` B, u5 w; L
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
9 Q& D7 T, G/ N9 D/ |& j$ t2 n - {/ {; E$ K! _5 u& I3 k: h
- , y9 f; m+ A6 S/ J
- }+ I6 ~7 m3 R4 Z* E( w: ~- e
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
- V: v& T; X4 F- g' `# g6 M - {
( q5 K" `9 I- k6 f6 x; M$ | - _DataPin = mePort[port].s2;
; V2 J3 B8 o J3 S - pinMode(_DataPin, OUTPUT);
. W- p+ D8 @! c5 P6 ~ - digitalWrite(_DataPin, HIGH);
% o- P/ G% ~& v7 h+ A9 t0 \' r9 w - }
* H" a$ F' E) a/ a C5 l- ] - #else // ME_PORT_DEFINED
4 v4 K8 x& S; p0 q1 B d( C0 w - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)3 {) `' Z- s; r; ^4 N
- {
D- Y0 S* p B3 | - _DataPin = pin;: d3 j+ s& p, N: d. g
- pinMode(_DataPin, OUTPUT);
: G5 M, g2 q, w - digitalWrite(_DataPin, HIGH);
% a9 J9 N+ ^8 ]0 F: `; g - }. p* Z2 y) L0 V
- #endif // ME_PORT_DEFINED
2 Q- o0 R: z/ G: S+ f1 V/ l
. ?6 c M. Y# s9 r# k |- ' f" D3 q' D. d# H) q
- void MeLineFollowerArray::setpin(uint8_t pin)
% P6 ~. J, l8 w+ l* z- [% ^* T6 w - {
& K) c! \& K$ e+ a% S - _DataPin = pin;
" g. \+ Z. e0 x- `$ ?) R2 V - pinMode(_DataPin, OUTPUT); ; \8 R f, o+ z
- digitalWrite(_DataPin, HIGH);
3 O/ b0 i! l V! v5 R - 3 z& K) a3 D9 \* R; Z. |/ Y/ z
- #ifdef ME_PORT_DEFINED- P, q. E" B3 f8 g% R
- s2 = pin;/ p& j* o' O2 X9 z; r
- #endif$ Y+ g H- ~, m& e0 ]& V( [; E
- }
/ `1 T' b5 A; W& n- B
9 L$ g9 f0 j- @- uint8_t MeLineFollowerArray::getValue()8 N2 u6 k P& L: ~$ p
- {( Y$ y! q% C+ Z0 E! O G+ F
- uint32_t LOW_level_read_time;
8 f6 o/ l# L& @ O0 |( H3 O - uint32_t HIGH_level_read_time;5 f2 q1 _4 U' g3 ]- t6 p H) N/ a
- uint32_t time_out_flag;! Y% h4 I6 [8 A' \1 Z; U" ?, m4 n
- uint8_t Sensor_Data[3];
* p& y" Y5 E% |2 W. p - static uint8_t old_data = 0xff;" }2 m P; Y0 `! x5 K
- % K; M; {7 v" q
- pinMode(_DataPin, OUTPUT);: ?7 e. ?9 G3 C8 f+ ^) R( |
- digitalWrite(_DataPin, LOW);" D- f$ V) C4 B1 p6 _
- delayMicroseconds(980);
) ]/ ? x" z8 W' E4 g - digitalWrite(_DataPin, HIGH);
5 ]" Y0 H$ @% d6 ^* T; r5 O& _ - $ m9 l! n3 ^3 L' p' g+ A: @
- pinMode(_DataPin, INPUT_PULLUP);
% z! `' k' i# [! D% L. e - delayMicroseconds(10);
+ ]- W* @: }- {7 o4 n% V9 R% C: G - 5 p6 ^/ R8 b" L0 p3 j
- time_out_flag = millis();3 l1 u& ?% t9 ~9 c
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 j$ r+ V; q6 y; u1 w2 ~, C - 8 D+ X) x/ P9 a3 X% F
- LOW_level_read_time = micros();" l1 c6 H& X) @6 A8 d
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out) Y' S# M9 ?# X: p3 {2 E$ E
- {
" p3 ^, o9 C' w' C/ U8 _& Y3 b - return 0xff;& F2 L6 q; W, f0 b4 q7 j
- }
* {" c6 J) J5 U/ p m - + E* Y% K: ^$ s. v/ J
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. S1 i( Z7 Z1 l7 Z2 P; u
- # h) q* Q& n0 x
- HIGH_level_read_time = micros();
" k, j% B4 K+ ~8 q - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level3 Q% I7 {+ [$ G; ]
; i3 i* y# P0 X- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
8 B W: \. d, n/ m5 x - {
" h/ S C% z) @9 }: {2 D; | - return 0xff;$ i( d: m3 e* K" l+ o# N5 l1 Y( f9 `
- }
3 M2 x& ?3 u$ \6 X) A6 } - 7 I! H+ C9 Y, O! s
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
# B( |# F; ]* e- @ f* [0 f& i - {% T% a, ]* K0 k' Y# I
- return 0xff;# z9 ]& i o, U
- }! D3 T/ s' s& I4 `% h
* l" u* g$ H6 z( c) z- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
& y' N! G/ Y5 G - LOW_level_read_time = micros();
! X; o% I, x. g - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level. B$ T, k( s4 y9 B" K7 }
- 6 D6 d5 I7 q0 S1 d8 b6 z. ^
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out0 J- V6 t7 j# k1 E2 b# W* L8 s
- {/ K! D9 t8 n1 ~7 D" H. m0 X
- return 0xff;
2 h; E t5 ~0 e8 R* z7 S1 M9 d3 R - }
% w" C! G% b( z- i: f8 Z
# @0 j, M+ z9 Z9 J- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)); D7 y9 a# e/ E' p/ v+ s
- {
; N2 m4 u- t, `2 [* @( ^ - return 0xff;6 ^1 r# G) e& u8 \/ q$ v! g* z
- } d8 @& b& m% a0 I0 z
$ v+ O9 h3 p/ k3 m! D- for(uint8_t k=0; k<3; k++)% J/ w9 t" ]: U! {: @2 T* k
- {
+ w: Q3 o6 q" X. B - Sensor_Data[k] = 0x00;( T8 J' p" O/ e4 E
- ( i" P+ H* l$ T4 t X# k
- for(uint8_t i=0;i<8;i++)# j% b( D+ _. p
- {
8 f' Z0 M. V5 R8 _! { - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
" D6 P' v8 v" X- { z* D - HIGH_level_read_time = micros();) x( S3 Z5 Y& `$ C4 l% t: k
- LOW_level_read_time = micros() - LOW_level_read_time;
+ z0 U; j7 }& F9 J - + h: l) }& N0 I% C/ u
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
- C4 p. X& i6 f& I. I- ]3 Z - {* m: ^8 c0 J5 U1 z
- return 0xff;9 `$ J) \- |6 I0 |
- }& j# `4 S: L" o( s* p+ O
, H8 m/ y' a; `- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
/ |5 e5 F/ Z6 h - LOW_level_read_time = micros();8 C: H5 K8 v( |8 n( @9 B5 z
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level+ P8 o0 [2 H& v/ ~9 u
- - q- x' Q- R( {' z/ } U- w
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 16 e4 L r3 P* g
- {5 l8 u* M" I" Q8 U+ r! `2 T! r
- Sensor_Data[k] |= (0x80 >> i);
- } z6 H1 f% T! `- [ - }0 W- M3 }( {4 a8 U4 M% z
- else if(HIGH_level_read_time >= 100)
* B- g. V: T$ h; q2 O( G - {
8 F u0 O$ u6 @ - return 0xff;! Q7 _+ m) ]2 A% r4 x$ F. Z7 O2 L+ @
- }7 v" W( M8 N' F4 S8 K2 S% K
! ^* V; H( ]: O/ J- L2 Q, W- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)" p5 ?1 ]$ }# d- ^# {; E7 F" a
- {" O: A# r/ M' E9 o+ R
- return 0xff; t# h7 k' {* s2 [
- }4 k( a$ o, S2 f
- }
+ U: B) n6 R. f6 ?3 u Q - }
# _: @3 m D- G ~% L D( z
5 l- u7 Y9 J4 l b- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
4 m" h$ U7 h: m% t5 q7 t) _ - HIGH_level_read_time = micros();; r: @1 y7 q9 K+ A# A' e
- LOW_level_read_time = micros() - LOW_level_read_time;
4 e6 X3 [) k& j1 }9 j l1 @ - & H6 i; n' V% G4 G7 f. @
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) g2 X& t+ ?/ F$ N1 n3 ^3 ~ N5 q
- {! Y( u9 L9 ^2 y9 g4 ]
- return 0xff;. C$ Y% Z- q( a& M* m% [' z/ I
- }
7 a8 I5 W& z# a
6 D7 D' ^1 A0 ~! K) R3 j- pinMode(_DataPin, OUTPUT);
0 {4 i2 `6 V5 J: O - digitalWrite(_DataPin, HIGH);
) e( x& S: K- m1 n
$ L6 v( k" b( [, r1 b" Q U- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0]))), `) E* e: N5 c# y# P- y
- {3 `) l8 w" }$ C: l9 g: R( h) j4 z
- old_data = Sensor_Data[0];8 F3 D# Y3 g& W# s3 B! h
- return Sensor_Data[0];8 m \7 c1 L- A& D6 F3 e9 H$ [7 `' h
- }
( S9 ~$ z' o, { - else
s9 e: ~. ~* @ - {
5 N/ }' b5 k% ]: d - return old_data;
% l- u8 F/ v, N - }+ |- I p) f x% o% }& z3 G
- }) i. N K4 V$ y9 d. @* t
複製代碼
( Q/ w6 V j! ?4 F2 X$ |$ e" OMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
1 u& ~) P5 m# a- b( w; V: i8 Z
" C# ]! |2 \: s
|
|