|
) E3 Y- j# k7 A, y2 T1 G/ Y' @+ U- #include <Arduino.h>7 F1 w6 X- f! |0 f' n5 w* A
- #include <MeAuriga.h>9 o, {; g2 O- C" q; R
- #include "MeLineFollowerArray.h"
3 N/ d/ ]2 k8 }) I4 P3 G" \% R
$ `7 W4 g0 \0 _- MeEncoderOnBoard Encoder_1(SLOT1);
" G7 f+ d( }! I, F8 k - MeEncoderOnBoard Encoder_2(SLOT2);3 }2 r' h7 Y& C
- MeLightSensor lightsensor_1(12);* k/ g2 I( T! p+ e! I
- MeLightSensor lightsensor_2(11);5 ~1 x$ g8 D1 M* E; `
- MeBuzzer buzzer;$ S X" K( O3 a1 i1 X
- MeLineFollowerArray linefollower(PORT_6);. k% I! Y; W9 }9 c5 }9 _
- ! N/ E$ N# ?7 g, a8 _5 q
- #define Error1 1$ @( `. z; T& o! q+ d+ T6 }$ I$ j/ U
- #define Error2 2; t0 L) w+ @* A
- #define Error3 3
! m" ?' B' N) L
: i4 T9 Z* u( ?- #define Kp 15& d. {' }5 u& h- G9 q+ C+ K C0 D
- #define Ki 0.15! q6 a8 E$ g" s% n8 q& P# G' n# c
- #define Kd 0.03$ ^! }/ W2 }* `; J7 I- s# z
- ) f& u6 X! ?9 c
- uint8_t sData;
# [3 u* k" J4 z' H; d" k3 g - uint8_t D1;
$ k) }: @& C+ W; R' w# m5 D' v& ?# V - uint8_t D2;
4 {, k5 K2 H: k+ J& v - uint8_t D3;
! S$ i' w l3 { ?% \' e( y* | - uint8_t D4;
{7 l$ b: }- X4 M+ @; R - uint8_t D5;
+ \7 T% j* q+ y5 z% f% V) B - uint8_t D6;* P. X0 w2 q7 B- z' c! a$ `
- 4 r& u7 a$ C) X2 p! s$ ~
- float previous_error = 0;
( I7 z- S% j3 A& c. F3 T - float integral = 0;; z6 H+ @% {6 N' E' P+ _
- float derivative = 0;2 L+ Q8 U" ~1 H% {( p# }
- int Speed = 160;
/ o0 X: `2 t5 b; u8 V3 u( ? - float output; T+ g/ d/ \8 a T! N1 |* T
9 P+ @1 b4 W! w9 }7 h9 }- byte Left;5 W+ v: U: U" |- h5 g
- 2 W i" \1 [0 J- w. E+ |% N
- void setup() 5 p/ z+ F# P* z$ y3 b
- {' E) ~3 m1 I, I+ Y* K; y
- //Set PWM 8KHz
9 h4 O S e/ ^ - TCCR1A = _BV(WGM10);' T2 I/ |( w1 l! n# s
- TCCR1B = _BV(CS11) | _BV(WGM12);
! g( T6 D& t% j - TCCR2A = _BV(WGM21) | _BV(WGM20);
; u1 @/ y: i I! N2 a - TCCR2B = _BV(CS21);
) J- g& t- F; v2 d - Serial.begin(9600);2 Y; p/ A9 D# o" j3 a+ r9 D
- buzzer.setpin(45);. l; Y9 x. z: p& W5 D6 A
- }7 v# d+ \) P2 s( M, |' D/ r }
' n) a* T' T* D( D- void loop()/ a& g8 R" b# R6 R
- {
( v! w# M8 { m& Q; i - while(!((lightsensor_1.read()) < (10)));
" W6 C% i% i8 M" s# B - buzzer.tone(1047, 500);% S) s9 Z! N/ t5 u
- delay(500);- S% _" {. b$ ]9 ^5 o# B5 G
- do5 {2 a0 z$ ?4 L+ D/ L
- {
4 R4 {/ A' X) G5 o* o9 j& C) x B* y - int Err = getErr();7 c/ r5 ~2 R/ p
- if(D1 == 1)
2 D3 ]" n8 i3 x4 G. U* O: L - {' q; g9 j1 L2 \6 U
- Left = 1;
: `1 Q) |+ M+ [8 z# \% q - }
! q8 \' q* f( q& }+ t5 t - if(D6 == 1)
" S4 s! C Z; a5 o( b" U' ]5 X' { - {
( H3 a7 a1 Q/ k6 a) E& H, Z5 X - Left = 0;1 ^& D7 i: }8 g5 ]
- }
' L/ R& T" @& `' S9 g - if(Err == 99)8 `0 I* R+ e& y2 K* }
- {
' q! \+ L8 e: o( l; i) j6 O, o - if(Left == 1)
# e9 J7 }5 q) g* P0 w! F ] - {' g- f$ d9 B4 J) O- O2 F2 S
- Speed -= 5;
6 f$ b7 {: p2 Z' M - moto(0,Speed);4 _7 b4 C$ g8 B1 f: O0 S( e
- do& K' W" m8 K2 o
- {
4 g* k2 @* V' N2 ] - Err = getErr();
+ b* {% V" o( o, A( J" |' N: |! ^* r - }while((D1+D6) == 0);! c0 [: E7 x/ H' ], u/ G1 Z
- }
" s e7 c, ?; R/ Y* C3 S# c* T - else5 Q2 l% g, \# Y, A: G6 g5 {
- {
k: ~- {% K, B. H( a0 r* [ - Speed -= 5;
, R( I9 L, i3 G) l - moto(Speed,0);
$ u/ b. z- p+ x4 c+ o) T" c. u$ O% p - do
& V5 U5 Q- D/ G) T1 Q - {
3 f' M. }) u' s9 S+ I& T - Err = getErr();" b' [1 q" h1 U- ]
- }while((D1+D6) == 0);
$ Q" k! r% f; E; S5 U - }
3 v# \. v8 p& _. b) V - }
8 r2 ~# T% q% z$ \ V4 o$ j - else
6 r' L2 C* q& s8 ~- \4 V - {
; X# g4 {" B; a [$ r - if((Speed < 160) & (Err < 2)) Speed+=1;
" _& S$ r: ^: n4 g+ I; C - if((Speed > 100) & (Err > 2)) Speed-=2;
r: ]$ w0 N' X" G - integral = integral + Err;
$ y: Z1 L" W& R& ~8 i& S$ T - derivative = Err - previous_error;
h1 u1 D% o4 Q# J5 S1 y4 r - output = Kp*Err + Ki*integral + Kd*derivative;) f. C; F- Y k2 i; u
- moto(int(Speed-output),int(Speed+output));
( R2 N( {' m/ z9 a# f- V* R - previous_error = Err;8 c, X- w' E5 u4 Q; Z5 c$ {
- }
M1 \+ ]/ j9 S - }while(!((lightsensor_2.read()) < (10)));5 [: Q0 ~( t) e4 O5 W7 i* e" s
- moto(0,0);
" ]' G3 H3 [8 O. I - delay(500);
; [* z/ s6 i% o' S - buzzer.tone(262, 500);
( W, |5 r$ F4 E y# j. d - }
" v# R, _* T' K" f6 | - 6 V3 z5 R, Q h+ A
- int getErr()7 u3 }# s' g- [ c9 n
- {
6 h; X, @. l, G$ d. h; ^+ ` ` - sData = linefollower.getValue();
! N6 q1 ]$ G2 f9 |' W) A% l - D1 = ~(sData>>0)&1;% j0 s9 V# A' W$ n! l% c& b
- D2 = ~(sData>>1)&1;/ ?0 `& R0 q& E( E' }; l! I% a
- D3 = ~(sData>>2)&1;
1 M. E6 O, I% M3 v5 w - D4 = ~(sData>>3)&1;, g1 {% I6 N. @' J( v, ^
- D5 = ~(sData>>4)&1;. B# T4 G/ @4 r9 v" l8 {. Q! y
- D6 = ~(sData>>5)&1;* [5 S' r, \/ ]& g, H5 ~( ^
- int downD = D1+D2+D3+D4+D5+D6;% _3 d t V+ W- n$ u
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);1 r- \* Q+ G: y8 }0 c, F5 L$ G
- if(downD == 0): M2 s$ j; j" j$ K7 V$ i
- {9 a1 [+ J2 t7 W: S" g0 g
- return 99;: Y% M: } a* `( D& s5 E4 K; H
- }, J2 ]% C3 c2 w4 z
- else1 L3 k. a/ F( H+ ~' `! s! P5 y
- {
6 ]: z& J3 J {4 x - return upD/downD;, k- g }2 p5 W( Z0 n
- }
# y/ F0 j$ k3 i5 ~% E7 t - }
# V, _. V4 y) X6 D/ d; ^& n
' g6 k: \ c+ g1 Z. [* D- void moto(int Speed_L,int Speed_R)
) `$ g* Q" X7 k A/ r! [2 Q - {
" K" l+ j% E2 J) z4 `: e( b - Encoder_2.setMotorPwm(Speed_L);
) z7 ^% A# R5 A- U6 d0 U" |" N - Encoder_1.setMotorPwm(-Speed_R);
$ Y/ Q% g% X; l6 i3 V - }
複製代碼 . T# B" B* y- |
MeLineFollowerArray.cpp5 d. u( @! Y* Z) {1 s$ Z/ ?
- #include "MeLineFollowerArray.h"5 p) k C+ |) P) h; {5 w
- ( k% @2 ?2 p6 V; [- h
- #ifdef ME_PORT_DEFINED
5 [0 u% ~0 N! w - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)4 G R+ A+ X, m% K, M
- {. k4 O+ X/ D/ }+ [( _' I
( W7 @: [) q9 B7 p3 ^- }2 l' @ c- s* f Q" a" h
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
7 A- }( W; y7 U - {5 _, L0 L; \; ?2 i
- _DataPin = mePort[port].s2;
$ }# }$ e% p1 x- G6 b0 V - pinMode(_DataPin, OUTPUT);
" T$ P5 N2 z9 n6 q - digitalWrite(_DataPin, HIGH);7 I/ H4 `% E; Q m: |
- }
" t$ b. U# j2 N/ m7 N' p; w - #else // ME_PORT_DEFINED
8 f- d. {- C# B - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)) o2 n0 l$ ?+ ~
- {
) ^: t% M& q$ [) p0 t o0 u - _DataPin = pin;0 p4 q) l- B& I# {
- pinMode(_DataPin, OUTPUT); & ^% y) k# a1 l1 n
- digitalWrite(_DataPin, HIGH);
" v, O. P' r4 {7 A& F - }" e$ G5 c. o7 K
- #endif // ME_PORT_DEFINED: U( C/ E2 q; q4 A9 |
, k* z Q, _2 H2 Z$ ^0 ?' o" J3 R( ~. J
% f/ p* t& r5 V& n$ i8 M% ^- void MeLineFollowerArray::setpin(uint8_t pin)
1 ?* I, T, d& H1 t - {
1 F. _7 u+ D1 a0 r - _DataPin = pin;5 S: D$ V1 b1 z7 v. |
- pinMode(_DataPin, OUTPUT); 3 z* j" q# g% a: ~& k
- digitalWrite(_DataPin, HIGH);
+ u" q3 k2 H* ] - 9 w8 a Z4 H7 A
- #ifdef ME_PORT_DEFINED* e& [5 L; c& V( z% v# d: ]. [
- s2 = pin;" I: C" W- w% a5 e0 g6 G
- #endif8 k: J9 y$ G1 V i
- }
6 h# Q' t7 q" n# X; q8 w - 8 ^: O- ~0 M; A6 a% L% ^6 ?
- uint8_t MeLineFollowerArray::getValue()1 ?5 ]/ p0 \ ?3 u( i7 _
- {
- b' R2 E1 r4 m( F - uint32_t LOW_level_read_time;
" L$ p0 ?( y7 u- p8 C2 r - uint32_t HIGH_level_read_time;
4 C6 C: m2 i( K( \ - uint32_t time_out_flag;
6 `% a' n6 V0 _4 w' g9 u3 y5 t1 v - uint8_t Sensor_Data[3];
n6 n0 A0 e7 z: F8 ` - static uint8_t old_data = 0xff;
( b' p+ @, I% [8 K, ^ - 0 |: F: _" R- Z6 v/ ~# g8 X( [
- pinMode(_DataPin, OUTPUT);
/ R2 G4 e/ _& h, S - digitalWrite(_DataPin, LOW);- _: `; g/ L# B) r; }" R
- delayMicroseconds(980);: L7 I1 X% x$ R% l% ^* T4 j
- digitalWrite(_DataPin, HIGH);% d; [, V1 }% w; f
- 6 X$ ~/ K6 n$ @( [" M) f
- pinMode(_DataPin, INPUT_PULLUP);
0 u# Z/ i( `; `1 m7 X. X - delayMicroseconds(10);
7 q. B m6 ]- E9 r' _ Z& V- U1 c - . [6 N, v3 x% P5 E* @
- time_out_flag = millis(); |6 ?* r& s* ~" K1 ~
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );7 R' Q) ^1 M' e7 ^: T
2 F: l' ]3 ?* U" n. A: X- LOW_level_read_time = micros();
2 j# i* ^1 I- q5 |& o, Y - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
4 ]3 k! ?# g$ H4 q5 L6 I - {, E, i6 u$ r. l" Z9 B
- return 0xff;
+ L9 E3 q; d [. ` m S - }
1 p& x. G- u; l$ N' L, W- X - # ^1 z5 X) i8 ]7 t3 q
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 C( w5 E# n; n, g# V: ?3 x - 1 `( P u1 l9 d" x
- HIGH_level_read_time = micros();
, G2 C. [$ m' }; U- G+ O - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
! B& {0 z4 O4 f - $ k' m- r+ ^, g; M" c% Q1 E
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 p4 ]) \: K5 ?& q - {
`/ J5 ^$ J8 w( i, P- r - return 0xff;
1 t: v' Q! j o6 }% d - }
% h0 ~( T' A* t9 e( R* V% e
% `( c% h- B( u+ |- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))/ q) K2 I: Y( |4 G
- {
/ Y/ T6 n6 R/ s7 U2 N# ? z - return 0xff;/ B) @3 x5 U6 `# T, ?# |+ F; {- i
- }4 o8 S- g6 A' \
! \9 {$ j" \# F7 b7 M" w- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );/ g5 z2 n8 Z8 X# E
- LOW_level_read_time = micros();
* K9 Q/ i; q; E$ [% [- e) Q - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
4 o A, k* v+ e
1 |: x! ]3 H0 H7 Z6 _- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out, q. z1 L9 A$ y
- {1 |9 @' y) v- b" r3 ?6 V' |
- return 0xff;
; ]1 ?5 A) c- l, _ - }
) M. W' A1 G$ c4 n" {' M
Q0 F; j! A8 o5 B4 C: m2 w0 R2 L3 h- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))0 z/ j9 u" _! f! O5 H" d
- {; z& D9 c" w; `1 z/ c% V
- return 0xff;# @2 L9 ^: R$ r5 J# b
- }
9 _; q2 A# J! P! A# S% v
8 B: I# O: d# f& {: b3 e: B% C2 _- for(uint8_t k=0; k<3; k++)
/ U: K% S+ _9 w2 q" b8 x - {$ D+ A+ I( C6 x- @! `
- Sensor_Data[k] = 0x00;; v/ n/ V2 {6 N: l% y: H
6 V' T+ o9 i+ E7 v% S' T% b- for(uint8_t i=0;i<8;i++)5 e1 E0 u4 h3 h$ C( e
- {
$ m" V5 t7 U6 E; y: C8 H& _9 Q - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level5 ]: W1 m2 O& I8 J* z
- HIGH_level_read_time = micros();
1 A2 R3 I/ I* W6 s, h K/ e& y - LOW_level_read_time = micros() - LOW_level_read_time;: Y% R5 `7 d0 T
/ A( M9 b( T- ^5 Z- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
! \; y) {; N9 W$ K - {1 t/ v' M! Y4 M( Y
- return 0xff;
' Z6 f) h; y0 I# K - }/ R- Z6 P" A: [5 r
- ( ^ [! J3 q3 u1 t( O7 A8 c. {
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* T3 ?0 f( @# d4 L9 _
- LOW_level_read_time = micros();
7 c7 x; C$ l3 P - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level: W2 [" g( W3 d
* ?1 C: K; l; y/ q+ Z- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
5 c7 M5 u# t2 O+ ` K F6 e - {" g3 I) Q+ a" q) V5 m8 o
- Sensor_Data[k] |= (0x80 >> i);/ c7 j1 z3 h0 y- }) e$ q: ?2 o& l7 f
- }
) _+ @5 Z, n' q( s - else if(HIGH_level_read_time >= 100)1 _6 m1 f2 ]! K: c2 W
- {
% O2 |( `; V C8 D5 \6 q - return 0xff;
) F4 S5 w, q D C2 l3 { - }+ h7 d! l+ J+ w* {" w
- % `4 o' H' t! n: k! D& ]: x& u
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
6 h- }9 D. g; j. \ - {
, }1 E" }/ w. P/ U( S - return 0xff;
& S8 Q2 Q9 T! x" e3 I H- h - }
6 Z( Q8 }5 a3 X9 x [ - }5 l! S! D$ r5 a, V3 W5 B' A
- }/ D0 x/ u# i" G9 K
9 x9 H' f- {6 x; e6 n- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
/ d4 w+ Y* F9 a& T1 ]/ V - HIGH_level_read_time = micros();
5 |0 ^4 @ t6 r: m; E7 {* w - LOW_level_read_time = micros() - LOW_level_read_time;
# Q/ Q; F: C, H& x; ? - : T2 }7 O6 O; B/ j4 ^
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
) S3 ` d: a u: C - {/ U( ^1 d+ C. u, X4 q& X
- return 0xff;
2 [0 e. r( m9 b - }0 x( c0 B1 m$ i3 l& v# ~6 a2 F( p: k
; {5 _" w) l" J" V% X7 B) Q- ?. y- pinMode(_DataPin, OUTPUT);
0 G+ o2 V9 @. U9 o2 _# v - digitalWrite(_DataPin, HIGH);
0 d# ?" F+ l# `2 ^3 L' {) G& X+ Z - 6 M4 e" E( ]4 D3 p# E+ v
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
3 r8 r/ o h+ B% H' z# f - {
# C* _( f7 e2 g8 _' ? - old_data = Sensor_Data[0];
z' |, k3 w9 v1 k" i4 R9 |# ?8 E/ J! X - return Sensor_Data[0];
$ N! I" U! n6 i: O$ h. | - }8 U4 Q, U4 j7 t5 u
- else
) X! b. U0 @) H5 k$ e8 _3 _ - {
: u; `: ~/ p+ V( A - return old_data;
* c g0 _+ r! ~0 B) `$ e - } _! _0 T2 F: i3 v3 w
- }8 ^7 G8 W4 }$ Q5 |; u; Q. N& t
複製代碼
1 h1 x- g2 u' ^, O- ?6 mMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
7 ~1 k3 b6 A8 l0 s3 M5 p* ], n, a: x9 D
|
|