|
|
) m# k. b( l6 `/ W8 ^0 N- #include <Arduino.h>
l0 ~8 z. P2 b; Q1 D - #include <MeAuriga.h>
: o8 u# x& L1 e! P3 ~/ j - #include "MeLineFollowerArray.h"
0 w% \3 v9 k S; P! A' u& Q3 b/ t* m - ; i: b3 [" P3 |1 a
- MeEncoderOnBoard Encoder_1(SLOT1);5 c* X! t- ?# r
- MeEncoderOnBoard Encoder_2(SLOT2);! g C% l. O/ \/ O1 v
- MeLightSensor lightsensor_1(12);* \1 m2 J) m( ^6 _5 N
- MeLightSensor lightsensor_2(11);
* H; Y, R+ K- `7 D* ]( V - MeBuzzer buzzer;! ?6 O+ Y! }0 l" }8 [) Y
- MeLineFollowerArray linefollower(PORT_6);+ [, Z2 o5 u5 a7 m' p$ S% w
- % D5 B) u T! ~+ I
- #define Error1 1
6 l) r. [# r6 f) V+ a! r" J - #define Error2 2
( x' \! @& ?, w: r- n - #define Error3 3
/ \' O: c& d" H' M- V - 3 x, J9 M9 m) ^7 J
- #define Kp 15" I1 a9 y5 m8 S# Y8 i, K
- #define Ki 0.158 Q& K' x$ a* a
- #define Kd 0.03% L" ]' j; Z" u: [- v; d9 \4 G
' Z# a, `, \$ P0 W- uint8_t sData;
5 l7 g7 D2 u# v. d) G( ~ - uint8_t D1;
2 l M( w2 H! `) I - uint8_t D2;3 L, q$ i1 s( i- N0 y
- uint8_t D3;4 g& L+ ]. E: [, e* b
- uint8_t D4;
( K, e, N% w) X - uint8_t D5;
" j, |! D$ b8 _* f - uint8_t D6;+ _; m, W# e; W b: V4 F! k- P/ Y
- 6 Z' y* i# l6 c3 \! @1 [. q
- float previous_error = 0;; c, B2 ]) T3 ]/ [
- float integral = 0;2 {' I4 t# |, ~& w" I) }
- float derivative = 0;
" Y, v/ G" e, J5 z* u - int Speed = 160;3 @( L }# Q# Q) V. k$ g
- float output;
7 @- N+ P3 p: M7 d6 R3 l
& S- Y' v+ j5 [' J- byte Left;; I9 k9 B( b$ d1 |& D
+ [& L7 U4 P' p# a7 G! t- void setup()
6 d9 P* l0 o# f$ C - {
* o" d6 T. z1 P& `0 w, T0 U( t; t! @ - //Set PWM 8KHz
# X! d: X5 n3 }9 _1 k V - TCCR1A = _BV(WGM10);/ r" d V# V9 v5 y# R s2 z
- TCCR1B = _BV(CS11) | _BV(WGM12);& h9 Z4 @* D w0 q6 y: |
- TCCR2A = _BV(WGM21) | _BV(WGM20);
2 o3 c4 s' F& y' o - TCCR2B = _BV(CS21);1 x* k% v @, c3 y( R. Y: o
- Serial.begin(9600);
; P4 n+ ^ u9 D: C- f2 n - buzzer.setpin(45);: E- x. B$ r6 \1 V: C
- }
4 k) S1 M: q9 o& T7 x7 c! f( D - ; {7 F5 Z" f( ]
- void loop()
/ z( _5 j/ i2 V% T) g! h& _ - {( C8 c- J8 n0 y) p
- while(!((lightsensor_1.read()) < (10)));
' J% h& O9 m" G! D# E! F' j - buzzer.tone(1047, 500);
% X5 b' U- d l4 O1 j& I - delay(500);6 K: ^, Y; O+ U3 m4 e3 \" N$ R% o
- do6 t) l) D! {1 P2 v/ [. W/ n
- {
+ z9 ]. m5 J- B2 B - int Err = getErr();& g1 t$ t. j" Q# \4 ~2 ^9 `) Y( t
- if(D1 == 1)2 d; X9 H/ f' g. g6 c1 a8 w7 D
- {4 I X- i4 w5 c5 H7 p
- Left = 1;! R% T1 t) c' _( S+ m4 J3 n
- }5 u' U$ E6 j2 M" V
- if(D6 == 1)
' P: C0 R( A2 B; @ - {
' `% q* [% O8 ]& s - Left = 0;
$ G& ^6 f5 Q/ o2 V- A5 { - }
. @) `- J! G/ p6 Q- E* j( C - if(Err == 99)( ]% |1 e( [+ ^1 Z+ V2 G
- {
0 A8 L' O6 R1 S( Y - if(Left == 1)
( Z/ M1 R0 [' }( ~5 _" G - {
; K3 I" e& |6 d- a! k4 V - Speed -= 5;
0 n* L) W; m$ I/ S - moto(0,Speed);
/ p7 L8 }! C* y& D+ U9 O! k& V - do/ h+ {% K+ o- C5 |4 `7 L/ q' M
- {4 W2 a/ a2 e, B; m0 s+ |% p) L9 i
- Err = getErr();3 j& t2 R' E" q; C( \: c; d5 }6 S
- }while((D1+D6) == 0);
% d1 ^ F7 F$ o) t7 V! W - }3 v+ @3 A! n5 E% _
- else+ m- @9 f! S7 Z' P
- {
0 \) K( I. z2 }/ [ - Speed -= 5;* j3 Y, @- j u7 k* E# M2 d
- moto(Speed,0);
8 `% w& f. _2 q - do7 |& ~2 z) j3 f
- { v& n3 s" d2 G+ g$ k* U
- Err = getErr();
* |' K- X! s1 p6 Y! E - }while((D1+D6) == 0);
4 V' \) _& h( W8 c3 E+ ? - }
- b+ @# w I b X6 f' o2 G- W - }
1 {. m' e6 n# e+ G, w5 H( M M6 G$ l4 ^1 m - else1 I* e t$ }. @
- {* n" e8 \: n9 |* U. U, T, p" X
- if((Speed < 160) & (Err < 2)) Speed+=1; ! S! N3 L- R; q
- if((Speed > 100) & (Err > 2)) Speed-=2; 0 X8 {% t& k# h4 N
- integral = integral + Err;
" {% q+ Q3 B& d; ]4 I7 Q - derivative = Err - previous_error;7 w; [, i: k! |1 t
- output = Kp*Err + Ki*integral + Kd*derivative;" A5 g8 K @; |: A
- moto(int(Speed-output),int(Speed+output));" Q1 w2 H( [8 k5 t$ H
- previous_error = Err;
9 L" P) ~. r; q - }
1 p3 ]9 G% Y+ Y+ R - }while(!((lightsensor_2.read()) < (10)));
+ p0 ?; r' `4 q: B2 p - moto(0,0);
' @: f+ v# |" n) W' o* | - delay(500);' k4 e% w* d7 k/ {3 T* f
- buzzer.tone(262, 500);
: R& C- G2 i- C9 k - }+ J4 K* ~# e& E4 g
- & [ b6 U4 M) `1 C" e1 S6 o6 T
- int getErr()# W+ u% J$ v3 P$ w `
- {
6 ?: W/ u; ~1 x# v - sData = linefollower.getValue();
d9 R+ o9 a1 u - D1 = ~(sData>>0)&1;. u+ \7 _6 I% @
- D2 = ~(sData>>1)&1;
& v1 V+ K8 \7 F) X9 p$ t3 s- o6 S - D3 = ~(sData>>2)&1;
+ y( j& w7 W% s5 g3 }; e% F - D4 = ~(sData>>3)&1;9 i% n" c! m& M1 [' ^/ A" u; w% L
- D5 = ~(sData>>4)&1;- I% |: B; Q0 B+ |
- D6 = ~(sData>>5)&1;* \3 c8 t9 c8 i o- I1 Z# @8 y
- int downD = D1+D2+D3+D4+D5+D6;7 D! `' Z* ~- {2 _
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
U: e4 I" q' ?$ ? - if(downD == 0)
/ J: g# ~8 t: _9 _# ]( L - {8 _ U- ?/ t9 b& l5 H
- return 99;
# m3 M" ]9 t; r' U+ W8 {. Z3 ]0 v - }2 I% [- V- l7 E+ y) q- _
- else. z: M8 h8 L( }. d
- {
0 ~" Z0 e( H% n! o# l$ O7 r* ]8 w& [ - return upD/downD;% q1 F8 b+ q8 U4 H
- }
" W" u2 Y$ t { ]1 j2 k7 v - }
8 B/ e* |- }- f5 M7 _; ` - 2 D8 c4 E, Z+ Q% E4 \1 b' S# x3 `8 j
- void moto(int Speed_L,int Speed_R)
* `1 n! A* o$ ?/ I/ K - {
' N3 I9 \. D4 Y, i+ _6 l - Encoder_2.setMotorPwm(Speed_L);4 r; @. h2 z7 w# k0 \
- Encoder_1.setMotorPwm(-Speed_R);( o1 C$ F0 W& p: L8 S
- }
複製代碼 ( x- I" h7 L. l+ b8 |( N4 I
MeLineFollowerArray.cpp
8 q! v% y: p. V7 M0 [- #include "MeLineFollowerArray.h"1 R3 G. d! H) _0 C. y* D) ~0 b+ _
- 6 e! e- D- i+ }1 x2 O7 a
- #ifdef ME_PORT_DEFINED
2 b- y) H( C, }$ ~( u7 f2 ` - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)( D0 W$ S) |9 ]: ?
- {6 K" {- o- l. y/ Y6 b6 J1 ^
1 |2 k6 ~1 X q+ C6 G- }" j3 l) u: |. C3 n" ~& r
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
' d: L: d1 N0 ~9 B; Z - {
% [, ` s! w+ N2 u- b& v, [ - _DataPin = mePort[port].s2;# y) D- P+ Y: i/ A
- pinMode(_DataPin, OUTPUT); $ o! K" M1 K! C
- digitalWrite(_DataPin, HIGH);4 y( m1 v, q( @
- }
3 H: J. }6 b9 W# B, U2 i - #else // ME_PORT_DEFINED$ t7 u9 m8 T+ Q7 @$ D+ s) N% t2 Y
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin) }# b5 G9 Y# s
- {
4 p. e$ P- o$ M* v - _DataPin = pin;. J0 D7 t! `& u, _& } n
- pinMode(_DataPin, OUTPUT);
% A; n8 v/ E+ A2 h4 X - digitalWrite(_DataPin, HIGH);
0 p4 L3 {6 y: p - }* _1 r/ X2 h. @. b0 y; [. t* M
- #endif // ME_PORT_DEFINED
. `- P5 e, f8 w - , U6 m# Z0 f+ P/ _& L/ |
6 B) H4 A2 w, o( k+ Y- void MeLineFollowerArray::setpin(uint8_t pin)0 w, U+ w& a2 |
- {% S6 s. @8 `8 Q8 x! T" P
- _DataPin = pin;. B' C. K7 V% c0 I$ B% i
- pinMode(_DataPin, OUTPUT);
+ ^4 y; x K8 u) H7 T - digitalWrite(_DataPin, HIGH);( _, i4 V( I8 a% |
- Z" U/ \% i( L& m0 h' }9 M- #ifdef ME_PORT_DEFINED
' ^' U1 k( f6 V, K# K - s2 = pin;& _2 R8 P0 w4 u; f5 E6 [4 A) [
- #endif% `) H/ \4 L' H* {4 ~$ W) w$ l( [1 K
- }
& X' {3 V9 q1 u, f) U - * b" k9 f( j' p3 V9 K. ?
- uint8_t MeLineFollowerArray::getValue(): P! X0 `* e3 r0 M3 `
- {
, H4 t5 P+ Y$ `1 N3 r* _0 r9 X - uint32_t LOW_level_read_time;
0 O. S* N. V5 o; f- e" j/ ~ - uint32_t HIGH_level_read_time;7 U; q9 E# y" b! ^+ g9 x8 }. y
- uint32_t time_out_flag;
; f# d4 R, ~4 q* v' t+ Y - uint8_t Sensor_Data[3];2 j% y" Y2 E) J% z" b4 Z) h6 }
- static uint8_t old_data = 0xff;
2 c% i3 c0 ]' D6 d! C: q6 O - ! c+ n( t" n2 j0 [
- pinMode(_DataPin, OUTPUT);
' q0 O, ^* `9 E# s. q/ a' c) L- ` - digitalWrite(_DataPin, LOW);
0 u: H/ g. I/ d: C v( z8 ]) g) \ - delayMicroseconds(980);, B# G. r* S3 {6 M
- digitalWrite(_DataPin, HIGH);
/ G6 Q3 \6 L4 P/ ?" c - $ j9 [( I x- z- E
- pinMode(_DataPin, INPUT_PULLUP);
6 `/ V/ m; B0 d1 G6 i+ Y - delayMicroseconds(10);9 L) {# _$ @) G) M4 S0 l! Q& R
- 4 [1 b7 K* p: r0 N/ z" G4 M' ~
- time_out_flag = millis(); s8 g. E5 n3 p" e' ]
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* p; m3 s* @3 ?, O+ ?- ]
- , q' ]* `0 d! ^9 I7 H5 J0 g- {
- LOW_level_read_time = micros();
" D8 t3 |! @* x, c/ }3 r - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
w" V* a4 C. r# X - {1 q# F* j: z, d% R6 T
- return 0xff;" C9 u$ O5 s" Q8 J% {7 I
- }4 \. N) m/ g( } r- }6 U; ?
- _' y. m" ]% U- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
8 G% J3 M+ F' b9 K/ j - 7 n2 B* D$ V$ d9 ~0 [3 U; B
- HIGH_level_read_time = micros();
" |/ R. K0 Z9 m* w- j/ ]9 W# ^ - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level) f" v/ p# I( u$ d- O
- 9 b( Y1 w6 I" a$ u! F6 Y
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out' q" z( ]& W2 s/ ?$ u7 _9 d
- {" o7 b8 w. O; o
- return 0xff;& F) e. X5 I. V9 C {, u
- }
) K5 h* s1 _+ H( [' G9 ?
! V* t9 x" K& e6 M q- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))( i3 A# [" W' k4 X
- {7 b4 Y. x1 z* h& R, {& G7 i
- return 0xff;
' }! j8 Y& l$ P - }$ c1 H- N( Z9 c/ \
% ?, n; D) O: S n* M: i# w- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 v3 m% u3 B `0 [' s% J S
- LOW_level_read_time = micros(); B& h4 m6 |# m# h
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
( l0 P$ J& G2 S! a4 u
, M' a" u" X9 A; B) R- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out: E) W( Q, ]7 u# ]: P9 g6 y# I
- {
1 {5 [3 \3 {; ?) n. U: W - return 0xff;1 { R: e N( q9 l7 c
- }
' p" m/ L- ^4 ?8 N1 g - % a4 f& U' A C7 @& C7 U5 B
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)): F; I2 i3 T* L } ~
- {+ b" G8 j* i3 W R" q$ Q2 ]
- return 0xff;. E" h5 L0 V! N3 F1 z
- }* [3 ?6 ]" J6 a; h; e4 s) u+ E6 l
- 7 w3 W1 J5 R$ x) f+ h0 ^
- for(uint8_t k=0; k<3; k++)& `. k3 ~$ B) q @
- {8 `3 z5 w2 }, Q4 `, p
- Sensor_Data[k] = 0x00;! H, ^7 W0 P5 ]
0 y# o( R( F2 Z4 ]8 a. Y. |- for(uint8_t i=0;i<8;i++)3 }9 @# n$ a5 e6 ^* c- l0 p3 B
- {
2 g. _, }& S! R2 b" J5 S. ?+ A5 N1 N - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
# k) Y) u5 [0 `- M z. j - HIGH_level_read_time = micros();
8 L4 C) `0 R* V& o4 R- x% c3 G - LOW_level_read_time = micros() - LOW_level_read_time;# |/ |+ Q5 }( G4 k; a; W
# w: C! p. y9 }2 y- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ). n5 B2 k1 K. N
- {1 c6 Y* U$ \! N5 ^
- return 0xff;
9 t6 H \" t; K - }
}2 p/ n# {4 O$ J% ]' k5 F - $ W9 `9 { x+ O+ q' G/ \0 b: h2 q/ D
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );& x% N6 B$ |4 j* H8 \2 V
- LOW_level_read_time = micros();2 q+ h$ c% J. G( B" Z3 c
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
! F M1 m L/ K% Y: M2 H
/ C: I J3 E0 I3 B3 H7 l$ I- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1% Z7 _$ H. B0 ?, N6 i% a1 }3 U1 F
- {' O8 o; n; t2 T+ ]. ^3 i+ v
- Sensor_Data[k] |= (0x80 >> i);6 {9 Q9 p( p e+ @2 ^" V
- }
) Q) u1 j; q" g4 q1 x9 W% a Q - else if(HIGH_level_read_time >= 100)
6 c% Q1 x T" z' V: z, q5 }0 f - {
1 R+ g* T- N6 H. T V- Z. a! f# p - return 0xff;4 `& O6 k( m( A7 ^* \
- }
2 @$ P; v0 U0 J
( E4 R8 C% y$ I' F/ B+ i- h- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)2 m; p, H8 S- S
- {
) d, T% F. \& k- O) D - return 0xff;
2 R# a, ~ g; e - }* \) b( d$ t( y, W
- }6 [$ s& J& @9 ?
- }
! s8 S7 ~$ Z9 m: x4 v" H
& _4 b! B y! s1 @. u- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level2 m* L2 r4 g4 Z; I7 i
- HIGH_level_read_time = micros();
$ @6 O& u, N7 v0 p( ?5 L - LOW_level_read_time = micros() - LOW_level_read_time;7 J; y/ o4 K a8 T' z, R1 j7 H
- # l- r1 W, j6 @. b- H/ R) c
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )4 v4 j# C) Z2 n3 ?& d7 J1 G
- {
: L0 F r \- d4 k2 D0 t6 \ - return 0xff;
0 [5 ?# g% }7 Q( Q - }
0 r$ J; f4 h/ p, Z+ |- {7 N - 0 m l `3 d/ V
- pinMode(_DataPin, OUTPUT);
& z3 L4 C' b4 F/ |) P - digitalWrite(_DataPin, HIGH);
0 I( M* z: J" j; ^, L q
* o/ M H% z. c6 T; o2 m- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
/ k ]7 g( j* o( I0 N# T - {+ k3 ?6 o% S, h5 c/ I
- old_data = Sensor_Data[0];" v9 Z% k( _; B p/ q; n9 e3 B0 E
- return Sensor_Data[0];0 |; x# M6 S+ |4 X1 M& [5 O+ W
- }! _) i# o0 t C6 C- O5 d- t) d
- else+ x( v% U6 j8 ]' b7 ^
- {/ `% {# x# t& z% P
- return old_data;
+ ]5 l6 P, P8 M& X - }' y" r4 f9 @; e( t7 z4 J1 I
- }: d3 }0 N3 O, D7 p
複製代碼 ) }+ c. F5 E8 e$ n0 a
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
4 F" J* l* G4 A5 A6 l
# M: A% S6 K2 m, G& g' P% P# x8 H
|
|