|
|
8 N9 e- g& `, v `' n6 ]1 Z
- #include <Arduino.h>5 V2 s; O' v5 [6 j0 ?
- #include <MeAuriga.h>
1 u# M: ^( ?+ W7 i% [ - #include "MeLineFollowerArray.h"
7 ?* N: ?- g Z$ J+ U1 J F - $ m, Y$ k, t3 H! d& v! {
- MeEncoderOnBoard Encoder_1(SLOT1);
/ O6 X5 _) C, w - MeEncoderOnBoard Encoder_2(SLOT2);
) P* U( I, x/ N: v2 Z/ G. Y - MeLightSensor lightsensor_1(12);0 q% C; G& V& R
- MeLightSensor lightsensor_2(11); s; l( B/ z R$ x8 K4 H2 e
- MeBuzzer buzzer;
p2 }! N0 u O! K. d2 e) t8 G6 ^ - MeLineFollowerArray linefollower(PORT_6);
6 w. o4 w7 d3 b$ k# k @) Z
Z! z: P: Q1 i3 o2 W9 z- #define Error1 1! u9 d1 c( F* ]5 Q, k5 a
- #define Error2 2
9 Z+ b# C( j+ B8 u0 j) d$ H - #define Error3 3; ^9 _$ E: F; s) {. ^9 Y% k
- 4 j {$ C+ i4 j3 X, b# m- `- y$ Q" u
- #define Kp 15' d4 L( C! S) B5 L7 ` ]
- #define Ki 0.15) l6 z% V+ v$ e5 ]. ~9 n$ U
- #define Kd 0.039 c5 N4 @3 Z: E+ S# U
* g! y% z4 @$ M4 B+ l& x- uint8_t sData;
- ~1 y. Y) [4 z; | - uint8_t D1;
* F; t' I8 @" e* H3 W - uint8_t D2;
: K b8 M% ]9 B - uint8_t D3;
0 t7 \; r4 v; U7 x - uint8_t D4;
: Y# `% I6 B# T - uint8_t D5;; f6 T1 K3 [# g; e
- uint8_t D6;+ ~0 p8 E4 P7 `$ g2 }. _$ R% r
* X4 e/ g7 z! @: P- j, B& x0 c% ^* C- float previous_error = 0;+ {; s( W) c2 [5 b% h+ E
- float integral = 0;
5 X( M8 m0 F3 Q5 v7 k - float derivative = 0;0 O7 t7 F2 D: T& f
- int Speed = 160;
7 U% n& U% e0 N7 N9 W7 V - float output;+ F8 v& u; f7 Z1 z( O6 `( c6 x
0 Y X$ { `! }8 ~! k1 Q6 ?- byte Left;
" {2 G2 o7 _% l( t' t - 3 u; q* g1 v, j2 J5 ~- e$ K$ L
- void setup() 8 C* ^: N9 R- j: l8 z. Z% J2 n
- {
8 X! ]7 p. T; }' L' j; {4 b - //Set PWM 8KHz
0 ?+ G% b# d1 \- | - TCCR1A = _BV(WGM10);
' `7 G4 j" s; i3 h8 c' R - TCCR1B = _BV(CS11) | _BV(WGM12);
9 k" N! X8 f; \ p8 ] - TCCR2A = _BV(WGM21) | _BV(WGM20);4 r' c5 d0 y" L: O5 d. |
- TCCR2B = _BV(CS21);
7 v/ l- Y, Q# Y) x - Serial.begin(9600);
g7 \) u/ K# f - buzzer.setpin(45);
# @/ C1 b6 E7 r: {: L9 e - }
7 L0 h7 E: u0 _" l9 K8 f$ _ f% y" | - ( n; X; K/ p L1 n% |* E$ \
- void loop()
/ M5 m: N, `( m* @" o! v - {3 Q7 M0 q1 |4 V3 d( |5 t" o7 ^
- while(!((lightsensor_1.read()) < (10)));
3 X* E: b3 o* x" O" F, F" G" T% { - buzzer.tone(1047, 500);- n+ P/ y% @& Y$ k0 k" w
- delay(500);
* U. p% l% H0 m4 s% O* J - do
) z- D' K. q$ ^5 `" H5 @" a3 o - {3 S7 X) Y# }" C- _, u! F! n: ~1 F
- int Err = getErr();) J8 W5 H* ?7 Y+ N
- if(D1 == 1)
* T* W* Z' t& B: g* B - {
A) u! x( x. J5 Q* ~ - Left = 1;- ]8 R" v4 D4 p" X
- }
, P' U8 o# a a+ l3 p) N: y! } - if(D6 == 1)
4 O {6 f( F) k' m0 q( _3 ?; M - {, z7 M+ u8 R. o |: e& @; Q- `& d0 ?
- Left = 0;0 a# m; N4 W/ P6 C; r4 J
- }4 ^( C9 u$ A- D. w* d/ C
- if(Err == 99)
4 s2 ]3 H- H2 [ - {
+ g7 u: F) L0 x6 Q9 w - if(Left == 1)
/ M" O. n" H, a: r( N: `0 ` - {8 x' R* o4 \% o
- Speed -= 5;6 u% H* I) j8 U" p' \7 ^
- moto(0,Speed);. g* F% U; J% J5 ?; u8 o6 {
- do! P4 G- }% T, u0 t& j: A7 N: ^ @( _
- {
8 `; d6 A; @( U - Err = getErr();) w) k2 l5 x" r0 n( f
- }while((D1+D6) == 0);
! @" Z$ ?3 P2 R - }' y% S! _% M; K& k% R: X- A8 F
- else1 N1 n, H2 o* z2 v$ }* m- o$ P
- {# f) m2 \: V. H/ ]2 q: r1 @
- Speed -= 5;
8 g" ^5 m' X) ~ - moto(Speed,0);
: ~9 B: I0 t: y8 X - do
* h5 D1 u: T, b6 C/ ^" ^ - {0 D1 q8 v# h/ {3 q- h, m: T" V
- Err = getErr();. v! M5 y# y; K3 e1 k: x' g
- }while((D1+D6) == 0);
% E" f2 C' H( G - }2 Z7 ^8 L# r7 N/ t6 l
- }
3 d* V, e5 m$ @& e( a. _" R# P - else0 y) i. e2 q/ W# }: _! W5 G
- {
6 S' a! ~6 d4 x* M0 ^5 u - if((Speed < 160) & (Err < 2)) Speed+=1;
" m3 @$ s0 f6 t( X+ P; L0 Y" Z6 c - if((Speed > 100) & (Err > 2)) Speed-=2; 0 J# {5 Q8 c: _% D
- integral = integral + Err; Y8 \3 J+ D+ |( V) j( W6 N
- derivative = Err - previous_error;: k/ r5 J2 u' S- P* g( F
- output = Kp*Err + Ki*integral + Kd*derivative;& @4 f. J$ M& ]( u4 t1 [
- moto(int(Speed-output),int(Speed+output));
( u9 X E P6 K6 e- ]" V9 ]1 u( N - previous_error = Err;
! D( @' F/ X* _% M: r4 W& E - }
; z j5 _' e) X, i/ r" ?! q' p - }while(!((lightsensor_2.read()) < (10)));( W9 I2 h+ E+ [# o/ K+ I) F- @
- moto(0,0); L! k5 Z1 P' K# [5 Z
- delay(500);1 k# d Y F* j4 b4 h, E( k0 E
- buzzer.tone(262, 500);
# k F9 g5 G- S$ P+ P+ H - }
! B+ N% r$ n( d5 K1 ? s
. w# \" C$ C3 D5 v2 h. _9 E- int getErr(), h1 F% Z( N7 c
- { - C- V4 P: l2 T e* X4 B
- sData = linefollower.getValue();7 W" F4 N) b) S5 O( E9 l. V. V
- D1 = ~(sData>>0)&1;, y8 b' _1 F$ ^# G8 C2 {8 D8 M# @
- D2 = ~(sData>>1)&1;
3 `/ y2 R1 x! B - D3 = ~(sData>>2)&1;' D ]$ Z: H7 ~$ \$ L, x4 g
- D4 = ~(sData>>3)&1;
7 ?0 @/ f+ ]+ C3 U& Q: S! [ - D5 = ~(sData>>4)&1;
( y/ E O' z" ]2 o) P" A: g - D6 = ~(sData>>5)&1;/ C, K- O) h5 {8 ` E" z
- int downD = D1+D2+D3+D4+D5+D6;( n# |4 |4 v9 ], G2 R& k
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
. z8 A) {1 k' ]# J+ J: C - if(downD == 0)
8 X4 a+ t7 j7 G: |9 g - {% D4 J1 r7 j# s: H8 [
- return 99;* _9 \" H; s/ p9 ^/ y. v# M
- }+ `" |. D$ y5 B3 F4 m$ f
- else
' q$ W7 p8 J" i- n; l; O( E; x - {
) V7 e; q1 ^( ~/ S3 m - return upD/downD;
- w2 D0 w3 o }/ x- q' f - }- U3 F2 O+ b- ~* }; ]/ T
- }8 J& W' U% A4 d! N3 J
- $ E7 c$ v; ?, N! ?
- void moto(int Speed_L,int Speed_R)
1 b2 ], m% n# v/ P6 m% u. {+ [ - {
4 N; S- P( |9 K! J7 I8 U - Encoder_2.setMotorPwm(Speed_L);
% ]1 o0 ?& x: ]; J% P - Encoder_1.setMotorPwm(-Speed_R);) m% j& `, Q7 L( M: U
- }
複製代碼
3 ]( v/ X+ H& ?5 `) d4 r! G$ NMeLineFollowerArray.cpp
- u! z2 C5 H- \8 c& q- #include "MeLineFollowerArray.h"
& N+ I$ @( b D$ X, }1 Q' @ - ; O9 [2 V& |/ w# X" s; b( g
- #ifdef ME_PORT_DEFINED
; d3 i) x1 q2 L# A" Z - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
& z' c4 m$ r; k0 \( g7 x# {3 H - {! \- ^2 @2 ^- T' P v
- 1 y6 n+ @% _4 M, y
- }
- r7 ~- ?. d: Z3 W4 H - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)( z X( D4 c$ \6 i# [
- {
5 l2 Z9 ]1 r- ~7 f. t& ^) r" A - _DataPin = mePort[port].s2;
% O H) m' _/ x( ` - pinMode(_DataPin, OUTPUT); s0 S: `/ U4 Z" H$ A. ~* ~3 N
- digitalWrite(_DataPin, HIGH);$ R# B; ]( C ^6 H. G
- }/ v% \2 H$ t) E) v
- #else // ME_PORT_DEFINED) I8 I. H ^2 H' |2 R7 |
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
$ f# S1 ?: F V* A8 {" a6 d) F. p - {/ U! V F+ O, b9 b1 B" @: P% `& U
- _DataPin = pin;) V3 N* W7 [6 [" x; G$ h8 B+ L
- pinMode(_DataPin, OUTPUT);
' v5 ]4 ~+ T5 R" g- B - digitalWrite(_DataPin, HIGH);/ b( V9 o1 T" ]# V5 G7 }3 |4 v
- }& y5 L6 s6 ]$ T0 B
- #endif // ME_PORT_DEFINED
* d* c U0 a& Q7 r - , d. P) X7 M, m) S6 g7 e* I
- 0 A2 f; e- L3 u6 I8 l1 S1 \
- void MeLineFollowerArray::setpin(uint8_t pin). |+ p ^& m3 b* l
- {
, I! H) {5 J. @+ y0 {9 c7 v - _DataPin = pin; o$ n! x$ x1 W& A8 ^! k
- pinMode(_DataPin, OUTPUT);
7 y% ?. e# M$ l/ u3 ^( a, r3 X - digitalWrite(_DataPin, HIGH);( J% ^# |: C: k; d6 {
- " t6 g7 n8 n- j8 M- O% \! Q3 c
- #ifdef ME_PORT_DEFINED6 _2 ^; ?$ Y* l0 E) H
- s2 = pin;
) n5 o* F( N! i2 e+ h+ J2 b - #endif- Z( @8 w! G" n% b
- }
5 M) Q# y3 k3 B p0 j6 Z - b3 O# M5 b: |, V) W7 o3 n9 B0 R
- uint8_t MeLineFollowerArray::getValue()
2 }0 _0 M2 c8 ]2 `4 `+ Z5 l0 B - {
) T Y2 E3 r) b$ F/ @! c - uint32_t LOW_level_read_time;
; V+ b6 m' J1 e$ }: E - uint32_t HIGH_level_read_time;
- M) l' B' S$ G5 P0 I. j/ w! Z - uint32_t time_out_flag;
9 I, [6 V5 V& \0 @, a9 i$ B - uint8_t Sensor_Data[3];
$ v0 R1 Z2 _5 P7 K8 w& X - static uint8_t old_data = 0xff;
& l% c4 P6 u# d8 n" I- q( ~
/ }& K: F& ^! _% Q0 @1 ?- pinMode(_DataPin, OUTPUT);& F) Q/ {0 O# F! {" L* P# c. ^
- digitalWrite(_DataPin, LOW);
( F( R* [2 L& G3 x: _ - delayMicroseconds(980);1 W, f9 q4 u# g! O7 k
- digitalWrite(_DataPin, HIGH);
" n5 [8 Y1 m: i% Y; Z; f5 U
, ^/ k: J0 Q4 X- pinMode(_DataPin, INPUT_PULLUP);
+ h8 K( b! Y- {' u6 B - delayMicroseconds(10);+ x: G0 n' @# {8 H
z A: N" l) X h: f5 l0 f g) C- time_out_flag = millis();9 K* Q8 y: L$ b |" z: i, l
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ L+ z+ a1 b# k* U2 k - $ W7 B4 ^( |5 A( l
- LOW_level_read_time = micros();
; j" _' n5 o0 y1 q: b) S- ^ - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out, c/ Q' o& C- ^0 `1 E% ^; {; I
- {3 g: r$ |5 G5 }' _+ O, L! \; |1 x
- return 0xff;
1 H8 [6 G0 ]$ Q1 N' p - }4 t4 l4 h% r' Q) \* G3 b5 K7 z
- ! ~1 F% j+ J9 y6 \+ w
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );! L3 t* N# } w- I" J
: q9 v- M0 H5 o& I! a5 B: ]! Z1 U# J. U- HIGH_level_read_time = micros();/ Z' F+ |6 r" z& U1 C
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level+ t! Q$ ~" p' n# |$ ^
- , x6 L7 V, V6 b# L# q7 k2 S
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 F; w: \! k; u0 e3 t$ k; a
- {
! o" I0 O3 s& q" _0 O - return 0xff;
* V$ x# o* ~* J& A# T8 `) v - }2 L ~" ]( [) h1 U
9 h2 K+ _5 ~: D- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
: {+ q" ~1 ^" n - {, s5 o6 f* F- b* B5 R
- return 0xff;
: [; O& K/ Z7 T1 g, [4 } - }! c7 D+ n F4 V4 Q
# o% K+ e" ^/ U0 y- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
* y0 a6 V! w& E! [ - LOW_level_read_time = micros();
( h& Q' \' H2 o5 Y1 @ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level, Z6 a0 A6 [* J, c" Q+ n1 w
. [+ T/ S. A( I, X: \- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
# D# b; _! T0 G, N3 f+ w - {. W/ |5 q8 c$ w0 |1 C- o8 G, w3 r
- return 0xff;
3 w& f( N' ~7 s& r" y5 i- } - }7 x5 g- X/ r8 B3 ]; S$ X* q) s0 a
- 6 X% A# e+ I: _# l
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
0 U8 w- ^3 ]& _/ }) I, C E - {0 Y7 \3 |: ?" I8 G7 e
- return 0xff;
X& l2 ], D. o" }' n - }2 L9 @/ G. J8 r- g& ?6 z
- ; y0 h3 D( {" b2 u4 F% w
- for(uint8_t k=0; k<3; k++)+ U8 f& |& X/ x/ O4 q
- {
" h+ v( ]) s! x+ i" } Z& L" s - Sensor_Data[k] = 0x00;
" @0 M: d; h1 @7 ]( \& o
/ u* e# g: s& \$ v- for(uint8_t i=0;i<8;i++)( k* s4 J/ d& V' M9 V1 f* |
- {
y9 r; [- m- C4 } - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level4 ^ x1 ~) h' ?' `1 S) h% Z
- HIGH_level_read_time = micros();
8 g" i9 I( M/ Y& ] - LOW_level_read_time = micros() - LOW_level_read_time;
3 H# V& D* k' F% d K
& C" C! M* D; I- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
. L" p" X! c& V0 v" T! {# a - {7 k5 W/ B: K G! o2 J$ G1 S
- return 0xff;7 X( U& z: A; n7 H( V) L
- }
# P2 e* j6 h0 p* s {+ v& Z6 ?3 N1 y - * O: I6 o# I. S! Z$ x
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- E3 i K% F' }) y - LOW_level_read_time = micros();7 t6 k! \* i: G3 M. b: l
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level6 ?. ?9 u: D$ c# P
* p( i2 I" C+ F- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1# Y$ \) K5 {+ d1 B- C1 e6 g
- {% V" U" E9 O/ f' B0 _$ X
- Sensor_Data[k] |= (0x80 >> i);
9 q, m7 G# j4 c. e' G - }5 T& }) @% v% {0 J: F
- else if(HIGH_level_read_time >= 100)2 f: t2 R- k/ Q. C9 Z" P5 H
- {
2 e; k( _* j& D6 o - return 0xff;. j0 D+ T# Z* ^! e
- }
& |1 H2 | U) B. [* C - ! v4 k+ P8 G) C- t; G4 V$ o6 [
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
% S( k# p" ~$ v4 C7 M8 [ - {1 s8 G9 {+ ^3 V. B9 p6 A1 S7 @
- return 0xff;8 m2 @, _/ k3 w4 x5 a* g
- }
4 {9 O) h$ c+ _: ?' d2 j - }
, P. Y& F: i! M+ I - }
) D4 o/ w6 b, q0 w- J; B8 s - 6 R1 O! K! V4 Y* R; t5 x
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
$ ? N& z" j* l* d4 v - HIGH_level_read_time = micros();8 \: D( V$ s0 d. ]+ O
- LOW_level_read_time = micros() - LOW_level_read_time;
( s# P& I4 K; A( i - ( o+ m) S, \! M- ]* W' \
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )/ |& e. O2 \" C9 d6 ?% L" y
- {
& [' e% r7 n' T# G - return 0xff;, D; |- k0 @% ~- h
- }; P/ p; w9 N* M& W9 D
- % k1 n4 E/ M8 E. \2 z: m
- pinMode(_DataPin, OUTPUT);; D* B. j; a; ~
- digitalWrite(_DataPin, HIGH);* r( Y/ G& f' }9 W9 L7 W& M; i, w
- * e7 _ T: F9 s. A m3 H
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))$ x O; a# F6 d7 L
- {
9 ]3 `$ e! m ^- s/ u4 ` - old_data = Sensor_Data[0];9 ~" e3 ~) {' b7 e
- return Sensor_Data[0];
5 C8 Y+ N; ?1 h* `9 _ - } E) z# ]) Z- Z4 [& Y' C
- else2 l4 n1 o4 l# z' z, A- |. r
- {
}+ x# [1 M. d( I' B8 b - return old_data;8 f+ v/ U( K! g1 h( j
- }
7 A" l( W/ [- [9 B - }
3 }& b c! Y2 K
複製代碼
; { a6 F- H# [( FMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
9 c6 |; \$ E1 l) p e5 x5 Y1 I* j3 j7 e
|
|