|
|
4 V% g2 v0 S5 N* V, A- #include <Arduino.h># I' `! o. ?; v
- #include <MeAuriga.h>
% U: V/ i- i/ e, a8 `7 y - #include "MeLineFollowerArray.h"
7 F. y& Z. G4 p) \5 g# n
% Q, T: d* B, B# d& Q; U- MeEncoderOnBoard Encoder_1(SLOT1);
7 J7 ^" d$ z) o4 M0 u/ g0 `: x" e - MeEncoderOnBoard Encoder_2(SLOT2);
- t( X8 `$ K; [, s5 K: \5 X0 k# X$ z - MeLightSensor lightsensor_1(12);
" [. y) X% q2 y0 b$ J- H - MeLightSensor lightsensor_2(11);2 r2 i* k( k2 q+ ]# O6 d
- MeBuzzer buzzer;# k4 \: A7 K% Q' W6 v9 J
- MeLineFollowerArray linefollower(PORT_6);& _- M$ W4 y$ ^
- - |3 \7 G: S+ Z7 p" ^ y
- #define Error1 1
8 L+ e! g0 w; v; f- G# h, k5 d: h - #define Error2 2
* G/ y. @0 I1 b4 \# S0 ` - #define Error3 3* C$ m2 b6 P0 E+ v5 t
- ) b: o0 w! U) e3 h
- #define Kp 15
! u9 F: |1 s% u3 q - #define Ki 0.15
1 ^% }1 H1 v7 J e- \ - #define Kd 0.03
. J& K- r, l( T - ) t6 g" e7 a0 e9 V1 y. f s* F% t1 B
- uint8_t sData;3 |+ t8 S0 ?" C
- uint8_t D1;; S Z0 b2 b3 ?7 k1 O3 l- O, x
- uint8_t D2;
# o" o/ k0 o0 M( P W0 l - uint8_t D3;
# Y% a9 F& u1 ^( i. s - uint8_t D4;
0 P! T# _, H4 ~' S0 W1 j) n0 Q - uint8_t D5;& U) B! M/ d- J: u
- uint8_t D6;: v# C3 ~( e% |1 _: b
- ; D! X* Z6 D1 q! }5 y# F
- float previous_error = 0;
& K/ V- [! c" A7 h: a - float integral = 0;
" b1 q! R+ Y* U) P# E+ |& q - float derivative = 0;4 X6 K& q* R3 [. f/ n/ j
- int Speed = 160;# T+ t; V' b+ j- O, v" W
- float output;% ^8 C( b$ i* o2 L/ @" q1 l- ^& G
2 t# g* r9 ^+ V6 s* f- a& w' m- J6 K- byte Left;
8 ~, m% R' Y. G& q$ R4 o z' v: V# z
' k: b; h! L/ k; o! \- void setup()
/ C1 O+ {9 o$ f2 i - {
" ]! h, D0 L/ a- h4 E6 | - //Set PWM 8KHz
/ T6 K: J+ z+ w5 P7 a) ` - TCCR1A = _BV(WGM10);
" w- ]; | b7 B& ~* q7 y( D - TCCR1B = _BV(CS11) | _BV(WGM12);: p% N% K# l0 B/ I |( \- N( p
- TCCR2A = _BV(WGM21) | _BV(WGM20);- C# [* S% y! g
- TCCR2B = _BV(CS21);% ?5 F( ^% L6 U. C" }, C0 q$ j* `
- Serial.begin(9600);( m$ {" a1 i0 C7 ~; A/ h5 D% y
- buzzer.setpin(45);
: D5 g4 x. h$ s* {0 Q% ? - }% t2 h7 ]( o" c% d* B5 E
/ X. P9 j6 p; O: w; A- void loop()
" T: d# M& \% R/ M1 G+ l - {# X( I6 {/ [( r. D
- while(!((lightsensor_1.read()) < (10)));
2 l3 V5 Q7 A" @8 M0 A7 u- C9 U - buzzer.tone(1047, 500);
. i8 \$ R0 C# I - delay(500);: [$ N, }# z3 Y/ ~
- do
0 \# J% J( W B& U - {
1 P# F" s2 B1 R' v4 J3 T - int Err = getErr();9 ~) _$ @! B! S' d0 P
- if(D1 == 1)" p' H' t* z9 ~/ Q
- {
. g% a5 Y6 A$ S; W' _ - Left = 1;
7 I/ i; Z4 t9 E9 _: a( ~ - }
, L8 a: A; ^8 u/ U6 h) X - if(D6 == 1)& n3 T, Y- w/ Z1 Z: }
- { W) e, e7 R6 A" d" T. \$ ^
- Left = 0;4 U7 ~. `! v$ H5 a/ [
- }5 d; I4 U& ^; s/ x6 G/ y$ N+ K
- if(Err == 99)
* y& U( R! t3 [1 i$ } - {
@5 C1 M2 a/ u- h! W; h9 Y - if(Left == 1)9 Q% q3 g# U( V) z! ^( p2 `
- {* r1 R' L. f0 v* f o
- Speed -= 5;( S; h/ ^8 b p! }' f3 y U
- moto(0,Speed);
" @' Q4 K E( N; M" }' g ~. H8 j - do9 U( ?! v/ v1 d
- {' ]/ w, X9 L, [$ a& l+ W4 R0 {5 \
- Err = getErr();
) q$ [% ^' R% F& i6 S - }while((D1+D6) == 0);
/ @. k5 B2 ~/ c - }; A/ J& ]. F$ _
- else1 K+ `8 g: k% S" x' v0 F
- {* ?5 t# A& g( g1 ~' C6 u
- Speed -= 5;" x. ~7 x6 ?5 o& l7 g" b
- moto(Speed,0);
S R; b7 p4 r; _ M- {$ T. C9 p - do
1 Y+ ^# q: C) S& T3 ~ - {( c# E+ e W( i2 q. {; M0 z# X
- Err = getErr();6 Y1 ?& Z- a& @/ I" r) z
- }while((D1+D6) == 0);
5 H5 v' h% p7 d( B. F - }/ i* K/ e- L: @
- }% W5 s. A/ C' U1 }7 f
- else
9 q6 l' l. ]8 d3 x - {
9 D8 Q& c @$ _. p - if((Speed < 160) & (Err < 2)) Speed+=1;
3 k6 Z3 t( v8 P# e [ - if((Speed > 100) & (Err > 2)) Speed-=2; & {9 a- ~' M! ^$ C+ e! [3 T
- integral = integral + Err; P6 b0 q' @( x* Y1 u, O( J2 @
- derivative = Err - previous_error;
3 n' _# b7 c7 H3 x% I9 K5 } - output = Kp*Err + Ki*integral + Kd*derivative;' {4 [+ u4 N( o: _2 B! q* h6 {
- moto(int(Speed-output),int(Speed+output));
, m3 t; D3 _6 |+ y$ x3 ?6 W* _ - previous_error = Err;1 W5 H; V5 f) y0 i7 ^3 Y* f) `
- } Y- U. c- ~. V: _/ r
- }while(!((lightsensor_2.read()) < (10)));
8 O; A7 ^+ D- @3 R. Y, Z. L3 s% B - moto(0,0);' y% g8 `. G8 H: t, t4 `; X
- delay(500);6 x8 i2 n% H. W5 w/ X, R+ Y
- buzzer.tone(262, 500);) K! f8 \# B+ e4 J; L
- }
$ d6 ?- N! W9 a( ?2 ? - 8 _5 c9 f2 J( I9 ?
- int getErr()
* C' e- p& D( ]" h+ k# q - {
$ V. l! N* b, y* g9 q) u - sData = linefollower.getValue();9 j/ z% }/ m: o7 K; w
- D1 = ~(sData>>0)&1;! G8 S# X' C2 c7 A, C; C5 o
- D2 = ~(sData>>1)&1;( U2 [4 ~6 S3 T
- D3 = ~(sData>>2)&1;
( V" P, R, k9 N/ g8 J" \& s - D4 = ~(sData>>3)&1;! ]4 Y9 @9 g9 l$ F/ x- q
- D5 = ~(sData>>4)&1;% `, ^3 d# B" r! F$ y o
- D6 = ~(sData>>5)&1;" `0 e( O* a7 C3 ]) r
- int downD = D1+D2+D3+D4+D5+D6;+ e% M/ M" e2 m A8 C7 g& [
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);& G; I0 F, C1 a" E6 S# Q
- if(downD == 0)7 r6 V9 z2 j' P3 Y& P4 ] `' f
- {' j; C9 p0 O; ^
- return 99;
- d5 u, Q9 }- D+ M* ^* _& `) y; o4 B - }
1 t' l' _1 V: |8 h; C - else
' ^. ~/ V) h) j; e5 w - {2 k. c% z! J: z9 w1 e! s
- return upD/downD;
, S0 M3 V# @6 z# a% @4 ] `% T3 d - }
" w4 s5 _/ w3 |& P$ i - }0 j% e( w. k9 `0 b3 M6 g: F L
& [* A* r8 R, C- void moto(int Speed_L,int Speed_R)
0 q1 B5 P8 ?3 [ - {) n7 L' z; Q; ]' }9 H
- Encoder_2.setMotorPwm(Speed_L);! X1 h2 [8 ~) o1 x. i
- Encoder_1.setMotorPwm(-Speed_R);7 Y: d! s$ C- c4 y, O
- }
複製代碼 , a$ z2 i& `% P3 J/ G& c
MeLineFollowerArray.cpp
- C ~" @( v# K) _- #include "MeLineFollowerArray.h"
7 E8 G2 T# ^4 V% L) G - 9 F+ I( C* g* W% F5 _' S
- #ifdef ME_PORT_DEFINED9 o& _2 a Y2 E2 d" H
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0); h4 o+ t1 s% u) `) x" B( A* ^$ Z
- {
* g3 B+ w6 C0 F- @/ r5 @$ c
; }# o" _( E, y- l0 Q( [- }
' \( k/ V! O8 _3 b- E; y4 Z/ A - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port). z) s- g8 Z" Q6 i4 ^3 ^6 Z/ m$ J2 @
- {5 ?$ a+ w; i7 U; i
- _DataPin = mePort[port].s2;
3 k% d# S, {5 d P/ I- u, s9 | - pinMode(_DataPin, OUTPUT);
- S" I7 s3 E& R/ F - digitalWrite(_DataPin, HIGH);
2 a/ M* H* i3 s; N1 J - }8 N: f0 Z0 r. o6 u2 \; o! f ?) z
- #else // ME_PORT_DEFINED9 z) }2 S! J x
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
7 a* I& S$ N, E3 m - {/ F m) @& F2 ?# X3 k n* R! ~
- _DataPin = pin;' S8 t7 B# D7 j+ ] o, a
- pinMode(_DataPin, OUTPUT);
j+ r7 i- r! `; Q/ n - digitalWrite(_DataPin, HIGH);9 i! Y3 w! ?9 r% z; b: I$ g
- }# q( ?; Z3 v' r c
- #endif // ME_PORT_DEFINED7 X: s+ |7 E9 _& u K9 t
* A1 {9 C' G) W
3 u& _0 W+ D6 }6 ^; |* r5 C- void MeLineFollowerArray::setpin(uint8_t pin)
; x" ]# j* F( X - {- I4 S9 F* S8 R2 p+ s- L8 T
- _DataPin = pin;
- A& [( \! X4 _1 B. Y - pinMode(_DataPin, OUTPUT); : z! R+ T* s4 K
- digitalWrite(_DataPin, HIGH); t- ^8 l. B4 Q% }* K3 u
1 L! k/ `9 p9 v3 n% b+ i/ j- #ifdef ME_PORT_DEFINED' {- n4 y8 K; s
- s2 = pin;
+ B: D5 [5 O$ Y- `2 a$ l - #endif
" f! Q- C6 e& u+ |4 h( [! P. M4 Z - }
6 y9 E' C) M4 \, s* V* ` - & ^; L; L# K/ \1 n* f# d" B6 m
- uint8_t MeLineFollowerArray::getValue()
8 ]* j6 P' ]# ]- _4 l) j' |- ^ - {2 _8 C3 }# O( m: _1 F! v6 A7 S
- uint32_t LOW_level_read_time; ~( C7 y5 |7 O% s( }
- uint32_t HIGH_level_read_time;
( B, K* t( E o9 B* ? - uint32_t time_out_flag;( ~1 c a! P |/ G& N* P3 ^; l
- uint8_t Sensor_Data[3];
6 G/ K( U/ T( T/ ~3 `( | - static uint8_t old_data = 0xff;
4 E" T5 b2 M8 Y' Z) C+ p
/ t8 `0 I- T* c9 \- pinMode(_DataPin, OUTPUT);
) j1 T6 o _9 [ - digitalWrite(_DataPin, LOW);
$ U4 X5 v2 X2 J, T) S; g. T - delayMicroseconds(980);
* Y R# X3 Y( [: v! R; D - digitalWrite(_DataPin, HIGH);& L3 n# F& [( i6 y
9 R& l2 h* o# ^+ k6 z% d) u- pinMode(_DataPin, INPUT_PULLUP);8 k6 A# }2 R# t0 g& i" |
- delayMicroseconds(10);
9 z, d( ~# o6 Z$ E% o6 n/ j6 W
) Y$ H$ o( m* [% B5 g. Z% E4 j1 ^4 y- time_out_flag = millis();1 b$ _3 T0 q5 ?1 ?# B
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ I6 m" @: w( `* ^
( N, @) Y+ k4 {/ }2 c( @5 {- LOW_level_read_time = micros();8 ^7 H! n3 S; I7 d
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
! l& D% @( x' w3 i - {- ]+ X7 S! U, Z D! S* h1 H& i
- return 0xff;! c6 U7 S. H4 m) Z/ V
- }
) a% e+ ] c) w/ I: F- l
8 {, N% u. L! [) Q7 _/ I' s4 ]& t- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );4 V% u6 i y8 Y8 G/ Y' Q. g
1 u4 ~2 c+ {/ C f( n& v- HIGH_level_read_time = micros();
; e8 N1 ]; Y- u: E; {. V - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level$ i# k/ G) Y$ C' [* d
0 \- L2 t' G3 u4 @" z' t1 J- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
5 w- c" g+ w9 X5 l - {/ F9 Z, V2 b/ s b6 p8 V
- return 0xff;9 u+ U/ ^, }0 b z* x% ^
- }- `! k4 F8 @" D% F
- 0 G9 g% ?7 K0 w4 ?8 l0 f
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
+ l% s8 _6 W4 Z2 v. m$ d6 v - {
5 ?# e. _. V# U1 [7 k# o3 t3 W% D - return 0xff;4 O. M7 w# ^( A
- }/ Q, h* B2 t6 H& t2 {2 }
* Z0 h7 s$ y0 \( \/ ~ Y2 c- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 E" y3 `! u5 a4 c" q( j - LOW_level_read_time = micros();
# G3 a" v: H. s- }; N4 w6 H9 Y - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
, x" B. N# ?) J - ! `8 i6 E% Z- \& u6 d1 ^4 K* b2 P
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out0 o$ }3 ]* n* o! w* j( e4 Y; R
- {" r1 P! i1 `5 Z X, I3 K
- return 0xff;1 J) U! W# P6 R$ y7 w Q3 [* t
- }) G( G+ w0 H, }( R" P
- ) t( m) I$ q. U* z4 x/ A
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
1 e: y5 B& Z l! ]3 M - {
/ z4 }/ R: Y% ]8 s9 W( W: s4 P - return 0xff;/ |% |2 z. e, A& v
- }" U* c9 T6 H7 h; N) b% }( l6 _
- 9 i% {0 ~5 h( P* n: ~: G/ c- v0 l
- for(uint8_t k=0; k<3; k++)
; t0 C- U W; g3 G - {
4 [9 k' d, |( G: o e+ D - Sensor_Data[k] = 0x00;+ v5 N1 l# H- D0 w8 ^+ {8 s
& t( G2 f" U3 b) i4 _9 X2 G- for(uint8_t i=0;i<8;i++)4 y7 N. s8 r8 ~9 }( J4 M
- {1 L' J+ K1 Y, V7 I( g* m1 Q9 X+ f
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level6 ?, f0 A: Z1 @- Q# V( W& K
- HIGH_level_read_time = micros();
( ^7 R7 K* [( x* V - LOW_level_read_time = micros() - LOW_level_read_time;# D, l/ Z+ C& [4 n& T
- 7 W( C* u0 o) n ]4 \) D2 B( V
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
/ |! f7 J2 X! r4 F# c: t - {: x( P& M9 |2 e2 c, Z8 Y% {
- return 0xff;" p, K, H+ [" E: L1 f+ u; b
- }& ^0 f! m7 B# \; ^4 S* ^: }
- 9 }1 _0 U( c0 f+ Z7 W. N
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );% E* W/ K6 o" ]
- LOW_level_read_time = micros();
% X5 }* k5 _* P% U2 N) V0 e - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
1 Y/ J, C! o5 G! V( s/ w4 [
/ x8 k3 }' w9 ?) n5 f- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1( P; v |5 [0 {! j7 p; _; U
- {% y6 B, Y6 w# j* \9 z; y
- Sensor_Data[k] |= (0x80 >> i);
5 P: r8 o9 Z; V$ [0 l+ | - }8 K# h% l2 g. G6 e. w# Z
- else if(HIGH_level_read_time >= 100)2 e9 j8 n" s) k3 V
- {) b. C1 u* y/ n- V' k
- return 0xff;
; ]% D0 t3 d$ A5 |( r) Y$ ] - }
0 Y/ Y/ R- N- V9 c% ?
; ?! }% D, P# D. `- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)+ ~( B4 A% W# P+ e! I3 O$ w
- {! y2 F: b( X5 A0 B
- return 0xff;: l# w; y( G. H
- }" K# w# X, V4 r' p1 a! b% i
- }
2 f: r' Q7 v# w; @3 b) c - }
; r; [6 r3 K1 ^; v1 k - 2 ?6 a1 i: }, b/ \/ c+ Y7 C) h
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level, X& `; x- M1 Z# l' r
- HIGH_level_read_time = micros();7 i9 ]& B" |: D/ D
- LOW_level_read_time = micros() - LOW_level_read_time;7 T" O+ t8 g$ e3 v9 W' k
- ( f0 u# i+ {" h8 U. C' V2 l
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
- l; z% X/ y6 z& |7 f* d. B# P - {
: [6 {4 `, T. U, Y+ c6 j - return 0xff;" t: y4 s$ f6 `
- }% W5 [# N8 j. R3 p, {# Z3 \
- 8 e$ [" {: p+ ]
- pinMode(_DataPin, OUTPUT);
' Y4 D4 ` u% [* ^ A8 [ - digitalWrite(_DataPin, HIGH);- r7 r# l" E( c, g2 y
4 }6 k% C, O: x9 Y- x- W- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))% V) O! k' q. g ~+ s
- {
# P% u/ r4 c( U+ ?/ E# S - old_data = Sensor_Data[0];
: p- P1 L. S- s - return Sensor_Data[0]; y5 Q ?( ?& o; K Q; K
- }: D" J4 t0 @, S( V5 l
- else5 D5 Y% \; r R9 |% ~$ Z& Z
- {. E! b! D7 H, D2 I5 A
- return old_data;8 x+ @7 j5 |( s5 y3 ]. Q2 ?
- }
. E+ C* I1 h9 i3 k9 k$ s' A - }& ?. @+ R4 u: U
複製代碼 " K. Y* t# J" B5 [; i, Y4 c& a
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
: B7 N- ]/ r, t; I6 t5 B
$ S" d6 D$ z5 ], a% b- X6 V |
|