|
|
" ~, C4 B& K! k* n% O6 f/ g
- #include <Arduino.h>
: p) b& r! u K2 u; z/ C - #include <MeAuriga.h>$ d8 d! D1 K# ~: u t$ i
- #include "MeLineFollowerArray.h"! }! _0 p% c5 Z$ _5 Z5 l$ I4 v
; g4 }2 F! A; P( a* [* [4 j0 U+ B- MeEncoderOnBoard Encoder_1(SLOT1);
" A- ~! s \6 t) y/ m9 U. J- U - MeEncoderOnBoard Encoder_2(SLOT2);% l ~. g5 B$ r+ ~; f- W. @
- MeLightSensor lightsensor_1(12);
' j9 K4 B+ F o1 u! Q - MeLightSensor lightsensor_2(11);+ K3 i# m; ?- T1 G2 G7 c
- MeBuzzer buzzer;9 U; w8 c, A( G% M4 H. @( f( H
- MeLineFollowerArray linefollower(PORT_6);* ~9 n y o% Y ?- V0 |
+ l* c! a" N, E- #define Error1 1
$ q2 D- j' E J; G! ]6 K1 i# }$ M: p - #define Error2 2$ L& f. N/ C, }0 ~3 B
- #define Error3 3
, w$ u/ T1 X8 W% e/ D/ s7 _
H7 c8 Y. T. |% [' w- #define Kp 15
) i% A5 U9 Z1 `" K# i7 p$ A* z - #define Ki 0.15
$ W$ D6 Y3 \( [5 Q7 w - #define Kd 0.03
9 @3 l+ i4 e2 n) {$ Y" I- C3 k: _$ T - 2 l! M) }3 n7 Y6 b) D
- uint8_t sData;& U( D% e1 `( v4 t& k; a" ~# D
- uint8_t D1;" N$ D. c: U$ k( Y4 Q) L3 D
- uint8_t D2;
/ ]4 F1 |; I% D( L5 W c7 y. R; O0 b - uint8_t D3;' o8 D( p: g1 _1 p' q( ?7 Y
- uint8_t D4;2 j/ U. `2 [. ^0 w6 `" o1 F; @/ n& o% M
- uint8_t D5;
: F4 I, X6 B$ G4 ]6 |) o: {: P - uint8_t D6;
, c) B/ g( V- z, g% u
7 A4 w; Y; M) N/ q# o* y/ [% }- float previous_error = 0;% {5 z) [ ]; {9 M: ^3 B: N, B8 g
- float integral = 0;
3 j, l! e i9 s$ b; f1 @; e% R! ] - float derivative = 0;
: G% k* a3 P0 d6 p - int Speed = 160;- B3 I6 G6 N& x( f$ k) q$ ?6 W
- float output;
: e1 _2 h+ x P/ v: U
6 n) _( Q" r' \; T6 ]. b) z- byte Left;& {2 z0 R$ L% L6 R ^
- 1 R! P9 [+ b- F9 x
- void setup() ) n' ? G G Y( q" E
- {) T, w4 V1 o8 M9 T
- //Set PWM 8KHz& q+ d; z( B9 K% w2 X) J
- TCCR1A = _BV(WGM10);( h4 M. q) P5 J
- TCCR1B = _BV(CS11) | _BV(WGM12);7 N; m k- g: u. f' W# U
- TCCR2A = _BV(WGM21) | _BV(WGM20);
F. m/ f* |- h - TCCR2B = _BV(CS21);
0 Z \6 B3 n. ]3 B: m0 s - Serial.begin(9600);
1 H( X. b: |' d, n# j; `! N8 k2 m - buzzer.setpin(45);
7 F8 t! ?$ K2 ]6 A: l' Q. {3 u - }
7 Z- y7 P" a; X/ E
7 @7 ?0 u, V9 g3 K/ W- void loop()
3 ]* S3 B0 O- c! o/ f - {% B$ ]& Q1 w* |6 c1 O
- while(!((lightsensor_1.read()) < (10)));
. Y+ W6 ~1 _8 G4 h - buzzer.tone(1047, 500);
6 ]" x; G; D4 E( e) F - delay(500);
6 M1 q2 b3 Z7 a6 g - do
p9 t; j3 l* D2 m - {
2 }- w5 r! ^1 _4 h" t( x - int Err = getErr();; g2 Y5 w- s& Q/ U1 \* a# a
- if(D1 == 1)) h4 g0 x9 m" j1 L! o$ `; U
- {
% |8 H5 ?& T, D$ H L% | - Left = 1;
8 m$ [; M6 k* f! V# x2 ^ - }- v. v6 Q% K8 ]$ `6 k( K8 _5 ~
- if(D6 == 1)
) x T" n5 U4 e# Y- t; ?0 g - {3 ?. l$ f& Z4 n1 X! U) j
- Left = 0;
/ u. _$ d& u7 ~" _8 R - }
7 x- Z% P2 E2 c- z- h - if(Err == 99): v; a% b1 B2 Z! t4 W6 |
- {4 M* h3 L8 v9 M
- if(Left == 1)
# Y+ x" |+ N8 F' Y - {
/ e; P% t' x! R9 a$ q E - Speed -= 5;
" q( y% f" ~& U$ m# @+ R7 d - moto(0,Speed);4 D$ C, V: B. v7 K+ p$ N
- do$ u% I; |& K/ j& [% d
- {, s. n/ _2 s. `2 w. Z, `0 p
- Err = getErr();7 f9 n! @4 {" H, v; e; k3 D
- }while((D1+D6) == 0);
; M( a* R, [1 \. M. E - }" ~/ L3 W1 M2 B1 x) I
- else9 _: ~- H1 P) k8 V
- {
+ N" Y0 N% k( H* c4 w+ y - Speed -= 5;
4 {, L. M+ r* i4 s- G: N# u% n9 p - moto(Speed,0); / R+ v9 @3 y8 o' k
- do
& [6 Z! K/ D$ h3 u$ `! N$ @3 v" _ - {9 x) V* H1 ?1 P0 d: e2 a0 ~# C
- Err = getErr();; o% I6 W9 k0 h# q
- }while((D1+D6) == 0);
+ _- D2 l7 v/ c' n" X4 Y# u - }; S2 l, a: F: R6 }& a7 @/ l
- } p1 S! H" e( |" t/ d4 {9 B8 ~# O
- else
# V4 R' E( e! N' l% Q" S1 E5 B - {8 e5 V1 B/ r" T7 {) g1 }' O1 O
- if((Speed < 160) & (Err < 2)) Speed+=1;
8 \6 T) j$ n4 Q4 ~( @% D - if((Speed > 100) & (Err > 2)) Speed-=2; 8 k1 D. W6 j* K& A0 m6 @/ R! k
- integral = integral + Err;3 {- t: \6 _4 ?' d
- derivative = Err - previous_error;. i6 r, a( h( \0 L5 u. \; J
- output = Kp*Err + Ki*integral + Kd*derivative;
9 a9 R" P! @; Q. m0 @ - moto(int(Speed-output),int(Speed+output));
( l- k/ H, ^* a0 c - previous_error = Err;
* H. L/ j: j1 q2 R! u/ K) A - } }) L# a, [/ n2 A! J7 H
- }while(!((lightsensor_2.read()) < (10)));* ?) c! G% }% d
- moto(0,0);
9 _- Y2 i% ?! i, q, l6 J7 l - delay(500);: h+ l, X. s$ z3 q7 a% _/ N
- buzzer.tone(262, 500);- r7 C. G2 a" Y: \+ Z7 T1 H P
- }+ _$ c* B) X; n: K/ X, P( G1 ]
* |# `$ Z( J' o* Z& B- int getErr()# y8 ~) m5 e! r W T4 z
- { $ G% a- y( Y. @6 G$ n/ X3 S
- sData = linefollower.getValue();, ~! ?5 H6 g$ j+ M" t4 Y+ ~
- D1 = ~(sData>>0)&1;2 K( \' D. [, [( V( P% r0 ?
- D2 = ~(sData>>1)&1;" i0 E9 n. U* }( p6 e- }
- D3 = ~(sData>>2)&1;9 x+ w: K: t" {/ ]
- D4 = ~(sData>>3)&1;
* C/ P# k7 m0 u+ T: i - D5 = ~(sData>>4)&1;) U" |5 T1 W* | j8 w% f- L5 U
- D6 = ~(sData>>5)&1;
' E1 K! D4 ]! n* H1 `$ G - int downD = D1+D2+D3+D4+D5+D6;
. w8 [$ N4 F2 y- A1 z - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
- w' Q" {0 j& s" t# D$ X3 R+ { - if(downD == 0)8 t6 q3 E# b" O$ E, G3 W2 @
- {. R) A8 t, ^0 ` _$ q: ]$ e
- return 99;: M/ P0 b5 B& W5 \ }7 g$ b6 p
- }. b" h% Z r4 I S3 v. i6 n7 t
- else! B) |/ x7 c/ J5 T* o( \' o
- {
4 ^- i3 i+ ?2 v" k3 i' {, ^ - return upD/downD;8 l2 R# R1 b3 |4 l
- }
, k( J, Q" C: i+ ~" J - }
" E* Q6 U0 d" W4 e" v2 j4 R
# R+ C( F+ w2 s8 v2 y) [- void moto(int Speed_L,int Speed_R)! R* ^' O. p8 y! F/ o
- {6 o3 V* M% k3 D' {; f5 ^
- Encoder_2.setMotorPwm(Speed_L);$ F: S3 k! B9 K7 \6 w( V. k
- Encoder_1.setMotorPwm(-Speed_R);
: `& J3 m7 j5 S8 q- R - }
複製代碼
. J3 y" l- h1 ^/ _% v. hMeLineFollowerArray.cpp
, @9 H8 s& ?) X$ I3 X) |- #include "MeLineFollowerArray.h"
, l) |& I0 ^2 S2 E
3 a R4 _+ n9 u& C- v- #ifdef ME_PORT_DEFINED x" {7 ^: i" K
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
/ i0 s: [- U4 ?: M$ I1 q$ V - {
* Q% K/ s& _! h( @$ Z - : K) h) C; @) j& W7 P
- }1 {; W; V0 @; s' }5 c
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
. U# J4 }6 U& L/ Z1 K5 @6 I - {
. _* ^8 {( p- \- P - _DataPin = mePort[port].s2;) X3 C8 n6 c* S! s1 u
- pinMode(_DataPin, OUTPUT); - o! `! |8 z4 { _# q# T4 E1 X
- digitalWrite(_DataPin, HIGH);
3 J. ^; A. V( T5 n" ] - }
2 P+ t* F- g5 ? - #else // ME_PORT_DEFINED
0 m, o; g: q( T7 x2 p2 H. c - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)# U: L! c6 A, i% b4 E* p( v
- {
. Z; \6 q# d+ v: {5 F - _DataPin = pin;
% h3 U& X- S5 { - pinMode(_DataPin, OUTPUT); ; y" ?- V6 g% i0 F/ s
- digitalWrite(_DataPin, HIGH);8 g! ?* C* j& O# }3 S
- }
1 }2 D2 s2 d5 X! i - #endif // ME_PORT_DEFINED+ p- w2 {+ |% G% C" n
) Y4 s* `0 C# K' K e
: D/ h2 w: `# L" G- void MeLineFollowerArray::setpin(uint8_t pin)
* A! p! L/ I" E, A9 R# `: T - {
0 E3 a. l: |9 |0 [& r - _DataPin = pin;3 j3 Z8 N& p3 v) |% U5 t' N
- pinMode(_DataPin, OUTPUT);
4 O( I* e/ w3 W) ]* P \+ G - digitalWrite(_DataPin, HIGH);# n4 k# g& k& p4 D
' {7 F8 ^$ ^( }8 W- #ifdef ME_PORT_DEFINED
- n) k, I! O t Y( K - s2 = pin;/ j5 N; k# F9 M; z
- #endif
) [* N1 h' p& V4 }# G: v+ u - }
( V S9 C8 K- B, P - 7 C! i# Q1 \. W. z8 X8 m# b
- uint8_t MeLineFollowerArray::getValue()' R# h* |% f: g _; a4 R4 ?
- {$ A3 [( A% z0 ^3 W
- uint32_t LOW_level_read_time;
5 j6 U3 a4 y c- G7 y6 m9 F - uint32_t HIGH_level_read_time;: Z; Q! z* s4 S
- uint32_t time_out_flag;# Y& a& g( |# u4 V
- uint8_t Sensor_Data[3];# M- I6 w9 b8 T( r$ \
- static uint8_t old_data = 0xff;
& O( _7 {: i; M' b o - $ l- m' `3 K2 x$ I" N9 a1 E9 q
- pinMode(_DataPin, OUTPUT);
( l" y, h2 s: T* o2 Q - digitalWrite(_DataPin, LOW);+ w1 y" G2 \) n5 a& M
- delayMicroseconds(980);! i; k; M( U' a7 U d1 C
- digitalWrite(_DataPin, HIGH);
( A+ w, m! U7 P9 k - 3 g5 m G, S5 c5 I
- pinMode(_DataPin, INPUT_PULLUP);
6 H: D( h. T/ e& B* } - delayMicroseconds(10); z4 a5 {4 u- K/ o6 P
- 1 ?, T$ c6 r6 F/ ~* |) B
- time_out_flag = millis();% ~& T* V) S% R' ?# @, M0 {2 C
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 r/ U) j6 ~% C3 j0 t1 }9 d' D - ' I6 V" w% \ o5 ^. g" }
- LOW_level_read_time = micros();( V2 ?+ _( {( Q- w3 o/ q/ h* n
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out4 ~" z6 F" X% q: Q0 {& k
- {
) c$ i% f- ?2 e: l: O% o: C6 ]$ q - return 0xff;& J, v' D, h7 K
- }
# j4 `) g6 L) b' i2 \: o' ]8 v* K
/ b# R1 t' G2 r3 @5 V+ l9 G, f j$ l- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. ]6 L; l8 ~. f: n+ X& q+ V - 1 ]7 M' t+ T4 s* i1 e3 ]" A) u
- HIGH_level_read_time = micros();
3 [: ] _5 K; `% |( F& C- _ - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
' R3 C& s# T) _
" M2 V: I, U8 W, j- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 s7 w2 v2 ]; Y( T$ g) ^1 C - {1 R& ~ e+ i; S+ T4 b' Z! n8 V
- return 0xff;
" u/ J- O, ~. ^/ m) `, W - }
* h3 B. b( j5 J, `) ?8 g& } - 6 J$ g2 V9 ^6 f3 |! y6 g9 ?3 a& n
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))- r6 N$ L: f9 q {8 H- R3 V. b
- {
7 H2 \- d d& K0 Y7 |3 Z - return 0xff;
# t+ J" T! x4 J& d7 J5 p - }3 @& e1 Z: {; Z3 N& n) P) J- J; a% F
2 u- k2 P! i+ b' n- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' u: _- W9 o$ c4 k# N
- LOW_level_read_time = micros();1 @" `9 U" s1 X* |% M+ T7 @
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level, x3 H9 l: e, ^5 ?6 J
- ) b$ f" R. o; v$ z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out" {( u3 _) F! p: ?+ B* n6 `
- {
. x8 T) |' w% i1 ~4 K - return 0xff;
1 Z* G6 Z% ^: A9 g8 Z - }
U. W* \7 P- w% E$ H+ f, ?9 X$ o - 1 Y! }. D2 ^7 Y1 ?% e. N5 W# f
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))/ k4 | U6 Z- }: y3 r& a
- {
* o8 A" W1 D- V - return 0xff;3 P6 h" f! W m. E N7 \; |% R
- }9 o$ M: R9 g% \: A/ y( W* u3 {
- 9 w- V% c5 \1 c$ B" |
- for(uint8_t k=0; k<3; k++). S. U, U% O; }" _% ?3 ^
- {
( E) t: {' @# f4 k - Sensor_Data[k] = 0x00;
) U# K. o8 I2 K5 E1 j/ W
# O1 \; f; ? T) N& L- for(uint8_t i=0;i<8;i++)
9 S0 D# A* o5 l$ V - {
4 N6 u/ U2 `9 ~ - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level% |% z1 `- t. w" H" ~( a
- HIGH_level_read_time = micros();
3 S5 P$ [* Y2 \9 w n - LOW_level_read_time = micros() - LOW_level_read_time;7 {0 u: r& U- i6 I( _( K# k8 W
" V7 M, L2 k3 p5 _/ \2 I# I. q& V- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )2 F* _: A! |( k; [. \8 K
- {
+ [4 c- E/ f' l7 x4 R' M, e - return 0xff;
! `5 o2 H& d& e+ N4 d; {& W7 X2 T - }
8 h; s4 ^' R6 J+ v0 X6 R5 X
$ ]- k# y& G' x- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ R: v% }- Q% g( w% L - LOW_level_read_time = micros();
7 z3 K, r, x) v3 O5 Y# `! c - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
$ j B/ f% e v$ z V2 K
: G/ n' W7 V' k- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1" Z; l- ~7 C5 d% g0 V' l
- {
$ }# ^4 u- z7 y# T9 D7 U - Sensor_Data[k] |= (0x80 >> i);. U* d, l/ n/ I' f
- }- p! P- Z. C- r' l8 K/ r
- else if(HIGH_level_read_time >= 100)
/ \. h3 u/ |. I3 c; b - {2 C! C1 F$ s) C+ o* Q. J
- return 0xff;) x @! y% e/ n& s5 X3 q2 o
- }) d8 E) D/ m2 l: r. Q
- B+ s; z7 X' p+ @& d- j0 Y, b- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)6 N# P; a$ `, D( W8 P
- {
9 k7 h. e8 u+ m0 n' v - return 0xff;1 k2 ?2 k: m4 k( E7 h
- }
( a2 G7 A% X$ U0 B - }
' h/ n, d# }# M0 B" U* K8 A - }. b: l8 y& J7 Z* q- f" c- Q
& n; }! @% z5 O5 v) l9 |- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level1 C3 I2 A; H$ o6 R
- HIGH_level_read_time = micros();
! J7 g/ i( L3 A$ }5 c - LOW_level_read_time = micros() - LOW_level_read_time;% E! s" l; K- g% T; \; J
- ) j6 }/ `' g, I+ b/ m4 i; B ?1 h
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
0 ^/ z- u( F, N2 }6 K5 V - {
3 M7 o5 H/ j4 b- K5 y0 [ - return 0xff;
- P/ X8 _( h7 F1 \. r3 M - }# o2 V5 t/ ?5 c) `8 P v
' ^% H5 R2 K4 [, r- pinMode(_DataPin, OUTPUT);
: f: h# j" |8 S# U - digitalWrite(_DataPin, HIGH);9 S6 }0 @" i$ `4 O/ U* Z' u
& f7 P, z, L, D8 z- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))0 M3 X7 e& V' \3 V, _( O% v
- {
% C: h+ ^* d- }1 Q, A8 ] - old_data = Sensor_Data[0];6 o% B8 f7 Z2 s c; f v$ c8 W
- return Sensor_Data[0];
6 \4 B3 C9 @9 j2 Y4 n! {% N - }
/ J/ H) h9 {& X! ] - else: j# A) e1 n& G' f3 g( o' i& f
- {( n9 p5 A3 n* n! t6 m. ~- r- H
- return old_data;8 ]3 O. s# ]# O. }
- }# P9 H, K& u! @- j7 i
- }
+ g4 E2 V% a' F8 N
複製代碼
' ]3 C/ O% c8 _. JMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
7 }. Y3 \: x% L0 T) `! B: H3 Z2 a8 y: q" U% y+ z. B
|
|