|
|
4 ]2 o3 h q# x4 b4 e1 Y* g# \( Y% F
- #include <Arduino.h>
/ I" l% G ^% K) ]* x$ a - #include <MeAuriga.h>7 h: b; q# a& R9 H) U; N' l# H& R
- #include "MeLineFollowerArray.h"9 Q# f6 h9 O0 w
% m1 l2 D4 Z% n' _- MeEncoderOnBoard Encoder_1(SLOT1); z+ n/ E' y# \ f! F
- MeEncoderOnBoard Encoder_2(SLOT2);
+ K5 y, I, V; G8 l: G* Y/ y5 d - MeLightSensor lightsensor_1(12);7 q2 @, H: ]6 Q! r) h& ^, ^
- MeLightSensor lightsensor_2(11);
7 _- V+ Q% \2 T - MeBuzzer buzzer;
( a L! L- _, W/ P) N- g - MeLineFollowerArray linefollower(PORT_6);
8 U, `) J2 r R6 n+ x0 L2 L$ I3 L
+ f. @: |6 v( Y k% D5 v- K$ C( e( r- #define Error1 1* r) l1 j* l, s0 C8 Q5 w+ j9 q
- #define Error2 22 n O, Q; C3 s4 \6 _ u
- #define Error3 3
. H2 {' @1 w& ^# I0 D7 T! n+ M' @
, n5 z$ ?7 P, _' u: i" _- #define Kp 15
' x7 F( R2 P! K- }0 o" k, ] - #define Ki 0.15
! C6 r% j2 C; i/ N- o - #define Kd 0.03
6 S. P! Z0 `& q( B9 h8 L/ f
* @9 ]. y2 [3 y, Q8 C( m+ h- uint8_t sData;- O- l; g! U$ T, l* u; @8 z+ J5 I9 E, d" r
- uint8_t D1;
5 U" }" ^1 o$ T$ D8 i! U - uint8_t D2;
& v7 R" _+ p. F - uint8_t D3;* P7 D1 f! }: ?& w! J
- uint8_t D4;7 P3 y! D9 F7 `& K3 s$ i5 H# e9 `
- uint8_t D5;/ v/ v# w) h% n$ r/ r" b. ?6 G4 a
- uint8_t D6;
0 N) G! U1 m6 a - / o! N* [- V+ S& V1 y/ q- C$ s
- float previous_error = 0;+ R/ `# \( {1 ^0 U5 A
- float integral = 0;9 X$ g# H8 W% X
- float derivative = 0;
. \- B. p) L+ t( v - int Speed = 160;+ F T- ~+ F9 d) A9 O3 O% G' p. ~
- float output;
. M- S3 n4 Y) k5 B - 7 H2 G1 c/ N3 h) G5 F
- byte Left;# b4 D, Z+ N" }* i; }# |7 P
+ h! v9 h6 o% K D- void setup()
$ X4 w- R; S# n$ Q: b - {+ n2 N; @# r% m. l1 m' C
- //Set PWM 8KHz4 I4 D6 p; A' b, r
- TCCR1A = _BV(WGM10);* \9 J0 b/ f/ l7 H. G E; f5 y
- TCCR1B = _BV(CS11) | _BV(WGM12);9 W& K% m, R+ X& S6 ?
- TCCR2A = _BV(WGM21) | _BV(WGM20);( X8 `8 p) D, Q! n% T: {) f
- TCCR2B = _BV(CS21);" G$ q: @5 r0 K& y! j8 G, E
- Serial.begin(9600);
( O% v1 n1 t9 d' i0 } - buzzer.setpin(45);
2 S7 s0 G6 ~# K6 d0 R4 n3 X/ I3 C2 ~ - }" |; r' s4 ~4 ^: a
- 0 {/ p+ Y6 _; b# S3 d0 L
- void loop()
2 t' o, X- ~7 O8 T - {
- d) N; ?( r9 c - while(!((lightsensor_1.read()) < (10)));
3 M; X) d. w5 S - buzzer.tone(1047, 500);
, M/ V! ] q3 ~2 z9 H - delay(500);
" ~4 ^ B! M2 F' C3 S. ?) N - do
5 e; z- f q$ \ - {( m; L2 F5 ~) ?1 Y3 O
- int Err = getErr();. y9 i* E2 E# L' ~1 {
- if(D1 == 1)* x' j( g2 }% S2 Q9 a3 n. H
- {
/ H2 n2 _$ \9 ?8 |+ o0 |8 q - Left = 1;
- U$ T& I+ R t - }
5 V1 j; ?- {0 o C% E& R' o/ i' H - if(D6 == 1)
& t# V& w/ {* r. H6 |& t' B/ P - {* M0 C0 R: [, {* x9 |
- Left = 0;9 a& v: l" M- Y/ C# r2 P4 h# h% F
- }: X1 E5 Q- K5 t1 l
- if(Err == 99)
|* K5 ]% b! u! t7 k8 @ - {. i. b* |6 ~, \4 e* h
- if(Left == 1). A0 y, L9 }' w [" Q9 ^
- {
- F7 W X% ?6 P8 M* E - Speed -= 5;
7 K( `! M/ b7 O9 K - moto(0,Speed);
) T7 L X; ^/ z4 V9 n - do
5 F I: o3 J% x, x* p' d - {
) _ _* R; @* E5 s a - Err = getErr();
A+ y; l; M* g8 a - }while((D1+D6) == 0);5 U9 q7 ~& q8 r: G: T; X; G
- }
5 L1 }5 P% m4 F4 t" R2 T - else
' U, D& Q1 j# g8 ?* T; P - {
t& y9 k) }, `- n% s. R3 e0 B% l, m1 X - Speed -= 5;
A" z2 @( D M - moto(Speed,0);
8 j$ ]( }9 Q/ S - do
$ J' S# m+ w* l+ { - {$ F' I i8 b: M) b
- Err = getErr();6 k1 Y' M% o* F+ T
- }while((D1+D6) == 0);: Z$ O$ G* {+ h* Y7 f! N' w# {
- }
- W6 F0 v8 v/ k - }
) F {/ y# N% ]: L/ a - else
9 g) U) l' L4 j - {4 z1 K; [4 q: _( l/ e5 J. S. D
- if((Speed < 160) & (Err < 2)) Speed+=1;
5 H! x$ n2 V2 @ - if((Speed > 100) & (Err > 2)) Speed-=2; ! d) } W& _! Z# d* o7 B' r
- integral = integral + Err;1 u2 l* k: W- v
- derivative = Err - previous_error;
8 H) d) c U h7 ?, r- I( N, \1 a - output = Kp*Err + Ki*integral + Kd*derivative;5 j2 K5 a6 p l; w; L5 r
- moto(int(Speed-output),int(Speed+output));" T2 p6 T1 u3 T
- previous_error = Err;5 \+ M2 ~" A# P4 d4 E, c9 d
- }2 R9 e8 _' G* H0 P0 J
- }while(!((lightsensor_2.read()) < (10)));
. q4 _: c9 U" l - moto(0,0);! @# p: t. |, E! u4 h
- delay(500);
. Y U% x0 K6 j: g - buzzer.tone(262, 500);
* N+ t6 l" Y3 l( T+ |8 ]- [$ E0 Y - }, G' q* x4 ^8 m+ i
, Y0 R" G, U) v! E6 ~4 J' Y- int getErr()
# O# o, d+ @" G+ @, H - {
2 T* x% Y v' O5 |& f* l9 }4 L4 q9 ? - sData = linefollower.getValue();# M- w% H6 O9 C$ l" |6 G
- D1 = ~(sData>>0)&1;& v4 t& J# h8 Y; X
- D2 = ~(sData>>1)&1;
8 G0 X$ k4 c0 d9 t0 \ - D3 = ~(sData>>2)&1;
! e6 a8 B6 K! Q% ` P9 r) A - D4 = ~(sData>>3)&1;
* i Y. D2 x! e3 D8 v - D5 = ~(sData>>4)&1;- n9 ?$ E4 M1 _1 f
- D6 = ~(sData>>5)&1;! X+ e! j. Y% W# ~" o; c
- int downD = D1+D2+D3+D4+D5+D6;
: K( Y5 G1 F; H0 n" i! e) V8 E6 ] - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
% U9 S" m5 r, }0 w - if(downD == 0)( D4 Z9 a! _9 E9 L
- {* R7 H# i0 ^* H9 ~" `. d) C( w! ?
- return 99;
: D+ c! B' q- m0 t: a - }/ M8 p3 J$ j* d. `9 O; l
- else) e0 R) w7 x+ e- X) _1 p
- {
4 u6 Z& L6 s( Z% ~3 i - return upD/downD;- v8 @1 k- Q; E+ l
- }
1 N+ i# C% T6 T5 {/ J4 ^ - }- v3 K3 h" m5 T2 l# y8 I
" e6 |% H& }3 I! g5 d( Z6 W- void moto(int Speed_L,int Speed_R)
; s6 L: N+ L7 l( i% u; X! T, ~ - {/ l+ S2 F" v# \2 c' v
- Encoder_2.setMotorPwm(Speed_L);
1 h7 r) Z9 e& s( {# y6 ` - Encoder_1.setMotorPwm(-Speed_R);
+ _# p0 i% Y2 C4 ?9 C) f9 u - }
複製代碼 6 S4 F4 K6 g, h, K! y- h a/ u! B
MeLineFollowerArray.cpp
" i# ~! W6 |' M, i* S. _- R- #include "MeLineFollowerArray.h"
2 }3 `+ r# m' v! `8 R; \ - 4 I0 R9 h9 H4 A0 z- x$ n& h1 w
- #ifdef ME_PORT_DEFINED }" @6 v% _' q
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)0 d0 r" A, T8 {
- {2 @( z: d: _ Z! \- o
- 9 ~8 t. g3 c) ~1 K
- }
" J0 e" K; u7 a+ w( q) @ - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
5 [. u! g4 L/ N0 r" i - {
' r4 u1 a- L0 J. I. h( a - _DataPin = mePort[port].s2;
% F0 @" {, Z& H; ] - pinMode(_DataPin, OUTPUT);
# h9 @1 C0 k( [3 {( O q6 g- Y/ D8 j - digitalWrite(_DataPin, HIGH);
, X6 o/ |; e% V - }- K+ h0 M- G# n
- #else // ME_PORT_DEFINED6 v! A* Q- p6 m9 D1 C5 C- x
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
$ S( E3 E6 h% m - {
% D* j* C) P/ d! l: D( Q* H2 t6 N - _DataPin = pin;* t! o! W7 i. E* d) \2 e
- pinMode(_DataPin, OUTPUT);
! W' M: ^: f) s - digitalWrite(_DataPin, HIGH);( u/ q3 H7 r! X3 x c, A c# n" H
- }- F; I: t8 Y) e" ^' ?
- #endif // ME_PORT_DEFINED6 a6 R9 ?7 _) e; k
3 U- W$ n- q* N* j
/ \. p3 M+ h4 k7 f) k) c- void MeLineFollowerArray::setpin(uint8_t pin)
3 I7 f( F' q/ H8 _ - {
0 V; L6 o2 @ [9 p& C - _DataPin = pin;
8 r% ~7 r- L. r, d - pinMode(_DataPin, OUTPUT);
y. W4 n$ ?2 p' F' @ - digitalWrite(_DataPin, HIGH);6 p, F! t! _) K7 r
2 |6 p7 p3 c4 B+ `" y- #ifdef ME_PORT_DEFINED" o0 r& b3 t" X' c$ n# ~2 F" J0 _3 N
- s2 = pin;
7 c+ a, Y+ F3 L. O' ?/ q - #endif
3 @) s: \6 f% p- W6 P2 ? - }7 O J' ]# J$ z# Z) K
- p* g3 S1 Y0 @$ Q x$ d$ O; {- uint8_t MeLineFollowerArray::getValue()( Z x- \, }# x# r4 j
- {3 t0 ~: s8 Z& v: r
- uint32_t LOW_level_read_time;% }0 G3 b, _6 [2 z& v8 m, r
- uint32_t HIGH_level_read_time;& Y* K, U+ j. F0 X8 }# [
- uint32_t time_out_flag;# m4 J9 [. n( Q/ m
- uint8_t Sensor_Data[3];3 d1 h+ I; j S, X. s* X H
- static uint8_t old_data = 0xff;' V+ |# ` _. R W& }
- ' F/ {+ c. C. B
- pinMode(_DataPin, OUTPUT);
( u! \$ E& M) x8 O1 R8 b - digitalWrite(_DataPin, LOW);
! q7 @" q% ]; |3 Y2 y' L; b( n' ? - delayMicroseconds(980);% C% f4 e6 k8 M
- digitalWrite(_DataPin, HIGH);
* r( G/ q ~. L0 I3 A8 u p
) s0 M7 |" s b4 h- pinMode(_DataPin, INPUT_PULLUP);. S( O8 Z$ e8 {
- delayMicroseconds(10);
8 y$ Z) [+ M0 | - 6 ^/ E' j) ]4 ~& D8 z+ R, l
- time_out_flag = millis();( n# _- n5 i2 u j G
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( _9 U1 e9 L( O
- ! A5 _2 Z1 y1 I! A" o3 S/ B& D, }
- LOW_level_read_time = micros();
! T/ |* @+ m* X; V4 C - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* x# C" ~% N' _2 t% D" X+ e* x- g
- {
! D+ b' K2 @/ f6 w" I* ?9 |5 w - return 0xff;/ o3 q" |" ^* E" p. D
- }
( w, ?5 G! D2 N/ a
( n! C( b* z# Y& N- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' v, v( Y& i3 |9 \0 m; D
9 B* z, j' M& i) T+ ~ E& w" @- HIGH_level_read_time = micros();
+ }" i6 h4 O$ p6 ] - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level# g; p1 g9 [7 ?* H
- " e2 _' c$ b3 }4 y% m
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 ^4 b$ r5 P/ {6 \1 v5 f+ ^# c
- {6 d/ w1 Y7 a$ S
- return 0xff;
9 r9 x: F, E" L - }
* r; {/ K8 n' M- {+ v - & J) r! Y- K/ w% Z% z- L
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)), W: c' T P3 O3 d
- {
S: Y* ^5 m& B' P" ` - return 0xff;( W$ W C0 q1 o
- }
+ W% a0 E: z: a- b' O X2 Z) Z
5 k" \* f3 \3 W! p2 ~$ J/ ^- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 z5 s5 Q: q4 I4 |" e) E# L9 Q - LOW_level_read_time = micros();9 ^- D( e2 f! E& `
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
. M) l/ t/ N4 g$ L7 K8 E7 N& g0 q g
9 i% Q) l9 \8 y1 \; d T- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 J: M8 n5 i% Z# ~- W% f
- {2 c" ^1 O+ d" y# Y& ^
- return 0xff;
, K! k% u j! `# m, i - }
/ B* N7 a; @6 U3 w2 a - * M/ H) A6 H0 e$ b
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))+ y8 s+ l2 l) F' c/ s# D" n3 f' Q
- {9 K) F. ^0 o m1 }: z) l! G' G" H/ N: m
- return 0xff;' `6 d* c, C. D9 ]) Y% w
- }5 ~) D3 }: r2 V3 Y) ~
- % f0 x" j+ E+ C; e$ _( l& E
- for(uint8_t k=0; k<3; k++)& ?" \( ]" Q; Y
- {
0 a& a* ]& }; Y% k% w1 K6 q# y - Sensor_Data[k] = 0x00;* R- H2 }/ C( {5 Q* v
% @+ U( W2 D. `7 G" f) X% F& @; z- for(uint8_t i=0;i<8;i++)
# r0 x$ K! j: t3 f* _, a - {! d D" Q: z) R
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level6 w/ z# e( d6 o' q: K
- HIGH_level_read_time = micros();
$ w3 |- [7 {; Z; n* A - LOW_level_read_time = micros() - LOW_level_read_time;
* j4 d5 `. c$ c/ N1 b9 L - 5 ]/ R$ m' z7 E8 U$ Y5 L* e/ Q
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
0 D1 |8 H* _ u' w - {
5 x& q% C) F4 G - return 0xff;
; [+ |* B6 c1 h3 a - }/ O( E6 `/ W m* K( S+ J
& q' D! G3 i+ F4 O. f8 s: t- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 ]7 i# X" L/ F) i" P. k - LOW_level_read_time = micros();
k+ t Z* j; |4 ~/ i0 ]4 j$ u - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
: Q# K9 a& j6 t- L0 B
% E; B3 z& M, V. F/ G- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 18 F; t6 L) ]0 i+ T6 ?
- {) d. k( F8 d7 e# b* g- i8 |% B: P
- Sensor_Data[k] |= (0x80 >> i);* _" K2 g( [. Q
- }
4 Y, x% |* N) ~: h- ?7 [" t - else if(HIGH_level_read_time >= 100)
/ U% J! z" t& n) S - {" ?$ g i" {+ S+ K8 O; w9 G/ o
- return 0xff;: z' i4 \/ b g$ n% |
- }" f4 [7 S% g" ]$ P) _
- * z( e0 ]( V3 E9 S6 I
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)% A" D% v& \& K7 _4 D: \$ f- q, \
- {! o. N1 N* e3 k7 n' t( q) V
- return 0xff;
4 X! a3 R8 Z; s1 R - }! J7 }2 C) J/ Z) m7 _0 h- O; h
- }
3 y3 j/ l3 m* x5 q; O - }+ t8 V @9 T% c# r
$ `+ u5 ?) E+ w: y/ N, k; @! d- m- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level) M+ N. q! N: {" c1 y
- HIGH_level_read_time = micros();( W; j& T' R5 E4 R
- LOW_level_read_time = micros() - LOW_level_read_time;
" j( @6 K- S) }$ y# ~
6 F5 I! t0 d. a+ {0 ~- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
8 I' y6 |+ f W2 {' b - {; Z+ g5 \2 p; O. a# ]$ V& K/ X; ~ H* J' \
- return 0xff;6 x! c% e, }0 w3 f% i2 X
- }
* j, x/ f0 |) t+ b6 \/ m4 p1 K - 8 x4 v) p# e6 V, S, i+ r
- pinMode(_DataPin, OUTPUT);
, R, i' E: Q* l0 A& g - digitalWrite(_DataPin, HIGH);
" A) N# {2 y( _0 e9 R% z1 i2 h' y - 3 M! F% j& L N) J! U
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))- x) |) S0 X3 d9 k
- {
) z' S E' ^+ t2 H; g5 D1 a; g - old_data = Sensor_Data[0];9 V$ k# v- T6 A) I0 Y+ J8 ?
- return Sensor_Data[0];
( n! {3 O8 }7 j& b: U - }
/ e0 D4 r% Y/ a% \, H( M, M: _, L - else
/ s# y( F8 {) G9 y - {* `6 G+ |0 @+ C/ K1 n5 \
- return old_data;" \* i) D3 z. u* l
- }
, [# t' i4 F( ] - }
& ?, k3 a: F7 s6 N9 v+ q3 `1 a
複製代碼 4 l) h9 B0 D( d+ G, w" w6 C- q
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
, |% c% w( `0 Q/ E- E$ ?1 f7 {, Y. b4 p7 A; l" l
|
|