|
|
; z5 T: E+ ]4 ]8 H
- #include <Arduino.h>
- O5 o- W* C" N. E# Y9 G - #include <MeAuriga.h>
* c1 H$ y6 S4 K# z - #include "MeLineFollowerArray.h"
4 B0 l. q/ u- v o' i s
' f+ K2 k. n+ b* m- MeEncoderOnBoard Encoder_1(SLOT1);
7 T w" a& T1 Q# L( p - MeEncoderOnBoard Encoder_2(SLOT2);/ y' }3 _; X* ]
- MeLightSensor lightsensor_1(12);
1 s. _' B6 Q2 {& k ~6 n - MeLightSensor lightsensor_2(11);
# V" v0 ^" {' A. _3 ] - MeBuzzer buzzer;
( W4 b* [) T ?+ q - MeLineFollowerArray linefollower(PORT_6);
4 N, j, l8 w. j; p2 N9 ] - 9 O9 f. x" |% ?# U! s: o8 `
- #define Error1 1
. @+ h! H: Q2 _5 ]% A - #define Error2 2
5 @4 l5 X; K7 {! X - #define Error3 35 R' v- s% [% h/ v
- ~# g1 k5 b# u+ g2 w, |2 M) j
- #define Kp 15+ p" ]9 J5 J* {" c1 ]5 z
- #define Ki 0.155 o+ k {2 [" `
- #define Kd 0.03
; H$ h9 E$ V, o' R - 0 N' I* ^& X" X& @
- uint8_t sData;& i8 B1 ~+ M- j3 d
- uint8_t D1;' C0 }( r* a/ t
- uint8_t D2;' k1 M3 g9 B- v" y) f) B9 P: \
- uint8_t D3;
9 B9 U) b$ u2 E, t - uint8_t D4;7 }5 t' Q% ~7 w) I; }! P6 d
- uint8_t D5;1 ?8 b0 N' `& A# O( H# [0 C# H
- uint8_t D6;0 t; p* ?1 [2 E W" S. { u& ~
- + O: X# I& Q' K* e; e8 k* Q9 `
- float previous_error = 0;+ S$ r3 G; p0 ]! Z4 p1 ~4 a
- float integral = 0;1 d4 [/ \+ q$ Q! [' X
- float derivative = 0;
+ ~6 e# D! H, K* S* R - int Speed = 160;) }3 t9 S) x9 n0 h+ l
- float output;
@9 P; i5 M! D { - 0 `, I& C5 k- u
- byte Left;
: V* j Z: ]+ H3 j. W3 M
5 r( u! h, f% K. }8 ^5 h7 P+ d- void setup()
6 W) g4 V+ t7 C/ R3 j" t! H - {
5 {0 s& Q A2 E( Y! K: w- @% t: V - //Set PWM 8KHz
0 Y% v$ [9 z$ A1 g1 F. [ - TCCR1A = _BV(WGM10);
; P L" Q: V d5 g - TCCR1B = _BV(CS11) | _BV(WGM12);( S) D# |+ ]& E, R! q* |1 W
- TCCR2A = _BV(WGM21) | _BV(WGM20);, P( V+ D6 a' f3 S4 G! {6 ^
- TCCR2B = _BV(CS21);
5 S4 |+ ?/ O5 S6 ^' K& n- f4 A2 h1 p5 l - Serial.begin(9600);5 ]/ t+ i) D! m O/ w9 N# L
- buzzer.setpin(45);$ F& {8 V6 z6 M7 e0 V
- }
# t. ^5 F) X) _ - 5 Q6 d0 G6 j3 T; r$ I( f& g* e
- void loop()
: o0 z2 C. k5 q - {6 C' o2 q- h4 s; M% J9 [
- while(!((lightsensor_1.read()) < (10)));- s2 c0 x) N) P6 L; O& B$ [
- buzzer.tone(1047, 500);% F2 V O m6 s1 [5 X$ A% \
- delay(500);
" w: U2 J4 a, m9 r4 E - do
- L/ r7 G$ D7 u0 s - {
2 t4 P9 F1 I: J$ Z1 w! n5 a3 l - int Err = getErr();
) Q8 ]5 T( Y8 } - if(D1 == 1)
, y' D3 z6 I0 P, w) q - {) G; f Z" T6 |/ h5 `$ k2 K1 C
- Left = 1;* }$ v+ V: J- N: E- c& q7 ?* }( F! {* j
- }& v/ ]1 l- b3 k2 j1 {
- if(D6 == 1)
2 V/ Q' ?& u$ F: t, `) C2 W - { f' B( ^- s; m0 } z% a3 i. P
- Left = 0;
1 P. B/ L! S& l+ x" `: i - }5 A2 T) O) r6 Z$ a1 x `, u
- if(Err == 99)
0 w% Q# o. V# n: c/ K. a - {
, P) s& e6 J3 g3 G1 V. T$ C - if(Left == 1)6 u+ W' k7 P* V( Q1 c7 R
- {8 j- v$ R4 r1 B1 G! n* i/ K
- Speed -= 5;
' o# X" G& i* h& J - moto(0,Speed);
% l: x& p/ C7 c$ \, A( \$ t, { - do' q5 l. I( c1 H+ ]& b4 X. G; Y
- {) q% I+ E/ }, U- `* E
- Err = getErr();
& f5 x: Z3 C _. y$ I* @+ M+ j - }while((D1+D6) == 0);
4 b1 K+ R4 E8 [, E - }
$ \! }7 Y; a, q T, \ d - else
0 H7 v5 l4 s( `# e0 t) T* N# Z7 M - {1 H0 N/ g/ l# x% Z
- Speed -= 5;% G- c, v# t% Z; O5 J
- moto(Speed,0);
! |- M0 B. |- y% y! a. C5 z - do
" G1 Y5 y9 ?2 W% F F - {( [7 L( m3 w4 d$ x
- Err = getErr();
$ M) F0 U+ E- R8 Y - }while((D1+D6) == 0);
$ e2 }1 q6 H4 | - }# F- L9 w1 g; b; L N* G
- }( e: E7 ]! V/ J3 V8 V8 ?" M
- else
5 r6 a/ {- r! h - {
* C/ G- X3 z3 G0 ^9 X - if((Speed < 160) & (Err < 2)) Speed+=1;
5 k. U" ]& {* R6 j! A+ ]% f+ J - if((Speed > 100) & (Err > 2)) Speed-=2; $ `* o) ~4 S5 @4 |8 O
- integral = integral + Err;3 j+ M2 [2 D. P0 d- S9 ]
- derivative = Err - previous_error;
; p; q9 c" r) u8 J# [- W - output = Kp*Err + Ki*integral + Kd*derivative;; ^; ]# [/ I6 u3 {( _, ]+ }- i
- moto(int(Speed-output),int(Speed+output));% {6 p$ T; _4 {; `; q
- previous_error = Err;7 A. ^3 p- ] U
- }
. u0 U7 x6 ]% L2 c/ T5 S - }while(!((lightsensor_2.read()) < (10)));
! e( O7 ?- m+ u& Z3 V" Y! j - moto(0,0);
! b, c7 I- a' B - delay(500);9 V! h$ a: a+ K: l4 A1 C9 X
- buzzer.tone(262, 500);6 ~/ q) R% P9 D0 b
- }* x6 h2 ^7 [4 d* M2 ^
- ; b3 G) f7 t7 |% j- I
- int getErr()% Y: d! K) D3 C7 I) n
- { * y- N7 y" O+ M2 G6 [
- sData = linefollower.getValue();
9 I) v0 b* \. ?8 R - D1 = ~(sData>>0)&1;
7 Q; s2 y. g! T, X7 L2 ~6 R1 k - D2 = ~(sData>>1)&1;
. N# d( x! a0 K - D3 = ~(sData>>2)&1;
) K- L6 o9 |" M% i4 q) Q( Y - D4 = ~(sData>>3)&1;
0 Z5 j' Y5 [ L' M7 ^* \7 _ - D5 = ~(sData>>4)&1;
0 ^- u, ]: X: k; a - D6 = ~(sData>>5)&1;8 z2 ]: T/ u" P( n$ w$ i7 |( J
- int downD = D1+D2+D3+D4+D5+D6;! f/ g8 c; K0 t: j# O
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
: z8 n, b7 x2 N* r1 o- H0 S - if(downD == 0)% @$ n: `* o6 F& |! ~
- {
& w, M0 _1 O/ c" N# n3 Q- o: ? - return 99;* [) v3 K1 N. i$ f' P- l& C; K
- }
& ?/ o, u/ Z5 x - else( k: \1 z6 `5 W C
- {
0 V8 E2 z/ ^ Z c" e$ ^ - return upD/downD;
$ [0 d6 m) t, e5 v2 p/ w# L - }
; o$ q( x: N9 G8 f4 k) C - }
+ c# k9 \8 s7 b \% y4 [2 Q7 R6 a
, A, C+ p8 M3 y& C- void moto(int Speed_L,int Speed_R)
" e9 }! }* o7 ~/ B - {. i1 j$ N' a P2 j
- Encoder_2.setMotorPwm(Speed_L);
$ ?; I9 W# K) F/ S P; t1 G - Encoder_1.setMotorPwm(-Speed_R);, A2 n" j/ p1 ]; U+ x
- }
複製代碼
* s/ X) A6 z3 R( q- k! dMeLineFollowerArray.cpp: ~$ W4 d% ?$ H, U+ ?' J" P
- #include "MeLineFollowerArray.h"2 \# U3 x! n2 Q% e; ~
4 S) q! K' ^* @& ^4 @1 c# K- #ifdef ME_PORT_DEFINED
. _4 ?5 Q9 d, n8 s8 e/ P - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)) K! s" @) d# Q; p/ f4 @- E
- {
7 x! D o6 ]1 i: @9 g - + i9 f, [6 E; d# J2 N2 N
- }
1 V {+ {; G9 o - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
, I+ l! X6 q, f+ L- o - {5 n* H2 e/ E# F$ m7 }7 d, r, l
- _DataPin = mePort[port].s2;
4 g4 U. R3 g* h8 Z9 v - pinMode(_DataPin, OUTPUT); 0 i* O( T t' |+ n4 B, i2 w6 ~% ]! H
- digitalWrite(_DataPin, HIGH);* B/ F% D( b& q5 ?4 w
- }
. A1 }) R5 S2 ^2 Y; q4 C - #else // ME_PORT_DEFINED. p9 P9 J$ S" G& d0 U: ]
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
" ^# _7 e/ G' S: C - {& B. C& r K* {, g
- _DataPin = pin;4 M' E. t0 E2 A2 {- e* `0 o2 \
- pinMode(_DataPin, OUTPUT);
% H; ^8 h6 H$ Y9 F; a; q) k - digitalWrite(_DataPin, HIGH);
$ x$ \) n0 T: K1 H8 \3 ?1 b - }/ g- r) D, N: }
- #endif // ME_PORT_DEFINED
9 b9 t# q8 Z% K1 }: d& \6 s
$ N4 [. s! _2 U" k- " Y3 z' T0 Y* F& M6 \
- void MeLineFollowerArray::setpin(uint8_t pin)' Q" I+ ~0 k6 z- \: p' e9 G7 }
- {; v7 o+ d. h4 M9 a, ]5 r* @/ C: v
- _DataPin = pin;' m9 V( n0 ~* a4 Y$ F8 y- `9 w
- pinMode(_DataPin, OUTPUT); ( D* @( N8 k( N
- digitalWrite(_DataPin, HIGH);
: l" r/ m# L6 ]4 u6 @4 ^8 Y+ v - * u2 j0 n9 u* z" I+ D
- #ifdef ME_PORT_DEFINED
0 y1 S4 Y; k, c/ r2 ` - s2 = pin;
5 R5 D" S( t$ E: p" t B8 r) g - #endif
; U( C. @: e. K - }
3 o9 I; i, k: Q) B# s1 m4 z! ?9 v - 7 ~" X) m' M6 Y- _6 ~! p
- uint8_t MeLineFollowerArray::getValue()
, `2 Q( d! m/ J% n) ~ - {
& R& c) J+ g6 Z: o - uint32_t LOW_level_read_time;, N! x' W$ c% H- Q6 Q% @
- uint32_t HIGH_level_read_time;
: w0 _) v: b4 V# a4 N/ y - uint32_t time_out_flag;& ]# S% [8 x% l) {1 i4 N/ A' M
- uint8_t Sensor_Data[3];; T3 ]% }0 |9 h d8 s$ l7 _
- static uint8_t old_data = 0xff;
" Z( w2 o j% H
4 D- Y' G [$ |. p9 O2 A7 `- pinMode(_DataPin, OUTPUT);
! a5 V; K' q; M6 Y5 R+ V. \& D - digitalWrite(_DataPin, LOW);
8 G/ X7 W r( r! r* V( `4 ^% d - delayMicroseconds(980);2 p; O! I0 ?% y) Y- F0 z
- digitalWrite(_DataPin, HIGH);
" Z; C/ p/ k4 Z! [4 @7 v: s - 7 Z, S( _* y, C4 O9 D
- pinMode(_DataPin, INPUT_PULLUP);0 v1 {' r- w! h d; P/ r
- delayMicroseconds(10);; ^ F# o6 E {/ U
" O- B- R/ I7 `. m; X- time_out_flag = millis();
* |9 J2 c* ?8 A" \# B6 S- p - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( w$ N2 H& B% }$ p- A
* _. w1 U* G4 Y) {! B- LOW_level_read_time = micros();: N) S" H* F' e' g
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 a9 ~& l/ e) o1 P" [: u - {
) h& j6 j) U/ |# @ - return 0xff;' u6 k1 W; X' R$ M* g
- }
9 h0 Q. D' @2 y- H: B8 V0 b
8 s+ G6 ^% O, d6 X" g- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 K' H, @3 `& r+ d8 U5 w
1 W3 t/ q% b$ [, l; }3 ~8 T- HIGH_level_read_time = micros();
1 R; ~: c, x' J* `" q$ E - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level& ~ b3 l4 g* c
- a6 [3 v" E8 B6 }! H- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out0 S: `6 ?' ~$ C4 [7 f; d
- {5 f2 |$ H3 ?5 ~( R7 |
- return 0xff;: v+ ~( @ V6 B' A* N5 {" A# ~
- }
Y o% j" @$ `( g$ t
% v8 \5 f m" t- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)). d4 Q$ I7 K' d1 k7 P
- {) ?3 B+ a" g8 {
- return 0xff;
5 a4 `# D- a4 U! J - }
& w9 G! d; A; p) [" S6 O - * ^7 f5 s$ A# s, A! m2 l: \& ^) Z
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ b/ ~9 h% _( h9 j" A. d- x
- LOW_level_read_time = micros();1 w' w, M O6 ]( W9 R8 [ c( g! U
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level/ \7 H2 M$ q( ]6 Z
- 7 u$ ~2 ^: L/ L9 r, [3 H7 a
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! I& B2 b, J1 S
- {- d& ]- b& ] U# y5 J
- return 0xff;
2 D* V& k0 v/ D; ~; M# u; H - }/ k! `* a2 q+ r2 ?8 ]" s
/ W- G# f% i. x9 b: O- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))2 I- h. Q* Y% K2 t# y" f
- {/ G- D$ v7 `/ p p% _- M2 A
- return 0xff;. O" r6 D5 o$ j# {% b
- }/ u0 Y% U- K& M9 j& Z9 f, b* k0 o
) M) Q* w, F0 _, ^/ g0 _7 ?- for(uint8_t k=0; k<3; k++)
0 l4 v9 C- C$ N) F/ h - {' x0 a/ s; f/ d1 J- M5 C4 i* X$ R
- Sensor_Data[k] = 0x00;
2 ]( u9 U+ w8 p# v# a - 7 p1 \. \; n, P- E3 @5 @: I
- for(uint8_t i=0;i<8;i++)
9 m) m: w/ E+ O3 F. L( p, p - {9 u! _& {- j& I
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level, N: l [# B% w# M/ C2 m
- HIGH_level_read_time = micros();
& M; X0 k. [; g" |' L$ Y - LOW_level_read_time = micros() - LOW_level_read_time;; h& X% g# V7 `: r% d( b. j
$ l* r& @$ t+ @' k% c7 M- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )2 {. w! V" O8 {6 l
- {, s, {+ k3 K& t$ Q* G. m
- return 0xff;9 e0 l# F/ y! J5 r4 q5 K7 u
- }1 y" s. Y& g7 s& n* S$ Z q
, R5 R: s) S6 W( l) U: @; E: V- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );2 P4 U; z5 h' W7 y0 t3 k
- LOW_level_read_time = micros();
9 j# q) c. K- G7 | - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level" e! r' w: U$ P
- 9 x& O p- t- T7 _9 }! H( w
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 14 {9 \2 n. y: X7 ]* }
- {
; E5 r2 o0 J& u, N& D2 y E" t - Sensor_Data[k] |= (0x80 >> i);
1 y7 g" D5 Z5 m, L2 L4 ` - }# b; p1 q, V: \
- else if(HIGH_level_read_time >= 100)3 g( Z7 B% b: a; f3 C! Q
- {
! a M7 Q. b0 ^' T6 @6 \( ?& V8 z+ L - return 0xff;$ u9 q ?( Y. n6 w# M" q
- } Q3 F& L' ?1 {. A1 K U
- ; m6 R6 G0 V- [5 v# I* G8 A* |
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
. ?% s3 w* ]) m. h, s. N5 p2 z+ s - {
- r4 T5 m. T" l% T# s - return 0xff;
; j' N# Z1 n# P( V$ C5 U - }
5 r; a% [1 e; ?( A( h6 ]! s8 y1 U - }
) a) O6 O$ {" O - }; d4 ~: h0 C+ U& I \3 [, H
- ( [ B$ J8 |5 y. n* M0 G
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level4 m- N) x9 R. o! A
- HIGH_level_read_time = micros();, X' [* K, b. r1 }2 v& ~
- LOW_level_read_time = micros() - LOW_level_read_time;
$ O+ S8 P! f$ \6 O - ; E6 |/ j/ k( u, r/ `
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )# J, ]) a9 [$ F" {6 H1 l+ I8 b* E
- {
2 u) f5 u) h; j( [) a' ~ - return 0xff;
; z; J) O' s* I4 R ?1 ?$ u! r - }
! t" j% _7 O5 e U - ; S* y( E) `/ C
- pinMode(_DataPin, OUTPUT);
' n M- ~: @4 l) B% \: D - digitalWrite(_DataPin, HIGH);7 a4 T; Y: G6 T* j, O3 `
! @* ^2 b* P+ K) W. E6 e9 \- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))9 e( Q# R- O0 b; n& D! y. s3 O
- {
* D/ `) X9 w( C/ q. A0 Z - old_data = Sensor_Data[0];
7 K5 Z% d, x9 Y* ` - return Sensor_Data[0];- Z) n) @4 l, y7 W
- }' R; t, r; D6 T$ A3 O6 }( |% b
- else: Q5 H, m" q. u( i% {
- {( Q2 Z- c/ f8 W. m% p6 Z0 `, T% z
- return old_data;+ ?" w, J5 g" N9 S: m
- }
) C" h, Z: v- F3 O+ o6 Z - }
: I: ^6 _. U- m0 Z, L3 t
複製代碼
( K' u- \/ m: MMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
3 t: y m/ x: S' i6 Y) M0 w
% G+ D l) o5 a$ k5 w, }
|
|