|
|
% U4 p8 V3 D, D. W/ a6 }7 R. _/ D# D
- #include <Arduino.h>5 Y6 p: i5 v0 Y! b% U) y
- #include <MeAuriga.h># T9 w1 y1 N8 P; \% Q
- #include "MeLineFollowerArray.h"
7 ? d/ ~ K1 N0 J1 @. { - 1 W& p) I, y3 v5 h8 F
- MeEncoderOnBoard Encoder_1(SLOT1);
& h5 E. p. r# i" q5 q {1 j - MeEncoderOnBoard Encoder_2(SLOT2);
- n4 g( `0 N$ g0 i) {8 i( ^+ b - MeLightSensor lightsensor_1(12);' U/ C9 J! i! I$ [$ O; n% f9 u
- MeLightSensor lightsensor_2(11);
; j/ j8 s7 M$ E, l+ M - MeBuzzer buzzer;
: ?1 r+ O; ]* J! {- K4 e/ r: I - MeLineFollowerArray linefollower(PORT_6);
( M; L" D+ D# p5 ] - $ ~- H) p( N3 v1 b8 D: W9 n" H" Y
- #define Error1 1$ C" i0 d/ V% c* G
- #define Error2 2
' o* v8 y3 }& F! {" @- G - #define Error3 3' d+ J2 o! e' ?- @/ A B7 x5 }& t
+ ~6 ^2 l( B* f- #define Kp 15
5 v' x" s Q8 O; m4 n - #define Ki 0.15/ K9 B& q: J Z+ b* A, y
- #define Kd 0.03
3 N* G* ^' r$ G" s8 k8 u9 p- h9 a* U
. H# r. I( k3 @6 ^3 \" R- uint8_t sData;0 M) P! a; R# g& q+ w
- uint8_t D1;
9 V, p# G* F% H' ^# v2 F - uint8_t D2;- L( I2 l0 M0 s+ `" Y7 [
- uint8_t D3;
( H+ A% S' H/ P+ @* N( c - uint8_t D4;
5 X/ i- k# G, d6 n" V - uint8_t D5;
. H/ b! W5 Y4 K& D& x! S - uint8_t D6;& i3 Q, k: @, Y$ U4 J
" P/ G$ X4 G: T" h+ G! L1 n; Y- float previous_error = 0;) N2 j6 B( X, l8 t. D
- float integral = 0;
0 y5 i2 e0 S- u& @) P% G - float derivative = 0;
4 c* K3 g. V. p R - int Speed = 160;
0 K6 p% U1 ]0 Q6 O - float output;* N$ |' Z& L! M
5 ]1 c+ m) m' G- U: v9 c8 h- byte Left;& a r8 C& |4 w# j4 c
- 4 p3 H* J0 k* |9 O0 w
- void setup() 0 G$ ], Q" s* B) U5 u* H, _
- {3 f2 I7 s' Z$ W7 u* e
- //Set PWM 8KHz' B5 T: S; C k% F( L8 `6 H$ Y
- TCCR1A = _BV(WGM10);* p5 Y; {* y) C2 q: J; J
- TCCR1B = _BV(CS11) | _BV(WGM12);2 i! L$ A2 n0 U8 c: M
- TCCR2A = _BV(WGM21) | _BV(WGM20);; o9 b" r6 V' _; f" n. Q
- TCCR2B = _BV(CS21);# n2 M0 T2 \9 `* G- k) t: y1 b" B
- Serial.begin(9600);2 S' r: M( f! ]9 ~) ~: K+ V
- buzzer.setpin(45);
. f6 |: R, r u: q - }
! [" T' N0 G( t/ I r2 i: V - 4 n1 a* Q: L; h
- void loop()
0 A% O+ H4 Y7 W/ C* j+ \. f; N1 d - {/ n1 V# \( w& q4 X Z
- while(!((lightsensor_1.read()) < (10)));
U" W$ a G* ?2 t9 g k: b. y - buzzer.tone(1047, 500);
8 M9 ~2 P/ K0 k: z/ C, n - delay(500);) y5 o4 e2 z2 E0 h
- do2 j' y) Q* n8 A9 d- U
- {
2 ?% j/ l$ @! d: G+ Q n - int Err = getErr();( I. i+ j' T' p) y
- if(D1 == 1)5 ~. J$ ]+ s4 g5 ^& b) i* [
- {
& D3 E' m. r, ]# J: N1 B$ q5 e - Left = 1;- w8 s# ]. f+ }/ T' ~ |/ U- u
- }# ~! Q" y, x& j) Q# P; a F6 H
- if(D6 == 1)1 q, S+ `2 u" V8 b' @, ?6 ~+ o: L6 I
- {
# |- s3 ]9 ~+ b9 [) d0 B/ t2 I - Left = 0;2 k' \& Z$ g4 |8 a& e, r, W, i
- }
. X* j5 v2 G) r3 r( B - if(Err == 99)# P$ y8 I& W$ M; g
- {
. \& W1 s3 G) H" ]" o4 z - if(Left == 1)
; Y) f/ |5 C: k% r7 T. u; m+ I4 e - {
7 d7 a) A& a; b/ Z% ^" I - Speed -= 5;
/ h/ w3 C' G7 u; i" R - moto(0,Speed);
3 f* H: c9 I8 U+ B* S1 L, l3 n9 @ - do' E7 F& ?0 W) z4 w) N
- {
, b6 w4 M! K1 L4 k - Err = getErr();
: Z# g \' t- V; |8 A - }while((D1+D6) == 0);
* V p; t4 }. I) a S - }, B. _3 U2 a' j5 S& }3 B6 z
- else" ~% |9 {* \$ R# S8 c4 Y
- {# S8 c* O- s8 j" e! I) n
- Speed -= 5;
) B" i6 w# y# R% O0 s% [9 r - moto(Speed,0);
7 t* x" P+ x+ ~: X - do; u) ]/ }. \9 T2 D4 q( C
- {
5 x: ]" J3 V2 K3 t& q - Err = getErr();6 D1 x8 ?/ U6 S# h" p
- }while((D1+D6) == 0);
6 W9 b, Y- |6 ?! y - }& [# j% @% @* ]( ~ b0 h' c1 M: }4 f; a
- }
3 J+ U% b" @& ^6 K& F# ^4 o8 i: i - else
* }/ e8 a+ [) U0 c, s' E# Y4 [ - {
1 p Z" q9 J" l# ] - if((Speed < 160) & (Err < 2)) Speed+=1;
3 {, v' c% T3 @3 o- L1 V - if((Speed > 100) & (Err > 2)) Speed-=2; - @. s- j1 z. F0 D! `4 }
- integral = integral + Err;
9 z3 L6 d* P; j6 n# W+ r - derivative = Err - previous_error;
* ~# E, V2 h; y' ? - output = Kp*Err + Ki*integral + Kd*derivative;7 I. r1 |5 j. n6 `+ O- q
- moto(int(Speed-output),int(Speed+output));/ L* I" y7 |+ I
- previous_error = Err;" N& w# n; J8 c+ b5 [7 Z
- }
: H2 H: R. i) d$ ?1 E) e3 h - }while(!((lightsensor_2.read()) < (10)));" R5 a2 O S- O) e6 Y( p: n! i
- moto(0,0);' y6 n: Q& w5 k& `: M
- delay(500);
! X% \( z" Y! a* |) Q/ I; K - buzzer.tone(262, 500);
- i9 D4 j; \( b. Y# q9 D- i - }
0 p! r, @1 P/ `4 P- W1 \
$ ?2 s& Z# h3 u1 z1 g2 G$ j- int getErr()
4 T* [; S/ w) B - {
" K: b: s; `9 t" j - sData = linefollower.getValue();
4 D' J) n, z$ x4 Q - D1 = ~(sData>>0)&1;
7 T4 h6 }" p1 A& @4 }0 N t - D2 = ~(sData>>1)&1;( b& i( u, ^8 I5 N) R" v" N
- D3 = ~(sData>>2)&1;
6 |8 h" x. w. P4 Z - D4 = ~(sData>>3)&1;
4 W7 P5 A' A' k9 v E - D5 = ~(sData>>4)&1;
. A. S9 x- D. N3 o - D6 = ~(sData>>5)&1;, M! K" O4 @# A7 P% Y# m+ A$ _
- int downD = D1+D2+D3+D4+D5+D6;
! k4 P* `( M- O1 W - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
( I6 O* h' o$ I8 l9 s - if(downD == 0)6 b0 O6 v5 K" {/ W$ c
- {
/ ~; t: z5 ]" Y* \ o - return 99;5 r# [* O; U; n, E4 R. N
- }& G8 z( H: V' o! \. P* R0 z6 B0 u
- else
6 {2 W; o n& L* [ - {
8 c6 k: V2 t! u% w - return upD/downD;
+ T6 d" l+ v; O. I/ B( U+ P. s - }
0 {5 z5 d1 V/ X: r$ f - }( a- L$ t9 V5 S
- ' w5 z4 D. C/ ]* w, t# N
- void moto(int Speed_L,int Speed_R)
2 O: j3 M$ H) K/ D9 j* o/ T5 N - {
2 O( o$ ]- w' O% q9 x - Encoder_2.setMotorPwm(Speed_L);
{0 f) {% P# ]7 v+ { - Encoder_1.setMotorPwm(-Speed_R);
( }" r( D) L+ A- O( w) q - }
複製代碼 $ J! x0 ~! c$ L1 ^) I& K" a3 |
MeLineFollowerArray.cpp2 U& w" T9 j% {' x8 P
- #include "MeLineFollowerArray.h"
& n; C$ l/ u3 w* I/ z h' t! ]
3 h+ |% i% @5 ~- #ifdef ME_PORT_DEFINED8 Q. ^5 v) ~% @6 G' ]% A3 e" W
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
6 }* M3 j7 f* i. X - {
8 [# U: ~: r5 F$ s( K1 Q3 M5 A
$ Y2 _- R7 f& i( H: x- }* s4 m. V6 _/ K3 u9 d
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
3 u6 t# k1 a' q7 m, b3 P3 X - {
- K4 Y% }, K, t7 e* N* m M - _DataPin = mePort[port].s2;
* d, a- f9 t. `7 }0 l - pinMode(_DataPin, OUTPUT); : k M2 d8 W# H! p8 |
- digitalWrite(_DataPin, HIGH);: ]% `* L f$ J0 B) q: `% b
- }
6 d6 T# U7 O+ T' N7 a6 c - #else // ME_PORT_DEFINED
7 s3 W9 `3 K$ G( I - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)$ a; _ L+ w' F3 v: b
- {
! f: Z9 L* X! i+ X5 Q( I: o# U5 S - _DataPin = pin;1 t7 [! L4 g/ e& c2 a; ?
- pinMode(_DataPin, OUTPUT); : ^# \/ f3 d9 `
- digitalWrite(_DataPin, HIGH);; F$ n+ i4 H+ P7 _& ~4 s
- }# o: c# d0 l, P( T" g. X( S
- #endif // ME_PORT_DEFINED3 Y ~* X6 \4 g' q3 V" Z9 B
- , |1 Q% n, M6 o/ |3 i. A& p, ?
- z3 |' |& X! E8 x! t6 a" H- void MeLineFollowerArray::setpin(uint8_t pin)) E1 u6 O4 ?6 R
- {* {& G+ `1 c$ z1 U8 Q& m" V
- _DataPin = pin;
/ r: O9 S9 S, x% ` - pinMode(_DataPin, OUTPUT); ( n. @9 ^ X* P+ X2 q" Q0 _, H
- digitalWrite(_DataPin, HIGH);6 ~/ k5 ^6 J- C" Z8 n
- + B) `$ T# W) O+ m( q4 m. g. J* F
- #ifdef ME_PORT_DEFINED z+ ?* P9 }1 L4 S* D. X
- s2 = pin;
3 L. a0 i/ o, s5 }+ o - #endif% t& T2 X6 f# P0 Y# L8 B3 `
- }
6 u# M5 `& |5 j( R8 f0 H
' E G* c5 A T! W7 e- uint8_t MeLineFollowerArray::getValue()
2 a' S% n. W4 c6 w - {
! S6 M$ Z' Z. C7 m3 Z - uint32_t LOW_level_read_time;# G0 ?* @6 l: L# I3 ^! v
- uint32_t HIGH_level_read_time;
/ L( g m3 N+ x8 a8 }+ b4 b: k - uint32_t time_out_flag;7 o( Y" v/ z. T7 f; P( d
- uint8_t Sensor_Data[3];) U6 b- V) j4 Z, r _: i
- static uint8_t old_data = 0xff;* Z6 T: h- B: ^9 R g6 `- [. |+ G3 p% M
# X* b5 a6 }3 \! v( C( E- pinMode(_DataPin, OUTPUT);
+ T: ?$ N% t. W( f7 M& i) Z+ J7 k - digitalWrite(_DataPin, LOW);5 _" D: e6 Y1 x+ e! ~; b
- delayMicroseconds(980);
( e1 r8 u, E% k; K - digitalWrite(_DataPin, HIGH);
% v- c$ n, Y4 A
2 s5 w. l# [7 E& h* V* f' J- pinMode(_DataPin, INPUT_PULLUP);
3 w7 r4 z( `6 N5 t: ? - delayMicroseconds(10);) e4 G; O; j7 G
, b: S* o; I) Z. P9 P+ \. c- time_out_flag = millis();
* }0 k; M) c8 \ - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
n) r. A; y' n9 W1 }5 {! @+ z - & j6 K# V, Q/ E: ]
- LOW_level_read_time = micros();4 D" _" i3 r! _" g5 u
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
5 @+ C# p0 g, W w% Y - {5 R5 ^3 u- K+ ^8 `! a8 @
- return 0xff;
7 \, p$ ^$ s; W l. w - }$ f' Q2 f! }$ U2 t7 S- X" {
- 3 v. V% J. O P) ^
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ Y9 m/ u3 I. [* a - 8 G& M& A+ q# P1 s: P+ S* m
- HIGH_level_read_time = micros();1 q: G9 p$ D8 M. J0 j- e
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level& K" C& a& {) ~2 k/ N
- ( t3 J' |) e# Q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 { C* O1 R+ v s' g! [$ {* e, b: v - {6 d6 c. K/ E6 Z" F
- return 0xff;
) R4 ]) i+ H( ?' g& G* M! q - }
1 ~/ x: |! C( f$ g9 x1 U2 Z: @, J
" a0 ]) E$ `1 [/ n2 D- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))7 c5 L8 E2 h& w4 E: g
- {; q' V- k! n$ D2 d% ^7 y
- return 0xff;( U: U: |3 |4 {& ^, Y$ [
- }( `5 n, G" s2 G& F! P7 Y+ F; K
0 U [7 m3 t; W$ X6 M1 z- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );6 W2 ~) _) M% @
- LOW_level_read_time = micros();
- s& o+ d: t3 ~* d - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level6 C* Y. E" ^$ z6 q+ ?3 y
- 3 e2 }3 M2 R8 z6 C- j3 m
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out# E' p1 T; O4 L
- {% [5 b. @1 v* q: ~/ n% ?
- return 0xff;
& a8 j3 O" {7 V# G& Z! I# h6 [% a - }
) r5 r2 X5 i1 b& F4 n
) n: W8 U ]: o& R: O6 K- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
" z5 F) G! B* z: Z& W1 q - {
. |$ S- i/ b6 E' h2 e$ _ - return 0xff;
4 z* e/ [) c- v. c" D% T5 c - }1 z! Y; {' N8 G9 I
- b6 L! Q, a8 U
- for(uint8_t k=0; k<3; k++)
3 o8 X/ S: D/ M) [- b - {
3 o5 j' Z: H' `9 @& \ - Sensor_Data[k] = 0x00;
- ^7 r5 L* b2 A; M* u5 k& Z" p6 C* }
. X3 l3 {6 Q+ i+ a" E- for(uint8_t i=0;i<8;i++)2 B. a- j8 b3 a2 w+ |
- {# u# A; p n, ?4 g& i3 r
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
* U& T2 s4 S$ M( |# @ - HIGH_level_read_time = micros();
9 b( c4 s& y- e1 C# G - LOW_level_read_time = micros() - LOW_level_read_time;
' ?8 e/ E/ E+ ]) x
; w W# s* z/ M, p6 U( W, m; J- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )% H/ r0 G$ P0 [; K
- {$ W" z. a( R/ P+ a3 p+ t
- return 0xff;8 r9 [6 s7 A, {, [, Y6 C7 Q
- }
. V% S& S. O& P- a - / B; V$ ^* b3 \) G4 p9 j' ]
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );7 d7 e; `2 v2 r+ y0 \: @
- LOW_level_read_time = micros();
$ q' z9 `, _1 O4 E8 t! d - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
$ i! @9 D) }$ Z! _, A - 6 Y$ ]' G5 f6 C& K, ]
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
& K/ {7 h/ |, S( y6 d0 f8 m - {* Q8 d- g. _" G
- Sensor_Data[k] |= (0x80 >> i);9 c6 m: ~" z4 c+ w6 m
- }0 C% ]$ d3 Y# a9 W6 U% u; t
- else if(HIGH_level_read_time >= 100)$ d% o5 S& m* H' Z7 Z/ ^$ E5 h
- {
. G4 a6 D. I2 t5 ^ - return 0xff;
- _- h4 Q; Q' k" O - }
( i4 ]9 C+ Y4 |9 {/ p( q- p# D* t
& V1 `) L$ P( K7 b1 U- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)3 V( e' f$ b% ^; B1 b! x n
- {( Z" B' `) E* I1 T" ~$ |& r6 ~ h1 l
- return 0xff;# T2 d: ?$ M& m
- }
, p' m, m1 Y% u8 b) T- f: @ - }
9 | f& y- v% R6 b+ ` - }
8 l$ n8 C5 t4 t# e* F* c+ c4 a
/ {1 @7 l6 V* q6 }- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level- Z2 i5 Z X$ G2 A
- HIGH_level_read_time = micros();6 K- l2 I4 H. M7 J: P) L3 L
- LOW_level_read_time = micros() - LOW_level_read_time;$ L7 B+ Y( F8 D2 M
& ~( P# p$ t2 n, I. h+ O- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )( U4 z" m1 I* c1 J) B" x) Y
- {
' ]$ ^ R: j" p) ~! }6 O - return 0xff;
5 w% q+ d/ c0 ]$ _ - }
# l- _9 `3 k$ L0 j9 r
0 m9 a/ g9 Q8 G2 n+ v( o- pinMode(_DataPin, OUTPUT);0 J+ m/ K4 u. U6 e
- digitalWrite(_DataPin, HIGH);
& {; n4 _' P% u2 F) A+ q- g0 L3 @ - 8 t' e0 L& o+ F* v
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))" |' }; a: ~- m9 O: [; j) m
- {2 ^+ |. b2 U6 c' @: N, x/ R' j
- old_data = Sensor_Data[0];# o) B1 C+ }0 } x* r
- return Sensor_Data[0];
$ y/ G/ J; |$ Z! d - }0 v) }$ m# R' v+ I2 ~8 r
- else7 `* P, k9 \" I3 B% n
- {
: @( W6 b0 n1 c - return old_data;& C i- P1 T4 o6 w- W; A \5 ^
- }
" J D% o$ p' M% w7 @& F - }
& p& [! N4 X4 K. d
複製代碼 8 u+ x+ Z: a3 t, |& N
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
+ H& }+ W) N" m/ t
: z$ e9 Y& T6 }. t5 H& H: { |
|