|
|
( ~6 x( M# b0 L/ Q- #include <Arduino.h>' c4 F4 e/ o* A
- #include <MeAuriga.h>- c' M. N8 S! s0 M' q) L" k
- #include "MeLineFollowerArray.h"
/ z; A8 J* _! @' w. v2 V - $ \1 b3 D4 L) t; j# k# s
- MeEncoderOnBoard Encoder_1(SLOT1);# Q- `1 V1 d3 @0 w
- MeEncoderOnBoard Encoder_2(SLOT2);
3 [$ S& J8 f+ a- i. v2 i' ` - MeLightSensor lightsensor_1(12);
) g5 E5 y. q$ u9 s7 L - MeLightSensor lightsensor_2(11);
2 F3 D/ ?$ b& Q - MeBuzzer buzzer;* q2 E- P, A' G& Y# H& _4 S* R
- MeLineFollowerArray linefollower(PORT_6);
+ y0 H# e6 _- u. j9 V4 D$ L- s
$ x: y7 N4 v7 t" k" s. i/ G- #define Error1 1. \4 R0 _2 B# \* V8 O! u% e
- #define Error2 2
T* U9 j; v# I# j- a [. Y - #define Error3 3
; \9 B k8 y+ B5 H: G7 k! m: x7 G7 |
: f% `6 i+ X; q) j( p- #define Kp 15
9 X I, p, t: m, U$ X' s/ F% o K5 T - #define Ki 0.15. X* p `( H4 P
- #define Kd 0.03/ W+ d. i9 f' D3 H. a4 ^+ n
- + J# V2 _; i. E6 w- d4 m( g
- uint8_t sData;! x# @9 Y" l1 X+ \! O
- uint8_t D1; n6 w2 A8 e/ e
- uint8_t D2;
: [, k5 O$ {0 w& @ `2 U - uint8_t D3;
4 q& C! b9 M4 i4 I& J - uint8_t D4;/ z. ]5 h( t5 E- p7 Z
- uint8_t D5;6 G2 E+ B# Q0 P K
- uint8_t D6;1 O6 I* }+ t" m
- # t6 W. U0 u! ?) V; E7 \
- float previous_error = 0; _& t, f ~. ~7 a
- float integral = 0;
A; Y0 I8 Q% F1 f% k' w' x* g - float derivative = 0;& Q7 p( C f1 f, H; R
- int Speed = 160;
B# h# Z; S3 N- [! ?+ T - float output;
8 E- x5 m$ F- n - w. k) x; o. i$ c3 U* ~
- byte Left;
4 r, e, V7 `( a$ {5 K( {! x - 5 `# C& e" b, r! s$ c$ D) y
- void setup() 2 O- P- T6 Y4 q+ |9 R+ J- Q! @
- {5 H8 U( ~" s5 p9 }+ u: i7 X3 F9 B2 x
- //Set PWM 8KHz: R% x9 W2 i8 C' S7 Z
- TCCR1A = _BV(WGM10);/ ]' C! B/ F* z
- TCCR1B = _BV(CS11) | _BV(WGM12);
1 Z2 D1 _2 v# J - TCCR2A = _BV(WGM21) | _BV(WGM20);3 x3 ~/ @* ?! B1 Y
- TCCR2B = _BV(CS21);/ j- }& J5 ` |7 L
- Serial.begin(9600);
' v- T* `# e/ s& R7 j' p. L8 V7 Z - buzzer.setpin(45);6 s3 {& N% F9 U- ^; _
- }
4 P" K) a) u2 G; W" b" O& S' s# r - - ?' |: m7 C8 q0 U1 _
- void loop()' {) s; b+ t% G$ H
- {) e* D2 Z0 o* m4 W+ s; n
- while(!((lightsensor_1.read()) < (10)));
7 c. `' E9 O4 F6 j3 \$ x' O - buzzer.tone(1047, 500);; N d1 W* J2 T% C) \1 E' F
- delay(500);
# R6 i: |- l; {1 i+ m4 V - do4 x+ a+ i& B& r3 E' U1 h% M
- {1 P1 r: E- I0 e% w* D4 I E
- int Err = getErr();
1 U: d" n$ E! }# T( c - if(D1 == 1)
; b }/ r1 R6 [3 U; a5 h$ c0 f - {
; m5 P' z/ s5 p& ^' W4 e' ~0 l - Left = 1;
" K% ^3 D' W" U, `/ o( V! z - }
1 z/ j9 s; _1 b/ T5 _$ Q - if(D6 == 1)/ J# Y4 g4 u( G. r
- {
8 ?6 S- M0 {1 H - Left = 0;, ^( y8 F n( c# \- `
- }. R8 T d8 C" I( G2 ^
- if(Err == 99)
5 @$ ^5 f8 G# ~! H. t* ] - {
7 n; D2 i6 y6 H2 l - if(Left == 1)/ E' ?5 L5 I; \5 Z; n3 t; x
- {
* ?$ E: j. H [) M - Speed -= 5;' B' F) t3 E4 Q4 q1 K
- moto(0,Speed);$ l8 L9 F2 _0 ^- ?/ q C7 S
- do' r6 d" l$ o& C" k6 A
- {5 Q' r, p8 i, `9 P
- Err = getErr();
& y2 ?7 A. z! n2 m* f- r8 G - }while((D1+D6) == 0);2 o& L0 v. j! M) N/ z
- }1 b( O1 _: d0 I1 Q+ ?$ }
- else! R- @, G; l# L3 {" ^. G7 Y; Z
- {
, o1 E) \' a" s' F2 `% {9 X) ~/ X& O - Speed -= 5;! C( Z0 o) D q' Y! L( }
- moto(Speed,0); 3 _) c6 b( n5 _4 p: r
- do
* V9 A: d9 Z2 B' I - {+ I2 E1 O% S% N1 ` C
- Err = getErr();
a/ K+ N3 I9 k' U - }while((D1+D6) == 0);
! C3 o) ^9 ~+ q. O - }5 b' Y' Q% `* {2 e, ]# ^
- }9 G9 f2 E. ~) S! A
- else' t" h. h9 f: v# ^0 g
- {
5 D' p1 F1 [0 u* q1 Y- D. F. J - if((Speed < 160) & (Err < 2)) Speed+=1;
& f D, S$ _& D - if((Speed > 100) & (Err > 2)) Speed-=2;
" O# m5 b/ J {8 X1 h8 p+ W$ h, ~ Y - integral = integral + Err;; S% _+ O$ T0 |
- derivative = Err - previous_error;
: }* E1 N+ U; X8 A" W& b* B" z7 { - output = Kp*Err + Ki*integral + Kd*derivative;7 C+ h: c' e* }# v
- moto(int(Speed-output),int(Speed+output));* z' Y1 M. r: ~/ \* S+ T
- previous_error = Err;
" U3 R0 `. M7 \5 x - }
, ^ w9 ^$ m" ^0 e- } - }while(!((lightsensor_2.read()) < (10)));
2 N0 f6 G- }/ D5 t- j7 P$ K - moto(0,0);
1 H1 }3 [) ]7 }4 Q - delay(500);* u) p5 i) H! M P+ a
- buzzer.tone(262, 500);* r6 U& a7 d8 R6 M8 Y
- }! n, v% U$ K, a, o% P9 j5 u1 l: u* }* |
- : [8 }6 B/ F! I6 _
- int getErr()& Y* h' {3 x, ^, l) Z Q
- { : N1 n) `, N) k+ J, w7 b
- sData = linefollower.getValue();# f7 t) J( y0 r0 W
- D1 = ~(sData>>0)&1;
6 e3 o7 f+ S7 _. `- o# i# z - D2 = ~(sData>>1)&1;
, V+ S% q" u, h8 ^6 F; F8 F3 k - D3 = ~(sData>>2)&1;/ b) {. o; g+ R% T" e
- D4 = ~(sData>>3)&1;: H0 D7 |+ |4 |$ }
- D5 = ~(sData>>4)&1;
. ]! u: H6 u6 {3 g - D6 = ~(sData>>5)&1;1 E1 g* ]1 U$ t/ ^. }2 h/ P- O
- int downD = D1+D2+D3+D4+D5+D6;
0 e( v0 }% g5 e. A b - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);/ P5 S9 Z6 B8 U4 `5 |" ]0 y3 p
- if(downD == 0)
# c9 L6 |7 w$ H' M* q8 o- K R - {
2 v2 Z( k5 U8 B& M - return 99;! [5 V; E. w/ _ a$ \
- }
0 A0 I# a4 ?; h B& e - else3 J3 Z) E L4 F t- S' e
- {& v8 ^. L* t, o' y6 b1 Z0 t
- return upD/downD;
& z& ~$ P- i3 O7 w3 g6 y3 { - }
6 W' A" e0 J6 x M7 B. Y$ F - }
3 _. g( L3 i( }& D3 e9 n
* N$ E$ l$ _1 h R- void moto(int Speed_L,int Speed_R)! w L: M& Z. \# h' ?
- {" p, d) `* N, o- b
- Encoder_2.setMotorPwm(Speed_L);
' R9 G" _% `$ @* n - Encoder_1.setMotorPwm(-Speed_R);
+ B+ @7 m: h* `' \2 I9 ^( ` C - }
複製代碼
9 C* j1 k8 D' k: y( E. sMeLineFollowerArray.cpp- F' q1 m& r; Z- Y* x0 h" }$ p7 E
- #include "MeLineFollowerArray.h"/ c' B5 W' T6 b8 C
- & L+ z# Q6 T" Y0 _; y) U& I
- #ifdef ME_PORT_DEFINED
4 w$ ^% g+ p) c# [0 O) \7 B - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
0 B( @" h( u# h7 m- C Y. D - {
2 L& R {' r# b& l0 v; O+ j - 7 J6 Z6 a R: U8 [/ X: K' `! k
- }
$ @+ {# L7 S/ l' y - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port), e. E: \. F- M: J. q7 N
- {; F% q1 _* H# k+ h1 b& w) E
- _DataPin = mePort[port].s2;
9 u7 a' |+ X# c8 K* N; O - pinMode(_DataPin, OUTPUT);
6 a- ?' N' w: b8 d' Z; O - digitalWrite(_DataPin, HIGH);8 @# p' S5 j x& u0 B# @
- }- g( ^9 u0 H% b( O4 R1 w
- #else // ME_PORT_DEFINED
% j5 j& z: g# z2 M1 a, x - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
v4 g# W* ?0 J8 L u0 k4 k - {
+ O9 V: o) W0 H. {; f* R3 D - _DataPin = pin;7 M( i8 |" x+ j* H# i. T% P
- pinMode(_DataPin, OUTPUT);
4 @( I6 y' x: m" l6 I N9 K3 P - digitalWrite(_DataPin, HIGH);5 z' k9 Z0 q! b. y" a' |" C
- }$ Q5 R$ J; t6 |) d3 R6 m* R
- #endif // ME_PORT_DEFINED
0 @% {+ d! W/ G) _ - " J* w' J2 p* }# {
- " \. t9 }& `4 @$ ?
- void MeLineFollowerArray::setpin(uint8_t pin)
2 i1 i4 z% J* e, R `4 A$ h - {$ S' s B7 v. ^8 l3 j) h6 z/ R7 |
- _DataPin = pin;
2 \0 j$ D+ Y# A$ F0 u: w( s - pinMode(_DataPin, OUTPUT); 7 N% ^% x, l8 n* k. H
- digitalWrite(_DataPin, HIGH);# X. c8 I% s, |6 N
- / n1 P9 m r9 K0 f) e! K3 K+ B
- #ifdef ME_PORT_DEFINED
- C5 }2 _2 w" e. A2 c. @0 t - s2 = pin;& L3 w( b8 Q' a% O' Y
- #endif
: M' k$ j' ] Y' U0 H - }
0 n. W# X+ k) c, F' J9 a2 U - $ {: { ^. P" V4 K4 |
- uint8_t MeLineFollowerArray::getValue()
8 x: ?/ N! k" P% k - {* e0 A @/ B6 z# ~5 x
- uint32_t LOW_level_read_time;
! Y, e0 Y* ]4 }# U - uint32_t HIGH_level_read_time;. M) l* o5 X! w# j5 [; H& Z1 Q
- uint32_t time_out_flag;
$ p/ w1 @' g6 G" u0 ?4 N7 y" T - uint8_t Sensor_Data[3];) ~4 Z5 ?: q* P6 C1 p
- static uint8_t old_data = 0xff;
, p& H* q. ~) s( ~. t - 2 ~0 b# V$ @9 {/ k
- pinMode(_DataPin, OUTPUT);% s' ]! K& m& k" K4 m0 k' M
- digitalWrite(_DataPin, LOW);. |5 K4 f2 M* C8 e) R+ H% ^# G
- delayMicroseconds(980);
$ [, }* X4 X) |+ K! r: e2 E2 o8 ]+ V/ p/ n - digitalWrite(_DataPin, HIGH);
) _2 m# [! T8 H4 p% F" G# S- T - ) g# u: P( Y! w; @( {0 s5 {( E1 w
- pinMode(_DataPin, INPUT_PULLUP);2 W. {# r! a/ C7 {, {
- delayMicroseconds(10);
, O+ @( U0 q s3 j; o; Q& L - / I2 b3 B' \+ j1 W. W* E `
- time_out_flag = millis();
1 v% B( r+ M1 I% Q' o/ z - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );% N! k! A% }' R2 v0 k M% a9 P. _
- % U2 `: H' Y5 l
- LOW_level_read_time = micros();
- l' r/ V- P g$ C7 w& U - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out; m3 V6 n+ b' _ }' Z' s* s: Q
- { S0 C3 G& p8 } ~6 y4 S
- return 0xff;
4 Z4 f8 q, s' s1 J& b - }# b7 A3 d2 g; [" P
- # Y# r. k! q" z
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, p( X/ e. z9 o6 F/ F
- " ?& l- a/ c3 t' R
- HIGH_level_read_time = micros();
. G: P: {5 |8 ], l7 h - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level5 Q, s1 M f( l0 k, t
; j+ g u p/ G; w/ `' v- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 S+ L( D* P. U5 z* ]( B - {
7 J; S6 h7 I0 k$ }" r+ l - return 0xff;
5 m8 y! D8 U" `) d - }
# b& U+ K0 O+ q& J9 o% K$ C( } - ( V/ o, ]" Q/ s+ w
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))+ u, z+ U: O4 d( J8 S$ J
- {
! e# D: N3 o( O' o- J2 Z - return 0xff;
; F; u+ A8 K/ L - }
( |1 r9 o. ?% y2 N8 F - ! t0 Q0 w/ C E2 V5 a
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );% ~) A6 R% D7 D5 b- K% ^5 \
- LOW_level_read_time = micros();6 {! a6 U( U4 D( w
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
! z5 X+ z. [0 g, f - # P2 O5 x. L3 O2 L
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out T* U' p% v% X* l0 U7 I1 n
- {
; N" }+ U# {3 B7 w2 A( e9 H - return 0xff;
$ [$ l0 F# y% T6 h - }
" U9 s7 L. _ f - ]9 Y' @! h; E% Y& q
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
6 R& d2 B( y) _% F" Y - {
9 i7 x- S) `1 ^3 X - return 0xff;0 @+ T/ o) r" z. y) P
- }
- X% N3 J) l! f L; `3 U
2 F* l* A# {3 T, i6 {2 v5 F( f- for(uint8_t k=0; k<3; k++); T5 L3 G& x1 h6 _/ r. {# ? G
- {6 E6 [$ {8 ^( x6 P/ ]* L2 ^) |. K
- Sensor_Data[k] = 0x00;
9 W# k/ y! n/ l z$ d0 |. e: g - 0 S" ?' A: j6 C8 ^6 ]) K9 k* \* W( N
- for(uint8_t i=0;i<8;i++)
7 ]; H* i! w8 f! g$ C8 w# N - {
" [) N1 _2 S3 E3 O7 t! Z - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
8 n) L9 d0 A6 F: X7 \/ G. z# o" U - HIGH_level_read_time = micros();2 F- d# N6 y: ^6 Z5 P
- LOW_level_read_time = micros() - LOW_level_read_time;
! {- B K) A" Y7 U& f
( p* Q) A5 @8 w- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
) ]* S1 \: L3 g$ E9 U) N' n% g0 { - {6 E4 ^* i4 Q: g& T
- return 0xff;
0 M9 l6 W% T% f, A" L# ?) g+ A - }
5 ?# _2 o' R& E6 c2 w8 ` - ! n9 u: N' Z8 T9 P0 u' t7 v: o$ X9 }
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );! Z+ m! |' J! m* P3 {$ F
- LOW_level_read_time = micros();" ^& \; u( I5 d) h1 h
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
4 h! X. I% X; K8 t - * O% _1 j I3 o9 ~5 V7 ~
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
! a) z9 E- s: c! k. \ - {
5 f& ] Y: D+ B3 x ~ - Sensor_Data[k] |= (0x80 >> i);! R) U' _4 v ?% Y4 `
- }
4 d( O) U( A" b0 p# N$ ? - else if(HIGH_level_read_time >= 100)* d( D) g+ @" C. t
- {
% M8 r5 ^2 l- B0 v - return 0xff; f B( x! N, \# u. O- C5 S
- }+ I2 g9 {5 ]* }, E0 ]$ T
- 7 l( x; u/ E- n d
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
+ q6 R( _: X. B# Z# } - {- u- k' s- _0 @0 u4 N# Z* a
- return 0xff;( \4 ]5 g: [9 a, d' s4 m: _" f: j
- }
: ^' d; Z" N4 b! Y1 I - }+ x; \2 A \6 u. R
- }- {* I# R/ Y1 i: O& v
- ; V7 ^" b5 ?7 r' H7 ?. @
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
) G3 q! Y& Z% P3 F6 H - HIGH_level_read_time = micros();9 Q8 k0 D* ?: y
- LOW_level_read_time = micros() - LOW_level_read_time;: V" F# o) I# u D- {, H2 f
- 0 t8 d8 O1 b2 Z1 E5 g/ k
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
: O4 k; x7 B& J2 v; b' X( [ - {
: _* e& O* c5 G' q0 N - return 0xff;
- j0 _: c4 ]( B; z* T& S' u - }2 [6 H: K+ Q9 m8 X7 v9 ?/ L; H
- - |# j+ F) `. W$ {. q3 U
- pinMode(_DataPin, OUTPUT);
' d" C6 C+ t/ ~& q# P - digitalWrite(_DataPin, HIGH);6 k. Z9 d# S5 B4 W- H! d- d6 w9 M
- 7 q% a# v( H% T6 a( T
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
+ W4 ^ E( W, w" g5 M! i - {
: O- I, c( q9 E/ V; B - old_data = Sensor_Data[0];6 f* N/ @, K3 G1 q- f6 C b* Y |
- return Sensor_Data[0];- F0 F2 [& k$ B M" U; O' I' @5 m
- }' G6 b9 l A# H2 `6 k% E; A* |
- else
$ l: N3 l/ i7 u/ h G2 O- A - {
7 O! n6 M9 ]7 Q g - return old_data;
2 D# K0 q2 a) i+ ~# _ u# o - }
- l7 d4 V2 ^ ~ - }. H% b- V, o1 ?3 w& Y
複製代碼
+ M0 g) b* u2 T! g' b: ]: E' D& [MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
8 M* e3 d' i' s& c2 W
3 g, N' P0 p# @* ~ |
|