|
|
; q* {& ?. X: f8 k5 X
- #include <Arduino.h>
( n% [$ w4 n) ~/ c& \ - #include <MeAuriga.h>
8 l" m: G1 z2 a' K! r) a - #include "MeLineFollowerArray.h"' p) A+ D5 ]0 ~1 F! a+ j1 [
* X% D; r- \. D8 T- MeEncoderOnBoard Encoder_1(SLOT1);7 E* S2 \5 h5 X5 c w7 V% ?8 `
- MeEncoderOnBoard Encoder_2(SLOT2);6 Y9 C# L! g. p# z1 ]
- MeLightSensor lightsensor_1(12);& G2 e' ?+ O" o- q
- MeLightSensor lightsensor_2(11);4 }4 u4 `" z7 L8 T- R0 P. @& o8 q: Y
- MeBuzzer buzzer;% \! p5 ], @' y: j/ Z* q5 R
- MeLineFollowerArray linefollower(PORT_6);
) H# J' _2 [) j2 s6 x4 A
4 M+ }% v( D' B- #define Error1 11 c Y9 w2 t6 d2 U. Q
- #define Error2 2; Z. K y- H9 n y( }
- #define Error3 3( M; F6 x5 ]$ c/ u. D# l
- : T% o. J! Y" g5 N7 Z8 j
- #define Kp 151 }3 | U3 ?# K" g6 t/ Q' Z m4 Q) o. F
- #define Ki 0.15+ D. r8 B) j D7 _$ u9 g$ `8 w$ a
- #define Kd 0.037 D7 @# L2 X! T& r
- 6 A& m, y# d/ j
- uint8_t sData;1 m" t$ C' s( m0 _$ E
- uint8_t D1;+ X6 Z9 U0 y4 r. N9 k' x
- uint8_t D2;9 ~5 `+ d; b! F. N) A
- uint8_t D3; G& b0 S5 ?1 _) H2 s' N) G9 A
- uint8_t D4;
* ]# B$ W, _, s! D, r- R, Z2 N+ d - uint8_t D5;
n4 _" q+ `# {* L% s; Z - uint8_t D6;
% Q% c6 b' Q8 S0 u8 K; u: q - ; P, O* g5 M' \
- float previous_error = 0;3 c2 m! J8 n9 h# z3 n( I
- float integral = 0;
) D1 T5 x+ k) q/ C2 o, V - float derivative = 0;+ f! @0 f# r6 G# ?: v! a
- int Speed = 160;
+ f: O/ K- l* v4 r8 b - float output;: A4 _- ]6 ^+ Q$ W9 ^* u- X$ A" T
& I% \+ k# H6 T& H6 _- byte Left;
) m% v) X* C5 a
+ _5 q8 O0 o9 q, t3 x, H- void setup() ! M2 U u/ I9 m! w
- {
5 X, M3 I, A- W- E* g - //Set PWM 8KHz
& a1 _( X9 ^% f7 V% o ~ - TCCR1A = _BV(WGM10);& o7 c7 ?! B5 H+ m! a( G! D# S
- TCCR1B = _BV(CS11) | _BV(WGM12);" u7 H; s+ l! s5 y4 u
- TCCR2A = _BV(WGM21) | _BV(WGM20);4 Z7 F$ f0 a# b5 R$ R1 U
- TCCR2B = _BV(CS21);% F$ Y, H n8 d. J3 p% U
- Serial.begin(9600);) I/ s2 n3 w( H7 \) K9 L/ }
- buzzer.setpin(45);
3 t" C& T4 a! w7 P0 d% u - }
! f: h8 v R& Z. L - * g2 M; {4 X' m; G1 U% @4 g
- void loop()& f5 R' _( T5 l# ~6 V9 K
- {8 G! b& V2 E1 D) w ^
- while(!((lightsensor_1.read()) < (10)));
' G: ~. C5 P9 @: D! o1 Y - buzzer.tone(1047, 500);$ E" D7 B6 _! ~
- delay(500);7 t0 c0 P. H5 p: J0 s" G
- do1 j( B1 y8 w9 ]3 U2 Y
- {
3 a3 G2 ^1 M: ~, V0 T! { - int Err = getErr();9 j* A% Q/ D2 v
- if(D1 == 1)
- J7 C' Y% B, q5 \9 }1 H# L+ F# \ - {: Z" W& ?- R4 F4 c
- Left = 1;8 X! w- @$ w/ {% u# A# ~
- }% n: z2 Z! y$ p- o, m, Z4 [) F- g
- if(D6 == 1)4 }6 V9 H. Y; B! O! }, x' ?9 F1 |
- {1 X! [6 }, x' v) [: ^% V" x
- Left = 0;# b: A5 n$ ^+ k6 n
- }
5 ^4 \5 e6 \; Q+ g - if(Err == 99), ]7 J! x* J7 _" Y; h4 o0 ?
- {* ?: K- I( u7 l1 K8 |/ |
- if(Left == 1): o% ]9 l/ Q' o! c
- {
3 n) v! K- A+ N& L: V - Speed -= 5;
1 c* }) V& J# @8 |4 ~* O# w% G - moto(0,Speed);2 |6 z1 F5 u8 g7 c& ^5 F
- do/ ]+ l2 A) P' d1 S1 o0 n; {
- {1 J' |& ~2 f8 C# \. S7 F
- Err = getErr();7 k* Y H w3 ]; G Z0 b
- }while((D1+D6) == 0);) l) E: K2 X) r
- }' K0 d5 d! {% P% ^$ M5 }
- else) U; K! e* ]5 ~+ a L
- {
* X: z3 {5 x4 N) |- g3 X j - Speed -= 5;
: ?- `5 @; P/ |: U- g6 b f - moto(Speed,0); ( Y, V' _8 |3 H: g
- do6 V# S+ [: w* x! {( k
- {! n( }4 S* q2 n- _) C0 h
- Err = getErr();
' m: U/ ]: M$ @. M6 U - }while((D1+D6) == 0);
, W3 r2 X. ]% I, @, o$ z- p3 s) _" J - }
& A+ w$ I8 E& F# R$ ]4 N - }
8 N. w; G; W' k, W# h8 B, ?8 l - else
+ l/ a: {" X# q: R - {9 U& Z$ n# S. J$ J
- if((Speed < 160) & (Err < 2)) Speed+=1;
) K/ D1 R% T+ e; ^0 X - if((Speed > 100) & (Err > 2)) Speed-=2; ) k6 l3 F# j" N. _
- integral = integral + Err;
0 k# I6 c) m$ e - derivative = Err - previous_error;
6 V& t& {- ^" K- _4 B/ T; N/ ? - output = Kp*Err + Ki*integral + Kd*derivative;" C* X, K* [- u3 Z0 b
- moto(int(Speed-output),int(Speed+output));/ g% v1 L- `# a0 {) a# f' k% m
- previous_error = Err;
% O7 {: v9 C2 w - }
+ e, g N% g1 ^' d/ p - }while(!((lightsensor_2.read()) < (10)));
8 c4 A8 z V: s3 _2 |% A' ]" I - moto(0,0);* ?2 ~* ?% G* z ]+ N/ _) d$ ^5 x
- delay(500);% G. n k2 j! E* D
- buzzer.tone(262, 500);
/ L- d) O/ G2 u: ? - }
# R# Q5 B( _4 w0 n6 f( H4 L! C& p3 T - ) \, N9 ^$ u1 H! ?( v
- int getErr()0 H; h3 m" s8 H5 w- A" D$ E
- {
: x5 i) i- W; `3 Z! C - sData = linefollower.getValue();6 l6 K6 V2 M/ B, K- a* g
- D1 = ~(sData>>0)&1;, p9 s; ^5 x: a3 _
- D2 = ~(sData>>1)&1;
2 ^ n. y/ I7 \& n - D3 = ~(sData>>2)&1;
( O9 e( n( {7 L7 V6 K - D4 = ~(sData>>3)&1;
* ~+ n0 ^6 N4 C3 W2 w E: C - D5 = ~(sData>>4)&1;* Y1 V8 x9 c" k! j" A
- D6 = ~(sData>>5)&1;
% ?# u( E% `7 a - int downD = D1+D2+D3+D4+D5+D6;
; l- [3 ]. p7 z' [% }/ _) l - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
/ s( S" b2 ?! M7 r; w - if(downD == 0)0 \2 D" H. ~9 H
- {
2 e4 E0 o( X" X$ k8 h - return 99;6 g$ x0 ^' y! F: H9 c6 n0 ~
- }8 I3 |, M5 N( j, H
- else7 {7 g# U; H4 J( X, R! Q
- {1 s' p6 C& L3 Z& f" M/ ?
- return upD/downD;
1 }3 q% O4 ?; e) {+ d - }0 |# N; B1 w, ~9 F
- }
7 s4 H0 t2 X' j( T$ @" ~ - ; \& b$ V: f5 i- C# o( S
- void moto(int Speed_L,int Speed_R)( O& c& a" ]( S9 r* a
- {
3 ~2 o2 i# U7 }3 t* n; [: D% N - Encoder_2.setMotorPwm(Speed_L);
7 \2 l3 i$ W. J, a - Encoder_1.setMotorPwm(-Speed_R);
. e0 F5 Y0 |0 _$ y" \* l - }
複製代碼
. n* S# B, C( `MeLineFollowerArray.cpp
: G6 z" ^+ S. ?# W& |9 p `$ K1 h- #include "MeLineFollowerArray.h" Q2 j7 J& S6 _8 Z ]) y
- * P% P, E; k; a" _
- #ifdef ME_PORT_DEFINED* {7 {% G& X2 u
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)1 }+ ^+ ~* c1 m/ l9 W$ r
- {
2 p: B& s2 H$ C0 C
5 a0 O6 u& \% Z9 a2 G! b' Q# \- }" P7 c8 ~$ S# m% b4 S
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)3 i8 c; X9 e8 } f: R9 C
- {* p5 a; u4 W+ s/ `" ]
- _DataPin = mePort[port].s2;
: o5 t. b8 }# Q0 J a6 b- e& e - pinMode(_DataPin, OUTPUT); 5 t; I% R/ f) w3 F9 H; Q0 t; N
- digitalWrite(_DataPin, HIGH);
' i: M3 m3 @- O, g4 K- i - }) e7 R7 a9 Q! J1 g3 s0 r
- #else // ME_PORT_DEFINED
$ F" q( e* m2 i+ R9 e. s Y- w) j' \ - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)2 |# C6 @2 i* }1 ~4 t, T
- {
w# F% v& R2 H- M& | - _DataPin = pin;* j2 H5 W) \6 f5 |
- pinMode(_DataPin, OUTPUT); - L* G4 `) |1 y; h; s4 r6 ~
- digitalWrite(_DataPin, HIGH);
/ L/ |6 C; h2 v0 n0 z. N8 _! v - }: K, L7 C5 `7 Z2 |; `2 Q' W
- #endif // ME_PORT_DEFINED U& R0 H6 L5 u$ I
1 P. J y8 J) _' ~! k
S( D4 V$ ]. X3 s1 J8 M- void MeLineFollowerArray::setpin(uint8_t pin)7 K) G) }$ a3 l/ Z j5 S5 M' v' U
- {/ |* W8 z& r" o7 `9 s
- _DataPin = pin;
) H# t2 Y2 C( r+ J8 w" ^& c3 D - pinMode(_DataPin, OUTPUT);
( t3 O/ Y9 s' S A) v - digitalWrite(_DataPin, HIGH);& q% m, l) y: y. n' g% S' E7 q
- + t) N0 e4 c) x7 `! e2 i, e
- #ifdef ME_PORT_DEFINED
& b! Q4 }# t3 x/ W0 C - s2 = pin;
! t7 K1 m+ y) @$ k1 k - #endif6 C, C h5 n5 W5 T+ _+ m
- }
( q$ G! f- h1 Y% t2 \ - $ v" C. Z) t) r. `6 n3 S
- uint8_t MeLineFollowerArray::getValue()$ U% V x& D' ~; d' O
- {. A* h2 O' f* j/ N- |* V$ O- V! O' m
- uint32_t LOW_level_read_time;; x4 I' x0 x" V, i
- uint32_t HIGH_level_read_time;
. k# o' O; ` f. ]& M' C" v - uint32_t time_out_flag;
: V# M: `% ?" h% z: J" j - uint8_t Sensor_Data[3];
. x S0 H4 ]# u6 G8 n - static uint8_t old_data = 0xff;
! A) X+ D- L6 N4 {5 u" |) P6 e2 N
/ e Z6 w+ x4 l. l- pinMode(_DataPin, OUTPUT);, [: K5 J0 W+ i8 F m$ \
- digitalWrite(_DataPin, LOW);
/ R0 c5 m2 W' D' u7 b - delayMicroseconds(980);
! i7 B( c; M2 l: A2 U! L - digitalWrite(_DataPin, HIGH);
! p0 a7 R% \) q7 H- r. y, K - & n$ J1 [; e7 [) P* C/ ~0 a i
- pinMode(_DataPin, INPUT_PULLUP);: u, d6 M/ _2 u4 T h" r" l5 F
- delayMicroseconds(10);
, a, L. f1 T. r0 i
m/ `: ~, [+ P" B5 J- time_out_flag = millis();
/ Z' V6 j: C R* F - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); {: S, {9 D4 ~
2 h$ E3 m1 L2 a5 { }% a/ J- LOW_level_read_time = micros();! h/ x0 [( E1 H$ H4 x* a( F
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out1 U P9 b! V2 e. |+ W
- {
- p* o" \; [( W: \" u8 l2 d - return 0xff;% P4 S: g+ R" [# N
- }
% F# x# C( a" j6 g
8 {4 X3 p& S* `! M) [* W [- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# E, _5 Z7 g: W) i9 y
+ e. M' [3 Y! o- Q- HIGH_level_read_time = micros();5 {5 i+ o8 s& S
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level) f$ E5 M6 O" Y5 |: z
- 6 B6 @& B% S% _; F. ~; I8 b
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
9 Y, H7 x1 e; T% o+ g! } - {
, |* m; a* [5 M - return 0xff;& K* W# M6 P) N& P$ Q8 d
- }
0 H, s8 D1 b6 U; x; p! W
g: G7 U F3 o% D" x+ \- U- p- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
5 X* ?3 ]* f* M - {
8 N4 S7 ?" h( l0 Q$ O3 d - return 0xff;5 z3 b# F5 e$ } {" E( E
- }$ O1 i' J: n9 k; B$ A; o$ k) W( R
- % q6 ~8 m. E. ^& R, p
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( t7 d$ q6 v& |1 n
- LOW_level_read_time = micros();! h; v9 b0 I1 n) h
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level4 P2 H5 I& Y) F, t
- 9 K+ R' @" o7 U8 Y$ O. ?
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
; ^% ]) I% x! C3 Q1 L' f! l& ~ o3 i - {3 U. S9 m0 x* h5 ]" n
- return 0xff;$ T, o6 D2 q* h& M/ A! \
- }
4 C4 Q" U% W5 I. i) _+ M4 A - ' e9 d( ~3 i2 g; Z
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))6 X. Q0 U9 r, W+ d0 Y
- {
! ^" b0 | g& D5 U: e - return 0xff;' ^7 |8 Z' N& ?$ K
- }3 ?( {3 o# n( g4 p
# k* h( w" ^% v8 L& v$ ]$ x' E- for(uint8_t k=0; k<3; k++)
( \6 b* [! V! o0 V - {
4 C" ?6 V( r5 d& |) Q/ k - Sensor_Data[k] = 0x00;
0 e. W! s# M5 U e, q
( r. |* H/ |6 I! A; ]* Z- for(uint8_t i=0;i<8;i++); {0 k& e2 O0 k4 P3 }
- { v% G' K @3 n7 ]3 d; Y0 H
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level$ e" n5 w Q' L. h1 t8 ?# f5 n8 K7 W
- HIGH_level_read_time = micros();+ B S( v* a, Q% z) s/ g
- LOW_level_read_time = micros() - LOW_level_read_time;( p( O# N1 i% u7 z' z5 T
4 d' r8 p6 P* s- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )* D) ]# a- T: |$ T" x* r! \
- {
( V1 ]% f9 C7 u6 L' ~. b - return 0xff;9 |: u1 R6 p' Z9 ^5 J% y5 I
- }- _- { x) I6 j+ c m
# | R9 ?9 X" g* o H( |- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ b: P" A/ K/ C0 N( ?# w
- LOW_level_read_time = micros();
/ d2 a) ~8 v# |! E/ C - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level+ ]# L4 b# k* d
- 3 R( C- B6 W1 M
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1* w, b' E1 Y+ Z3 K$ c0 S
- {
- z& V& p: ~5 \3 f! y - Sensor_Data[k] |= (0x80 >> i);5 s5 e1 B6 x) ^7 Q( p
- }8 Y5 q( V2 i+ A! `3 z+ t
- else if(HIGH_level_read_time >= 100)0 d' U) J, w' O
- {7 Q' d/ O v9 y" C
- return 0xff;/ a9 m# S& j/ s( X2 j
- }
& |1 D; f# ^* w, G" w - ; H' `3 q$ o+ o! A2 O4 \
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)/ m6 _( U: E5 R# Y0 R
- {
+ p3 D4 j3 v7 c/ d0 y# Y7 G6 T - return 0xff;
3 G0 D5 [! } m" E$ m/ S0 G - }2 @5 z8 o0 T+ M8 s6 q& m* g
- }
( {: S! r8 V R. H7 p% i - }6 j* p5 M7 c2 A7 I A0 }
- $ w, L$ [& _* m1 e$ @( J. J
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level/ p" e1 h0 Y2 Z% {4 T) a
- HIGH_level_read_time = micros();
# X+ {/ z3 k; W7 ?8 a1 g& P - LOW_level_read_time = micros() - LOW_level_read_time;% H0 g4 |1 a, U6 r
) f1 m2 F! \5 z- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
' K% Q; k% _! t R$ p$ b2 l - {. {6 V; E, |0 p/ y6 A2 s$ h! W+ w
- return 0xff;' n/ ?% w5 p7 [1 e2 V- f
- }
0 Q' {" O5 n: {( R$ }1 J - - d4 n Q/ k$ A, U$ h
- pinMode(_DataPin, OUTPUT);
! q7 R7 e" v8 h8 t% b; ^ - digitalWrite(_DataPin, HIGH);5 X3 v2 Y. C" f1 J
- ' v# F j9 D1 p- Z8 l, Q
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))( M" \0 r: `8 \' p% _. Z
- {2 I: q: g' e: w3 t5 {7 l
- old_data = Sensor_Data[0];( V9 c# @3 L- a- ?
- return Sensor_Data[0];
- m" j0 ]8 T) u5 f7 Q c! { - }; M8 J- ~7 a2 V9 ^
- else f1 h3 Q. _) c+ k% }" p# M
- {
+ | {+ e% y) N3 D2 m - return old_data;9 X( D% a: {( H9 C4 S b
- }
. G2 j) u2 N8 K$ k7 I: R! J6 A - }' D2 i+ q w* {& w
複製代碼
e; P+ i* I2 RMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
~1 C+ z" j- Y. z, z; T: m
! Z# z; w+ D- A
|
|