|
|
% s7 X, ~4 }, H2 ?1 h
- #include <Arduino.h> h+ \+ p/ u9 p' q* h; L
- #include <MeAuriga.h>8 {( \& O; R) T' M1 u# ^+ R; Q
- #include "MeLineFollowerArray.h"4 {+ W5 S0 F% T1 I4 Z2 I
- & T2 N0 U! g; z }/ t
- MeEncoderOnBoard Encoder_1(SLOT1);$ M2 f* B1 o0 I6 i/ l; }, u
- MeEncoderOnBoard Encoder_2(SLOT2);
0 a6 A8 `$ |0 w. _ - MeLightSensor lightsensor_1(12);7 j9 {. R4 L" x( F7 F" ^- ~
- MeLightSensor lightsensor_2(11);
% y: G- y q3 ^( E9 b1 n" T, F - MeBuzzer buzzer;- P& k( j% s7 {/ D0 [5 J V
- MeLineFollowerArray linefollower(PORT_6);+ S# u8 H! L/ b* r5 q U
" `4 f. R$ K' B& i( u- #define Error1 1/ n. d0 l9 L2 a* x) k, i" V9 f
- #define Error2 2: v' i& o- e9 N p' E4 W
- #define Error3 3
- E, J# {% x2 V2 x$ p# {% N& c - 5 `0 g/ Q9 d: `; \7 F. z! }
- #define Kp 15
4 \/ m F5 X4 J4 h - #define Ki 0.150 A3 o. ^4 c [7 b. d1 @4 }8 H
- #define Kd 0.03
) T3 \ v j4 v3 c k* a - " C; U/ t& Z% J" I, |, q2 i! |8 @
- uint8_t sData;
# ^7 O2 V. h; a( ^ - uint8_t D1;* K% A& L! I ` [/ ]0 R
- uint8_t D2;
' j9 q# Y! V K, ]. F - uint8_t D3;
( Z: R, ]+ @7 d - uint8_t D4;( e3 c, z$ F. P+ Z8 q
- uint8_t D5;
I: ]: j6 |/ d0 M. z - uint8_t D6; x0 o+ F+ R4 M4 v- g g5 @; ^
z( C6 |! i R: G( G N; }- float previous_error = 0;& I$ t* E8 e# Q# B, s& d+ n# b% C
- float integral = 0;
3 l4 G0 I9 T: r# T" q0 m9 z' X - float derivative = 0;
8 ~0 \6 R$ E# x# Q2 \' k, L - int Speed = 160;, N. S) }' N, D0 t- o4 H) c; q
- float output;# `5 i; t* |0 J9 Z, p7 \4 \
4 F$ V1 v/ N1 ]- byte Left;
s! T4 ?( E. H' o6 Q" {' z8 e! w - $ C/ Z& {& M7 Y
- void setup() . O4 G! l- D% K! o
- {
3 V; x z* X! `! K4 P - //Set PWM 8KHz& D5 ]+ ~# ]4 D# d4 g, e
- TCCR1A = _BV(WGM10);- {2 E; e# n% \* @- K- R
- TCCR1B = _BV(CS11) | _BV(WGM12);
/ q: d! B" O. Y- B( D- O, J - TCCR2A = _BV(WGM21) | _BV(WGM20);
/ g# A$ x+ r9 R# ~5 f1 R( n- v5 B - TCCR2B = _BV(CS21);
P0 x7 C0 t5 e' a7 [: D/ P - Serial.begin(9600);
~* q: h% b& V% l$ O# g1 O! Q7 ? - buzzer.setpin(45);& i; _3 ^& Q8 e. d) B, c$ ^8 c1 G
- }8 L" e$ V1 T* M9 E
! C) ?; Y9 ^( @1 Y9 Z2 T$ e- void loop()
3 u: V( g* T5 M4 h - {
1 ~0 N! O2 [; p+ Q$ v6 g% \/ z) p - while(!((lightsensor_1.read()) < (10)));
$ k$ B+ V1 f# {$ U9 @ - buzzer.tone(1047, 500);5 ]% i6 w* S) A
- delay(500);
* J$ A7 h2 c p3 Z- ^, x6 L - do
! E' P5 c9 S& e - {3 R5 x8 D$ D" a3 T
- int Err = getErr();
1 c2 h: U+ V2 ~/ J2 `5 s& j - if(D1 == 1)
3 r9 l/ J3 q/ ?7 g - {
& \* v6 B+ {5 h* a C. K - Left = 1;
( K3 e6 Y: P( k6 ~4 a4 M o* g - }
3 H; {* j+ E/ K- u) c1 |9 i - if(D6 == 1)3 L7 O4 k2 I/ Z! T
- {
3 Y% B2 w7 c0 e6 o) p. ]; H - Left = 0;
8 I* S7 K# t( K) i# {! W - }1 f; E% s: M' x& |: d3 V7 ~
- if(Err == 99)
* u- T+ n |1 E9 b" ]% x - {
, l3 o0 }7 M% b% d) t - if(Left == 1)
) W2 b3 K' z; W5 ?; R1 A2 _ - {* b( l3 ~3 s7 j+ L1 U7 _) S w* ?; }1 u
- Speed -= 5;
( D( @8 A. b u1 r- F - moto(0,Speed);4 n0 F5 Y6 R: \* j- n
- do
$ x `( c$ b5 t; H2 H- S - {
+ q' l9 ?. V/ Y - Err = getErr();& S9 S6 h6 u) U" D _0 r5 z/ y
- }while((D1+D6) == 0);* V6 {! v8 p' o" f8 e5 X
- }) `, T# ?5 c1 J$ {3 E ^: |2 J
- else
8 u6 U3 x* y2 H( @7 P; b - {
7 I" {% C. y) c) m - Speed -= 5;
- T- d- }; {8 D; d* H2 K3 J' w - moto(Speed,0); % Y" s6 ^; T: a/ C3 w6 Z
- do
! g5 ^# v# V7 H( W' U \& H - {% p, h* X; T* n
- Err = getErr();
/ q% l; C2 v8 _$ l$ f - }while((D1+D6) == 0);! P* o4 Z0 K$ E, q4 F/ [. z
- }
# i' Q. z# w4 m5 ]. n0 l& L - }7 k' Z5 }+ N, @. T
- else
3 `) H4 a3 J% `8 G* Y" R* A - {
( i6 K9 G. e7 z( g; f - if((Speed < 160) & (Err < 2)) Speed+=1;
( M ^7 i8 t. V, X" r R# h/ S9 @, h- H - if((Speed > 100) & (Err > 2)) Speed-=2; : Y2 R' s; b# y4 n/ O( q! ]
- integral = integral + Err;
1 `! q0 s0 A$ C; C! W3 U3 a5 W+ | - derivative = Err - previous_error;
6 }0 ~6 x# E4 J7 D4 r - output = Kp*Err + Ki*integral + Kd*derivative;
: S) h. z. B$ f2 f- E+ \! _, h. L - moto(int(Speed-output),int(Speed+output));
- [$ H7 g5 ]* M4 [# d/ B2 b - previous_error = Err;* W% r1 G* D# D3 A- {7 C! ^- R
- }" Z1 x% E1 s) V
- }while(!((lightsensor_2.read()) < (10)));5 I V6 m$ a A1 w
- moto(0,0);
" v k; P3 g" S* { - delay(500);
, F9 A6 t. k8 {( b) V; A b - buzzer.tone(262, 500);
4 b- E1 r3 C% ^) h+ B! @3 y - }( z, B: @; a6 M
# f: K$ N; u$ u% i- int getErr()1 k' Q( s) V1 t7 ?$ F
- {
/ U# d# f( z/ s2 [. S+ J - sData = linefollower.getValue();
: d3 [' v) w6 ]! [- T% X1 O - D1 = ~(sData>>0)&1;
) ]9 N& M8 k( G& c1 Z - D2 = ~(sData>>1)&1;8 U; P; O! G: I9 l
- D3 = ~(sData>>2)&1;
( b. Y4 i2 [! e$ O7 D" p. X - D4 = ~(sData>>3)&1;
8 Z; z$ K+ z0 {5 t8 g+ y - D5 = ~(sData>>4)&1;
2 q: O+ W+ G8 B) ^- C4 ? - D6 = ~(sData>>5)&1;( P* `4 I: ~. K! `" M
- int downD = D1+D2+D3+D4+D5+D6;
3 X* V, f1 Q( O) u: v - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);# e3 {& H0 A5 S! Z% K" v, O; {
- if(downD == 0)
5 v, K( I7 g0 e- q- N% q - {- k- [, F0 s6 A6 e
- return 99;
( s @; {( t7 W - }
7 J% V) } v9 ~! H6 w% O3 j - else
9 J( i9 E1 m) w3 Y - {
4 C1 D; H8 Y/ \% q - return upD/downD;
; b2 b& V- l! i$ W+ _* X - }
$ T# q$ g6 u- {0 h# ?- t - }+ W$ O2 r9 h! L
- 1 \# v& J% D/ P* V+ ?6 O) S9 o! R; ~
- void moto(int Speed_L,int Speed_R)5 U/ c$ w2 ~4 `( p% P, X5 n3 M% l
- {0 B( N! z' e0 [3 p7 z
- Encoder_2.setMotorPwm(Speed_L);
: u, r& E, H7 U% l# ` - Encoder_1.setMotorPwm(-Speed_R);9 u$ f+ o$ p" x. C
- }
複製代碼 2 \5 g+ F8 x' h# P( g
MeLineFollowerArray.cpp
5 ~) R- k, r1 O4 P3 m. y$ e- #include "MeLineFollowerArray.h"+ A# j- W5 S" W8 H* N3 @/ i
- * l# a9 K1 O, \$ |
- #ifdef ME_PORT_DEFINED
! S$ C" a/ ?" e8 X0 n! Y$ s% o4 P - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0). c/ e0 W, q; @2 _
- {
, w( \/ L% ~6 l8 Q0 ~
8 x8 D( U0 Q# F! Z' B- }
$ s+ b: {3 h! t- y; x7 U/ n- `: y - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)/ ], v. T/ z1 B3 R
- {, J3 E! ?; g+ V9 j# S2 b
- _DataPin = mePort[port].s2;" \$ ^( @" p; a4 y5 ^% m# K
- pinMode(_DataPin, OUTPUT); ; |+ l+ t* ]! @0 A' F1 a1 }2 S1 v
- digitalWrite(_DataPin, HIGH);
' M7 d" D1 r: H& W - } k2 ^" m/ P% F: M: o
- #else // ME_PORT_DEFINED1 [% a2 L; w/ I
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
+ R7 a. N5 q8 C# G - {) }1 s" [* u5 h! n1 \( q
- _DataPin = pin;
6 E7 P J6 Y+ j- w e - pinMode(_DataPin, OUTPUT); 6 _' L- F/ S* Z v+ D, {
- digitalWrite(_DataPin, HIGH);( n7 x2 F- ?) g3 ^
- }6 ?; P/ s) l$ t5 t1 P
- #endif // ME_PORT_DEFINED& {5 E2 c n% l) v: g# U
- , E% c R9 c+ W& W# r% Y
: K# a; I7 N& l' w( A# C- void MeLineFollowerArray::setpin(uint8_t pin)) l& y: I7 h* P# \- A
- {6 h" T: A/ O8 S# h! q
- _DataPin = pin;% e# r! t1 t5 S) d4 x. A/ B
- pinMode(_DataPin, OUTPUT); , X$ M2 X0 o4 X A/ j* x) ~
- digitalWrite(_DataPin, HIGH);
2 ^$ R6 [2 L, [! L: [# ^8 q+ j - # c Z& y) c6 i, G2 z6 |- T: t
- #ifdef ME_PORT_DEFINED) E% i9 J4 c4 D6 E, j
- s2 = pin;
6 N, J1 |0 r0 Y6 K& d8 [2 { - #endif6 H, o% Z. j2 |: H
- }9 a( I X. K. Q7 e1 a$ N! Y
- ]6 \" f9 m; Z$ b1 q2 R' E- uint8_t MeLineFollowerArray::getValue()
6 Z( k+ ?8 t6 Q - {( F1 m) V2 D+ T5 M( s ^% W
- uint32_t LOW_level_read_time;: P( g1 K- y( m. p( S
- uint32_t HIGH_level_read_time;1 r* J/ D7 C5 W$ z" k- }; h
- uint32_t time_out_flag;1 H& _' U0 Q3 Q* d U" \' s
- uint8_t Sensor_Data[3];
* W5 Y' a9 Q/ r, ~5 q7 D - static uint8_t old_data = 0xff;* P' t) [# x5 O/ r) W" b3 N
- 6 E( q& [: R( q2 w' m
- pinMode(_DataPin, OUTPUT);
8 G, }9 O3 j W, i8 Z - digitalWrite(_DataPin, LOW);
5 v9 B; ?0 u6 o - delayMicroseconds(980);6 Y$ r: ]4 t& [) `0 @2 b
- digitalWrite(_DataPin, HIGH);2 k( H( ]+ |3 E+ \: i
* P" \4 ]( l/ a0 f; h& ^( s- pinMode(_DataPin, INPUT_PULLUP);9 d. Y, X. F& ^, \4 }7 F
- delayMicroseconds(10);* W# ^4 c6 f6 H( B
* A4 [8 ]: ]% B- time_out_flag = millis();( z& A2 W% d5 F1 r% ~0 q
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); V" T) ^- H4 e
- Y+ o8 D: ^ o; c, x
- LOW_level_read_time = micros();; E4 m: N9 J) N' z- |# V1 J4 b
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 c( c; q; t3 k7 c" x
- {
2 `8 `/ `4 l# ? - return 0xff;
# W t* u0 V: a9 K2 k - }3 }+ a, X. p: F
4 S7 u" M7 T; m |- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 a0 O/ o: z+ X) M7 O/ X# G
, E* @: z5 s0 H) z3 p- HIGH_level_read_time = micros();. u/ B; u( g9 B: M7 ?
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level: A; I. E- z9 m1 E9 K: l2 d
. _9 ~( k, {: V$ C7 D+ ]3 [- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
/ I& J7 m& ^; n% }$ k, k- e6 h7 ]( m) o - { V% I& B }$ @" r
- return 0xff;
1 M0 k$ A& Q6 k - }
, z- d7 J9 a& |% H2 H w% ^& R
* c& ]7 x. w# m% M3 c- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
/ o D L+ g" _0 N1 g - {, D) ^: M4 [8 S2 `
- return 0xff;
% g: x+ V& a8 J. T6 |* R: _ - }4 a( ?; K1 Y7 v. D& y
- 2 I; _6 R. l' @! H
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; _1 o3 `1 J, S; w - LOW_level_read_time = micros();5 |1 h& ^, v3 K4 D# g0 g; _
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
( `% G3 z, Q# K8 f - ' ^6 s5 f- _8 E1 J& u4 m
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
4 \2 I2 ]; R' \: U! i7 _5 A - {& s5 O- L A, ~2 m/ E5 a$ F
- return 0xff;
% C# k, e* W" Y6 l2 Y. T" v - }0 J) ] U0 x$ v& k+ u
- * r; N- W' K* L9 k/ H# T7 L
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
" C2 K8 f) \8 J - {* P0 T( i/ X# H, w2 }# ]
- return 0xff;; |' O5 {# z& ^ A- K& {
- }
% B2 Q4 j0 R5 s2 b5 ?
# m& R( ]% h9 s6 D6 {* I, R- for(uint8_t k=0; k<3; k++)8 P! m6 T* ^9 ~; l- O4 h S
- { Q( F* I4 N# Q; d. \
- Sensor_Data[k] = 0x00;% E! E) M2 ^- `) b& }2 m8 f- _ o; }
, O6 p. p0 V& _4 u/ @- y0 @8 h" S4 t- for(uint8_t i=0;i<8;i++)4 H7 U d) t2 {. ~0 J9 n7 f
- {
: M0 Y" b/ i1 C _* i - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level( R3 O" `; W1 ~7 d( p, A( q
- HIGH_level_read_time = micros();$ I8 l. I( [9 E" P. Z. d
- LOW_level_read_time = micros() - LOW_level_read_time;
) v$ [4 Y' }! K - ; Z- e D2 T" i! t1 T$ @& H: q; u
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )$ g: t3 ^! {( e# D$ {
- {1 f* }# D: _0 n
- return 0xff;' _1 w6 h$ x7 P$ t, _/ r$ K
- }- o. h3 a+ J6 h0 \9 O
/ |1 ?0 a8 d4 K z+ q; Y% f- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );/ g9 |0 @( a3 ^
- LOW_level_read_time = micros();
. F3 B+ K0 D/ ~ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
/ T7 I3 S4 K' h' a% |2 Z - 1 S! Z* e+ D% s0 ?: j" b
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1" ?- |1 I9 g! B% c, e/ a1 X+ k1 j. O
- {& X/ F/ a3 w/ H2 \, Z
- Sensor_Data[k] |= (0x80 >> i);+ X. {& n% s- R
- }
/ j. N; q4 I% @) ^- W - else if(HIGH_level_read_time >= 100)( S o5 m! f# a* ` D# B
- {
% i; ~3 G& M1 y% @9 H; a/ O" t - return 0xff;$ ~) j& c# N" R
- }; L0 u' f2 q- S9 N
- / \: y" y$ X$ ? c/ ^5 l
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
; J$ |9 M/ |, X* O; Y0 Z/ @ - {
% ?6 C& S8 ]9 i: C' w1 |3 O4 i. ?; V - return 0xff;6 C! }' B5 M; @2 V
- }
7 a: x& e; @0 ]( V# U. N% i - }5 c, q* y# |. T, X1 ^
- }/ Z' r6 O: b6 l y& P2 |+ O
- 8 P: S7 l" |8 M0 w2 u' b
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level: D2 E2 `$ \7 w* a; N8 {
- HIGH_level_read_time = micros();
+ D, v) n' v/ g' ?; q. q; \8 f - LOW_level_read_time = micros() - LOW_level_read_time; b$ d2 e: z8 [3 Y+ c. T
; ~7 x. t: m# J- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )2 f2 U. w2 |& x. P! Y
- {
: z, O* H9 Y6 m, w& ~' X& L; i. ` I - return 0xff;" b9 ^: E; M" x% _9 ?/ k: w
- }
! v8 F6 L9 o- Q p
- Y) |9 z) J' J' P- pinMode(_DataPin, OUTPUT);
8 y; e0 R7 j* q" ]- }1 ]8 Y - digitalWrite(_DataPin, HIGH);
! b* ?' T6 {% U( p
; v0 F& O# ?- B" n2 a3 O. F5 U4 Y- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0]))). A1 E* G! [) ~2 q5 w
- {
( [: G) q4 U( u2 f - old_data = Sensor_Data[0];
M, h1 p* {9 `4 N - return Sensor_Data[0];& S& E6 O+ T1 @0 b% |( U! ^1 {3 a% {' i
- }: a j. D2 l q& u$ D* E- q3 }
- else! W7 I H" |2 ]# _8 b
- {6 C, ~- y/ U8 w
- return old_data;& h6 ~, V0 x( z' d K$ s, Q' y6 k
- }
9 b6 V4 ^4 ]' b) G8 z0 {6 e0 {: l - }
3 X# F( v7 P/ b6 O4 ?
複製代碼 7 C1 P4 ~# Q* E7 Q4 \* s6 ^
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
5 O1 @; {- ` Z4 v5 M) l; n) h
) B9 _' [& w; t' [0 x+ m9 o0 r
|
|