|
|
/ W& W" U) h8 ?' l# b# N# W) v- #include <Arduino.h>% Q+ | D8 D3 f! T. ~
- #include <MeAuriga.h>
# S8 J) [6 z; o6 O - #include "MeLineFollowerArray.h"2 g' r0 B1 Z2 L2 ~
- 0 f5 H9 L2 {, q0 q: k+ K: K7 L. ~1 }6 o3 D
- MeEncoderOnBoard Encoder_1(SLOT1);
; m2 k! f8 t; i3 E4 f - MeEncoderOnBoard Encoder_2(SLOT2);* F: A( ]. w- G* n3 C. e
- MeLightSensor lightsensor_1(12);% K: f6 ]( D6 E8 J$ g
- MeLightSensor lightsensor_2(11);
& A0 v! A2 W: |- d, J. ~* k - MeBuzzer buzzer;( k! f6 U; O- ~, o( t9 Y
- MeLineFollowerArray linefollower(PORT_6);( C& `$ s2 k" U8 i) I% ?
- 1 Y# O) {8 R0 ^- y- u- Y
- #define Error1 1
8 m+ ]6 x' g% E# j3 x/ d - #define Error2 2
' {6 ]' T3 E# ?$ ^/ b/ i - #define Error3 3
0 j0 R. }2 I/ I/ W% L3 l/ D - . {6 b! B' V* W
- #define Kp 15
2 ~3 P Y- B7 @& O( J( o) C - #define Ki 0.15
/ Y+ g5 V' X2 z2 i* m, o9 V+ |2 Q - #define Kd 0.03
( U! h. L n) q) z3 U
* Z! S- [) c$ `% \# F- uint8_t sData;: \8 {& h W) V' w+ j/ l z( r
- uint8_t D1;
% P P2 L1 l# M2 i* ^1 C4 W( @! k - uint8_t D2;$ d, ]8 B* L; @
- uint8_t D3;* ~: G+ a3 c( r: ~8 Q$ S; h/ i
- uint8_t D4;
8 ?/ a- u/ ]. I& f" a$ m7 B# p$ j; } - uint8_t D5;, k: ~) H) Q7 m: J6 j7 _" P
- uint8_t D6;8 Y" J6 R+ [3 O% d, W
$ J6 j. [/ k8 c- float previous_error = 0;. B. ?9 R9 q2 e- F! J
- float integral = 0;4 J. ?* F. ^3 B% o3 |% y) E6 V! \
- float derivative = 0;
7 ^$ e: z5 U. C3 j1 o3 s - int Speed = 160;% f1 m6 I; \, O/ r
- float output;4 W: P7 A: q' y% r, S
9 H: B, e0 ?) S/ Q8 P9 v- byte Left;
7 h7 S! k; b5 C$ C5 q - 4 \5 J9 l5 \0 Y# |2 Q. D6 l
- void setup()
/ t6 {; ^6 u( Z - {
; ^+ [. g% \. |' b M9 ] - //Set PWM 8KHz
' p, e V/ O* e% V( }6 w - TCCR1A = _BV(WGM10);
, M1 X1 b8 h6 \# l9 P0 @ - TCCR1B = _BV(CS11) | _BV(WGM12);
* c3 D5 Z+ h) P( T' ]6 H; u - TCCR2A = _BV(WGM21) | _BV(WGM20);
1 ?; ?: E @$ u) I& C - TCCR2B = _BV(CS21);
6 X+ F& g, ~4 z1 ]4 s - Serial.begin(9600);. r, a+ r) _$ [9 K$ h
- buzzer.setpin(45);
8 K* ~& b, h' r. ?- p+ e/ t/ b _ - }
. `2 ]$ o5 X% g3 ? c0 \ - 7 ~! z+ d( q; i: e# \+ K
- void loop()
* w2 O# Q: p% c1 m; I1 Y - {
' Q# f1 R! D. R/ Y' v - while(!((lightsensor_1.read()) < (10)));' U4 {9 y( {8 s z% A' d
- buzzer.tone(1047, 500);$ j r4 g' a" ]0 ]+ l
- delay(500);) |8 C" M, W% l- R/ M
- do j' ~7 w% W; G5 h
- {
O, g/ V! Y2 M4 ~& Z - int Err = getErr();
$ V, L% [$ T( u - if(D1 == 1)( k& @+ a" J" S
- {; B0 A' k, d3 |/ _! m
- Left = 1;% L$ H) r: W' w8 F% \5 o; W
- }6 {/ e# T* Y8 a, O; B2 C
- if(D6 == 1)& D3 U$ Q2 O6 w, F
- {4 v- J: W- T( y
- Left = 0;' A u4 v7 q, D+ Y+ ^1 p t& P2 D% A
- }
9 E- t2 J- V, K* ^5 h- S - if(Err == 99)1 |) {: ]! [1 i; @
- {
' o) m- x! Q/ r, q; V, G9 e8 T0 o - if(Left == 1)" P" w5 ?2 M6 q+ o! V
- {
+ o/ {( x3 `" F' ` H" S/ M7 A - Speed -= 5;( F) ?2 w. |( d4 e( b6 K' f
- moto(0,Speed);
. M8 [) C3 L+ A! E7 b" A - do
( ]. D7 C$ i1 y4 b8 { - {
2 [" l1 G9 x/ a. M( f7 Q- p# s - Err = getErr();
, P8 ] e% R0 [* |9 p: O, }8 Z - }while((D1+D6) == 0);0 N& e, L& c2 u/ X7 a
- }: d0 J3 B2 E: v* \" i% v+ v: m
- else
+ r/ v" ^% g, S8 c% I- Q3 _1 ~ - {/ E/ ?, ~6 H. ?% U8 W( k) L
- Speed -= 5;' P4 j, j& ] ^, T3 a
- moto(Speed,0); c* _" J' o8 ]
- do
6 K2 T4 t1 ]1 p: L2 h1 E2 p9 F! t - {
$ a# G o; o* ]. f3 I9 @; O - Err = getErr();
8 O5 ~% P* ~/ M - }while((D1+D6) == 0);" a$ y) b/ G7 \9 W6 O$ i; Z
- }+ E, f) p* c% b! ]. U
- }$ x' N2 ?+ o1 F: {; X1 p
- else% d; ?2 H2 w1 m0 `! y
- {( o+ a/ S7 _+ H
- if((Speed < 160) & (Err < 2)) Speed+=1; 8 M, e6 n" d2 O5 ]
- if((Speed > 100) & (Err > 2)) Speed-=2; 0 Q% C; Y. }& ~: r8 m
- integral = integral + Err;) X$ T, G9 ~# w7 k
- derivative = Err - previous_error;
/ Z+ E7 j6 l3 v" }0 r! h - output = Kp*Err + Ki*integral + Kd*derivative;
) t# Y! q' J1 ~5 q: y/ x2 D - moto(int(Speed-output),int(Speed+output));6 E5 Y: A: z" G0 P2 n5 T6 }+ z+ H: K
- previous_error = Err;
2 _; i7 q; N0 t' h6 c - }
* i' F/ a$ j* F z - }while(!((lightsensor_2.read()) < (10)));) U+ N: W. U9 `, g W/ e
- moto(0,0);
* }! ?1 c) f+ C6 h5 X5 e4 u - delay(500);
3 [7 Z$ t( Q5 k4 e7 @2 p7 R - buzzer.tone(262, 500);+ T s4 P+ Q- Z: b3 M- i5 ?4 x9 T4 ^# i$ X
- }
, [5 O6 T) k! s: J, K1 ? - * @% ?" s% j x! k* P, S, d7 _
- int getErr(). D C) Z3 H. C% E) u; z
- {
* T" g, T, U+ R% Q - sData = linefollower.getValue();" k7 ?: D$ C* m5 R& {
- D1 = ~(sData>>0)&1;' D' k T$ N4 O, r* u) I* {
- D2 = ~(sData>>1)&1;- `' d3 b; q. N: p
- D3 = ~(sData>>2)&1;
% x% _! h( a& s+ ^4 t, j. R# G - D4 = ~(sData>>3)&1;
- f: ?4 L4 e/ p% S* ?2 v - D5 = ~(sData>>4)&1;
' h; Y5 P, L8 V/ w8 I - D6 = ~(sData>>5)&1;/ [2 M+ E1 d5 f
- int downD = D1+D2+D3+D4+D5+D6;# i- r8 U4 A5 H$ |7 y& z
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
# d& `2 l' ~% g7 X# A - if(downD == 0)
6 |' O( c, p( K/ z! X0 w2 R - {
* _, A' p& G( N6 M4 i9 |; T - return 99;
- Q5 {$ e' O6 X! d - }: a: s3 w9 l: Y: Z& z. A
- else
4 _. F. T6 b* d' E+ ]! F - {9 D$ `) j5 P4 }
- return upD/downD;1 j7 i; i! t1 z7 R4 Z2 k9 _* T
- }
8 t8 Q4 Z* b0 {' G9 F0 r) q - }. S0 c- q) J$ E8 n6 `: M
- . f; k+ w7 Y2 W4 T' f* g7 @
- void moto(int Speed_L,int Speed_R)
8 F% Q+ D0 E7 d8 a" X$ j. X! A: C - {
& F. m8 _. {& {4 y, h - Encoder_2.setMotorPwm(Speed_L);
( z) z, }2 k5 u% i) a$ l, b - Encoder_1.setMotorPwm(-Speed_R);
* I4 e. k9 `: a* w7 W% E - }
複製代碼
, Q: a0 b& o& p5 ~4 E: }) p0 l0 }+ lMeLineFollowerArray.cpp( Z' @. k/ F- b1 p6 _* `) y+ I
- #include "MeLineFollowerArray.h"( e4 k3 \$ y4 d' D$ f: ~! }3 u. d
- s1 C) V( B/ [5 ]9 e- #ifdef ME_PORT_DEFINED
" M: G3 M: o i6 E - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)+ c# K8 s) I& r1 t9 f
- {
* q, s! O. A( C) t0 |3 Z
# d; @& f# k7 [ ^- }% y0 W% }+ N; K. G
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
t! G6 c5 D' H, ]- f - { p8 P4 \3 x4 Y# `
- _DataPin = mePort[port].s2;
: K- }' \( h9 v F+ U& `" h5 s - pinMode(_DataPin, OUTPUT); ; l2 k1 z' n" K$ L- I" A
- digitalWrite(_DataPin, HIGH); Q7 ]8 A: y1 O! _5 }
- }
7 R) Q* s0 o3 C. ~7 Y) F0 y - #else // ME_PORT_DEFINED
) N8 G% U7 e8 l: Y8 j; z - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)) y7 `" I5 k. r
- {
# g* K) ?: [) y - _DataPin = pin;
: X* I) T$ @! u/ U# K$ o - pinMode(_DataPin, OUTPUT);
0 P2 C2 @8 q) L) f5 P - digitalWrite(_DataPin, HIGH);% N" G3 ?4 A7 M# P5 Q
- }
4 `1 @# T5 [9 M8 }2 i; o0 a! y: P - #endif // ME_PORT_DEFINED
4 m; s$ |* H' b; j0 B, @- ] - t4 Q: J2 l/ l1 f- {
- ! }6 k0 Z7 E% n, n: D
- void MeLineFollowerArray::setpin(uint8_t pin)
2 V: z v7 d7 C: W! m - {% y( X9 T$ T( W. t) V# Y
- _DataPin = pin;
0 ?7 Q5 X/ Z5 n- ]5 L - pinMode(_DataPin, OUTPUT);
, m- |# v" Z+ ]" ?1 K6 U$ v- y4 I7 o - digitalWrite(_DataPin, HIGH);
1 R, J( K9 V4 i, S. [% c$ @ - ! a' g+ U9 @1 {
- #ifdef ME_PORT_DEFINED, V+ o6 P& h4 i. A
- s2 = pin;
# a( h, ^. O4 Z! @ l' D - #endif2 [* E6 Q0 l; c
- }( d, r: t+ q. F. M
! V$ v2 `" Q# a) X- uint8_t MeLineFollowerArray::getValue()
" E8 K; B, F0 s - {2 z8 [2 a. ? ]1 d6 K$ n6 {
- uint32_t LOW_level_read_time;- {, y" ]* u; O1 y- M1 a2 E$ w
- uint32_t HIGH_level_read_time;4 ?' h: i4 T$ ?4 @. r: }
- uint32_t time_out_flag;
; N& b: T% R- l - uint8_t Sensor_Data[3];% h9 g( Q5 O$ z
- static uint8_t old_data = 0xff;6 A6 v# B0 w; T
- % [) U$ O5 M3 L: n
- pinMode(_DataPin, OUTPUT);7 |* Z/ i: s; P A. o$ Q0 A
- digitalWrite(_DataPin, LOW);
# K. k; F- f ]' @' @ N& b - delayMicroseconds(980);
; K: V5 {' T8 J. |$ B+ g9 L! b - digitalWrite(_DataPin, HIGH);1 ]3 i; K/ z k/ l: N" a- q1 y2 u
- % y. {' R5 a" K; D4 t+ [
- pinMode(_DataPin, INPUT_PULLUP);
$ u7 M5 q9 D8 [: K& b) t5 x - delayMicroseconds(10);
) ~* R4 w5 v$ Z$ Q2 Y - 6 l& p, w. M+ [: p5 X1 N5 ~
- time_out_flag = millis();, w; X* k' o' Q; k/ E: E
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% r7 ]6 K/ O) [; R6 R) l- w - ( N" B( T' q! z; ^3 |& B- p* h
- LOW_level_read_time = micros();
z/ A! {& f& F2 ~+ b - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
) W( q+ V/ L% S" _" u( O" j - {& H+ _. k+ Q- t, w+ j
- return 0xff;
- M: S9 k0 e# f+ A - }
0 w0 o% E" u+ g1 Z P' q
1 d) P$ Y! I V2 Z; N) g- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ | y1 |* z8 h, }; H7 m3 o7 _
, a0 r: W( G" x) F k9 \6 f& L- HIGH_level_read_time = micros();
" g. X8 c% P+ j, A; ]% G/ L, `" t% n - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level! d+ O! B; ^8 x: l+ W
- 4 V5 w. N; K. \' ~+ D
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
% N! f+ {# R6 d& G! F% ]- ~/ p9 E - {
: ]: W6 W/ J3 j - return 0xff; V: ^; B2 @) O
- }3 J# S8 |$ d0 v0 O9 D1 Q' n& X# ]
% w, u0 f7 S, u9 C' r2 }# q0 N- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)) `, o9 P# |, E' N
- {, z( @: c7 b" O3 }/ Z8 @$ |
- return 0xff;9 q. m$ t) W0 S. i4 K. W2 S
- }. b/ Y3 L9 N( r$ l1 D0 h
- & B/ }4 _; |& U4 B5 ~
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 T. F \) ? Y- Y# s& X
- LOW_level_read_time = micros();
2 C) F5 ^5 \4 J0 p2 W5 [ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
- M: V: X2 N5 u2 f& @. z. _0 [ - 8 R0 Q& G6 }# B) c; ]! m
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 s) j. q- w' Y: Y% d; ~, a8 \ - {' x$ A' t& U7 L/ Y; K) U
- return 0xff;
# m' d3 x# B) L' r7 k& x5 w. G2 c - }
/ U6 c- T J$ v9 g4 t
3 M) u$ }3 O# `! [8 U1 n- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))1 T4 C( i t7 h4 N* E. Y# J# f
- {
' K$ ^: i: i( O6 O - return 0xff;
, f8 p0 v6 j1 L4 z0 c' A4 y; [ - }
3 x1 I: s4 ]# Z; _( x - . u6 M2 p' e* @' x% u- S* c# n
- for(uint8_t k=0; k<3; k++)/ I8 h/ l1 F& a6 w" d# t
- {% `3 r6 ]. W& B0 h8 {
- Sensor_Data[k] = 0x00;8 O H) M& W5 M3 U) q) B: t
- 0 u) d0 E8 H6 e
- for(uint8_t i=0;i<8;i++)
% b& x5 G; u* x9 k. |# @% u( J - {# ]0 v0 Y& ^! ]" c
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level+ f0 P! H! Z3 P `- }
- HIGH_level_read_time = micros();
+ A4 L U' ^5 w - LOW_level_read_time = micros() - LOW_level_read_time;+ C2 ]7 w0 ]/ F. A& h0 y
- 0 C, r' E5 V; j4 N! c: o7 u
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
* T( Z3 H; [5 n$ {2 l2 A0 H - {) [. w1 j1 X! B2 b6 e4 i7 n
- return 0xff;
% O3 k+ w* [8 n$ H2 k - } D, P* S0 v) `2 n" d' ~! c U# R
' m( F) P) `% b- X' V- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ j0 S' q0 r, y P
- LOW_level_read_time = micros();4 E2 D4 u; x: y' _' e3 o/ G
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level' h6 ^ M# h; U! d$ x) N+ x+ h
- 4 t6 }! V2 p! [6 J9 ^3 @2 u6 C- E
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1$ k: X6 E+ w* x$ K$ s) a
- {
, Q1 q- P* `2 E; A% s F - Sensor_Data[k] |= (0x80 >> i);
8 \ D( Q) ^/ | - }9 p# k& E# }7 l
- else if(HIGH_level_read_time >= 100)
* K2 m5 H( ?- G - {( w* d2 \4 N" Q$ g
- return 0xff;/ i5 Z x4 X2 ]; y, [) l
- }1 ?* `7 Y8 M+ W$ i$ F
) {; C3 F% j3 h- d. V, K- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) r, L- C- \* F/ Z
- {2 `: I/ @( C; m# t' T8 E: V: V
- return 0xff;
1 i9 t! S! v- M - }
' |( u: |" v$ D+ g - }0 E C% I! b3 @2 N" m
- }
: D" S' ~' N, I8 l - : ~6 X2 q9 k/ k4 H
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
; c; W; j1 p) V' ~' @ - HIGH_level_read_time = micros();
2 l ~1 O/ |2 E2 }- H; H - LOW_level_read_time = micros() - LOW_level_read_time;
8 j- y @. n7 N+ V3 g$ ~0 \. e - 8 v) U0 C. k5 t# V. u: V: |' A
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
8 Z2 V; e3 q$ h: s - {, Y# p: \$ x W8 o' V
- return 0xff;
5 O* i* F0 o& X - }
4 k1 n3 D/ s: D/ ~+ i. i
% d+ ]! M5 Z9 v4 q' f' x7 ]7 n- pinMode(_DataPin, OUTPUT);2 {$ o' e/ K! ^6 Q4 O$ [5 D7 p
- digitalWrite(_DataPin, HIGH);$ j+ S! K+ H7 S# I0 ?% t4 f
- " [5 O" A+ L# V3 g4 {
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))1 s7 ?' E4 y9 d6 C+ E. N, {; G% X# z4 N* b
- {+ I2 _! n+ X! C% J
- old_data = Sensor_Data[0];* C4 Z' W7 x9 z% _* p( v
- return Sensor_Data[0];
4 j1 v- t. p! W% w( ~8 B - }) @7 f+ }' c ^
- else" J& E' c0 D2 [: u4 r
- {
7 T0 [( h9 O" a - return old_data;
2 v3 m- t/ |. e2 y3 P* q* S - }, }& p0 z# S% w: E% S
- }/ m7 ^: h& s& \: ?9 [2 \. n
複製代碼 % n$ B0 r: e* M; b: B; t6 L
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
' ~; I. b. T: B% q7 ~9 l% ]2 H7 v8 S9 K
|
|