|
$ m, D; e4 |8 }+ r" n: `2 I" G
- #include <Arduino.h>
% H0 [* U6 ?' P& c1 ]" E - #include <MeAuriga.h>
) x% Z5 z# V- T. j - #include "MeLineFollowerArray.h"
% I, m+ c X } U- e( [+ \: c - " T* P) i" `( E7 c" t% z: b
- MeEncoderOnBoard Encoder_1(SLOT1);
4 ^4 K8 e8 L6 _* c7 B+ L3 F - MeEncoderOnBoard Encoder_2(SLOT2);7 r0 S& X/ o/ s/ K
- MeLightSensor lightsensor_1(12);
* [ O1 [% p k5 v2 b- H( |; q: r - MeLightSensor lightsensor_2(11);- }) e1 W6 E B& K
- MeBuzzer buzzer;7 O& V4 @7 @$ _$ w+ [, e; y
- MeLineFollowerArray linefollower(PORT_6);
% q( W7 q5 O$ C, u1 N3 a - ! y0 w: J; l+ ~9 l
- #define Error1 1; G9 ^1 P) w' T' J: K
- #define Error2 2
f. q8 A7 X# X& |4 |5 h* N1 F - #define Error3 32 P/ l" h" g a+ M$ i
1 ~# V8 a2 s/ i- #define Kp 156 t& P$ @8 x# M1 \2 _4 R* d+ D5 E7 k
- #define Ki 0.15& a, Q b; j1 e' j* Y7 _ e
- #define Kd 0.03
! N; X; e) f( T x& x7 s - ) b$ h, W, Y1 v O( M
- uint8_t sData;1 Z* t3 F/ }, `# b. J
- uint8_t D1;
' s" n/ _0 |7 t- ]# X$ a9 c - uint8_t D2;
/ m& X6 K0 }5 O - uint8_t D3;5 h; x( n; [+ p' j* r
- uint8_t D4;- ~9 p9 m* d$ ]
- uint8_t D5;+ X! g0 i- U: h" B/ V% v
- uint8_t D6;
( ]' o1 y) s( q- [$ L& Q+ M5 n' f1 G. } - , k q( j, ]# q* M
- float previous_error = 0;
' E* y, k8 M0 J8 d4 {2 ? - float integral = 0;* W$ l7 R8 _9 u
- float derivative = 0; I9 Z! Y, N2 s1 w/ Q/ [1 ?
- int Speed = 160;6 w: e% S& q9 G) \! u
- float output;: l, d% E. H7 Z) p+ `$ E
: h% ~( v+ @3 n- byte Left;7 a% c [' q( S: \; O
- . u; e8 U3 R2 j* a. t. L
- void setup() / P4 S( x5 h# h; A. r: f1 y
- {9 h/ T: ^0 J5 C- r: J8 V" T" m, k/ i
- //Set PWM 8KHz6 |4 f) a" R$ B1 z
- TCCR1A = _BV(WGM10);
+ k7 g% I) L6 p ~3 a5 |1 e+ b: ] - TCCR1B = _BV(CS11) | _BV(WGM12);
& i; k1 ~( s1 t0 G - TCCR2A = _BV(WGM21) | _BV(WGM20);$ _7 |+ q: n9 @7 N0 E& a
- TCCR2B = _BV(CS21);& a" b' t2 O% P$ ?
- Serial.begin(9600);' y* A8 ~- R! l# }# K B
- buzzer.setpin(45);
: b0 T& z1 U" c - }
* G) C+ H9 P5 f! Z8 ?
: c2 S7 H; h# W- a# W- void loop()
& o! g' ~* V X$ H% i+ R9 u$ C - {( b+ a9 r6 H8 O) _
- while(!((lightsensor_1.read()) < (10)));. d6 K- j: I a. Q
- buzzer.tone(1047, 500);
- z" r* P) i& t2 y9 r% `: | - delay(500);
6 G/ D* J3 ?2 y, P - do) ^5 A# ~( v0 Y* F( n
- {
4 z5 k( x9 @2 h3 }; p- Y - int Err = getErr();5 q$ y% [, H& w( ]$ u
- if(D1 == 1)
& {1 g2 O6 ?: @: [ - {6 f/ y. a8 r$ Y$ x& u3 k
- Left = 1;
' E4 U: H8 p4 \! R) K5 P - }
+ F7 |( i8 P) s) u) b; Z/ g - if(D6 == 1)2 ]) @0 h7 `% t8 u4 a! x
- {
9 p5 t: l9 i6 d0 S0 L9 H: k - Left = 0;/ U( p0 Q4 a. g" s+ }
- }! L- I$ M# \6 i4 t5 r
- if(Err == 99)7 n( W: D. ^1 C7 p% e) W
- {
1 u8 J3 _ A% m- o0 N% ]! r( b* ~ - if(Left == 1)
6 _2 n/ d- R' }$ u6 @ r. v, }# ] - {- D! b6 y% \0 n9 e# |. |8 W' x
- Speed -= 5;
+ u8 ~ u5 y K) W+ q! d9 C - moto(0,Speed);2 w& P4 S6 S# ~3 v
- do% C: ~% G8 n. @- e7 m# l m
- {
: [- s0 q( m; o7 G- [ - Err = getErr();6 c+ j; d, v# {3 g! c" _9 a' r
- }while((D1+D6) == 0);+ i3 A) T p G6 b
- }' C! {1 h$ m6 i% x4 h/ ~8 z
- else& |; Z7 P7 s* H, y3 N
- {
0 Y, i6 j$ q+ C, V. k, p4 G" p, q6 N - Speed -= 5;
4 {' D5 ~# d8 i- {# f/ O3 o: A5 t - moto(Speed,0); * Y4 Z8 X# Y" C3 e- F# [. U2 J+ m% d
- do% E2 }' p; Q8 Z# x/ b6 L" C
- {6 E! U( T3 C' T3 c% p
- Err = getErr();$ V2 T0 N! P0 e/ `6 w
- }while((D1+D6) == 0);! u: G# w7 z5 l; ]; d$ [4 }
- }$ S" y+ `2 o; U. M
- }
2 c1 q" s+ o# N$ P! J1 c7 K/ H0 p; ^ _ - else A3 d+ |& i; r
- {
^; U ?0 N/ H& {3 e" J - if((Speed < 160) & (Err < 2)) Speed+=1; $ x% U3 o7 R2 O
- if((Speed > 100) & (Err > 2)) Speed-=2;
& ~# N) m3 G; o$ s+ n) T - integral = integral + Err;
. s1 ^, @4 Q( }4 P% C& l - derivative = Err - previous_error;0 Z6 f5 L% n- c, F% M
- output = Kp*Err + Ki*integral + Kd*derivative;" P% u; Y0 z, p# m1 s1 V7 A
- moto(int(Speed-output),int(Speed+output));& c% A4 v' s6 G& u) q( Z1 E
- previous_error = Err;
3 c* T+ n- L$ o2 I - }% o: f) K# u8 R- t
- }while(!((lightsensor_2.read()) < (10)));
I7 `# ?& Z" o/ I- a - moto(0,0);: U# W# k4 {8 A; ]
- delay(500);
& @5 ^, u* m4 ~, T9 g - buzzer.tone(262, 500);( g5 {2 A3 [1 H8 ~8 S
- }" b6 c$ ?4 _* R0 J# [* p8 W
- 5 h8 y! o! g# g2 B# [
- int getErr() n- Z( W6 w' d, K/ w! d
- {
2 ?% Y5 h9 {- i9 l! r - sData = linefollower.getValue(); m8 O8 F' c0 F! k5 H" C
- D1 = ~(sData>>0)&1;/ u9 _( ^9 d% g7 P& U4 ^8 Q
- D2 = ~(sData>>1)&1;# }% B5 J7 ?% c) S& j
- D3 = ~(sData>>2)&1;) w, Q9 V' i) [; k
- D4 = ~(sData>>3)&1;
- [- Z! P- [( D4 \$ T1 W - D5 = ~(sData>>4)&1;
2 N1 f$ y$ b. @) R" q. G, A - D6 = ~(sData>>5)&1;* [) I" h# Z! t' }
- int downD = D1+D2+D3+D4+D5+D6;* k+ q- J: ~# y/ ^8 z% {, u* X! G
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);' B* m+ ~9 \- b; o7 B, r! C
- if(downD == 0)
# x$ i3 D) x3 | - {
( I7 T" |$ c6 z# { - return 99;
$ @( d L: e- E; T- M! E9 W2 k) l - }
4 ~, b: E$ Y( R' b% f - else
) Y; x& B) V' V - {: i4 k& ^( D' p- ]! W5 t
- return upD/downD;* Q. _' [+ M% a3 C0 N- U. d
- }0 _* m) s! J, I( c7 s$ O- R
- }
# c: b9 A. z+ D* P - 1 A! Z* u0 t! x, n" a
- void moto(int Speed_L,int Speed_R)& {7 B) f3 T7 h) z
- {3 [3 l, d, _4 n( N- B5 ]
- Encoder_2.setMotorPwm(Speed_L);8 h2 T0 ^+ w9 \3 Z3 P0 R
- Encoder_1.setMotorPwm(-Speed_R);4 X) I- J, H: [! d. F" k5 v m5 v
- }
複製代碼 ) I8 S7 T+ j/ n+ n: x) @9 N$ n" X5 K
MeLineFollowerArray.cpp
' q& W1 g/ [2 f* S2 x6 O- #include "MeLineFollowerArray.h"
4 \1 h: D+ C& q9 P. m - ( K J3 o+ W, E7 k" @" _& s& B5 a; o
- #ifdef ME_PORT_DEFINED
1 B4 _) J( Z U( Y: l - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
5 O7 g( A0 m6 T+ e: r$ k4 ^ - {% E6 m) R! F$ F$ W* z; ~+ j
- 6 O6 D2 @7 E/ p2 n: N/ o& g. i0 N
- }
/ i# Q* \) b! {: h - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port); d) w2 `8 {7 C# r
- {- ^' J/ L" V/ n" C% L& _, X* r* d
- _DataPin = mePort[port].s2;. e4 ]0 Y" v) m R3 W# K2 t! g5 W
- pinMode(_DataPin, OUTPUT); % J3 V# d( C& Q1 A7 e
- digitalWrite(_DataPin, HIGH);
6 X3 A" K. [; h9 }7 b: e - }9 K- t1 K- M5 o( t2 \( d* u
- #else // ME_PORT_DEFINED
/ N! \/ Z. \7 G. [+ G7 N, P - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
: w) J) R* z$ C; d - {' M# d9 C/ ?9 s+ x4 A) ?& ^
- _DataPin = pin;
+ X$ _: r* \# ?0 M - pinMode(_DataPin, OUTPUT);
% G$ y; Q1 ]; C4 x - digitalWrite(_DataPin, HIGH);0 P" x4 r4 r( N& z& H2 h
- }4 d: m i8 v; I0 e; ?
- #endif // ME_PORT_DEFINED
/ R% j% a3 n2 [; } - & L% k3 g8 @7 f) G5 K9 ?2 T! ]5 U
- 3 C# v2 X7 y% y0 L' V N! Q
- void MeLineFollowerArray::setpin(uint8_t pin)
6 d) r2 m: o5 c8 m$ @2 @& J - {) K7 S+ T. |9 [" b; S# l
- _DataPin = pin;- d# g8 v2 q0 S) U; g" u" A
- pinMode(_DataPin, OUTPUT); 0 W+ m; }0 |* F h( C, @& W1 p
- digitalWrite(_DataPin, HIGH);
; _; o+ u: t$ h5 k+ s& L: o; W [) o - * _2 G. V- ?$ O1 i/ g' Z
- #ifdef ME_PORT_DEFINED, A' U/ u4 ]8 s
- s2 = pin;
+ w& N% B3 y" H E! _$ n - #endif
% A& H& B2 a( G: q! I. j' { - }
& B( b2 @5 G1 T, g0 x, s8 Y0 ~ - 6 t! J# Z% `. j" |
- uint8_t MeLineFollowerArray::getValue()
% N# c; F, X* @, M7 y4 @ - {. ?: g' R4 H; u6 k' u
- uint32_t LOW_level_read_time;) ~& M$ }# k; G
- uint32_t HIGH_level_read_time;# q ^& t3 \/ r6 J
- uint32_t time_out_flag;
+ A7 N; J9 c. Q0 T, o# E - uint8_t Sensor_Data[3];
1 x0 w8 U+ i; u) ]/ |( W7 l6 k2 _ - static uint8_t old_data = 0xff;
, I( y+ h1 E* W4 U2 w* W - 9 t- R5 ^/ S* z# O$ q4 P; W8 f
- pinMode(_DataPin, OUTPUT);
6 h# q! F4 X* b; {! a - digitalWrite(_DataPin, LOW);, w; C7 t* {6 { C8 C5 M4 g5 @
- delayMicroseconds(980);
9 O/ W8 ^( x/ K- h2 E - digitalWrite(_DataPin, HIGH);
& ^9 h. _' d6 w: G5 M8 r
# G6 z2 F3 L7 t- e- pinMode(_DataPin, INPUT_PULLUP);& n- P+ n4 m4 `9 b
- delayMicroseconds(10);" _3 D1 u9 S/ `3 C
- : ?7 `( G. O. M. A. x# G; w) f8 n9 K
- time_out_flag = millis();
5 }2 M* f8 m7 k+ }" O - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
Q+ R9 N* l: B3 |: F2 f4 w3 b/ W7 s
8 P! W9 @4 g& [- LOW_level_read_time = micros();; o# D- r- I2 P
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out1 A P7 [8 K* ~
- {
o+ L7 [: {+ A- b+ R6 r1 I5 f- A - return 0xff;7 F& ^4 f8 }" K, A
- }7 F0 X t% m: @) s5 n
- : n6 b) P4 d+ ~
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
^' X, T. S! y - , @7 o: z; z! o$ \, x& Z
- HIGH_level_read_time = micros();4 X8 r# i+ R; e) A/ m. @
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
+ R% N' k& w6 f - 2 m) O* O* C4 y) y
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
$ G, t7 D1 S: A+ t6 U3 r; z - {
" B, e6 t$ E, A6 X/ {0 T - return 0xff;
% `4 o" s& _9 b1 Q3 f" s - }
) @) Z/ U9 N! w7 A, D# b - 7 `: ^6 L9 H. E& G7 p3 @1 x
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))2 A( b* x3 [8 ~8 T$ h) |& r7 J
- {3 R; T+ l2 `% ~. J& `: {0 g3 @) g
- return 0xff;
, a" q* O% P7 ~0 s& {5 C$ U" S- f - }' H' o# q4 S4 s4 Q9 N( A
- 9 V1 p8 P/ L! Y4 E
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( c* p: v& Z. ` - LOW_level_read_time = micros();1 h1 t2 Y8 u3 C+ g( Y
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
+ u3 U8 s6 w. y- s0 \4 E - ' ^9 @% q: Q1 m- f: z& ~2 d. f
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 H w' v* M) \1 x; q, C7 u
- {( \* |1 ?' T: v! R, p- u+ B- D
- return 0xff;
6 _9 \- B1 t5 ~% G8 T5 M0 t8 @1 G& r - }8 ?4 @5 ?! t3 S2 N6 `
- . u, O" ]* \. k3 a
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))2 r2 f. q% Q2 T8 Y; a9 H9 `9 ]
- {0 p+ B" ?& y% d3 \
- return 0xff;
. q, }) l" ^5 e( f4 d - }0 P! V) [2 P# @! t3 m
- # C5 X2 v4 F/ ]$ O- Z1 P+ ~4 X4 K
- for(uint8_t k=0; k<3; k++)" o- M/ j# R9 ^! u9 s" F
- {
3 L' l! a4 x% ~6 _" y. ]9 \ - Sensor_Data[k] = 0x00;
9 c& M( [# v3 n" s4 X - / J% Y9 x! T4 W8 X6 b; G' e
- for(uint8_t i=0;i<8;i++)
$ S J$ y9 y1 l1 ?. |2 ] - {
! W2 ~, {$ x' P- [: W S& e- i& v - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level( O0 n, D! q9 t+ l+ Q
- HIGH_level_read_time = micros();
9 x0 Q3 [+ t& ~6 X1 Q# W - LOW_level_read_time = micros() - LOW_level_read_time;' z* {- }+ {. r, w1 A9 k
- " c3 D+ C: F2 [! v3 [1 t' b3 ~0 s+ {1 K* Q
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
+ r" Y* s: r5 D3 @ - {
$ y& y7 \0 k, t( W/ Q) F9 c/ L - return 0xff;
1 f- l5 @/ N+ U) d6 F - }
6 [5 q7 ?6 \( X& O
* h3 k- ^$ f' S' k c/ W1 m% [! _- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) v$ z5 L0 T0 M, a
- LOW_level_read_time = micros();7 g$ L( a% [0 A' y* w. s
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
( a2 P) B( I! `! } - ! Y$ f( n1 d' A% h# c: _
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 19 D' g+ X" }1 {6 `8 a" \9 a
- {
9 @* a& l+ F/ X/ Y3 {( j, ]$ L, m - Sensor_Data[k] |= (0x80 >> i);* [ C2 {3 j& E" O( `* u. v
- }
+ | { v5 ?. |# P6 d8 d - else if(HIGH_level_read_time >= 100)! q- p* y$ X0 `) ]/ g
- {& E5 ~4 w( d2 U1 r) i5 c
- return 0xff;9 {! G: b/ S3 P7 |
- }
3 f! A3 T/ f' F2 J4 \0 f2 I - . J% Y3 S. S# H6 t ?
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out): e4 w0 j2 y+ I
- {+ a9 F) Y/ H, l- d6 d
- return 0xff;
9 Y! O" F% t8 H3 L/ \6 P* G - }' u0 t, E% T( L0 E
- }
. x1 m+ g; I/ z( w - }% ?* p9 |9 J" E" n# q/ `
$ k B- q! p! }( m& [( B4 P- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level+ b; m# V. v- b
- HIGH_level_read_time = micros();
/ v6 M* K3 p! b* b# c- p - LOW_level_read_time = micros() - LOW_level_read_time;( C, K/ a6 v0 x% M* r! i0 f
) C+ C" x- c% |' t+ d) F- J- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )$ c4 T5 s0 k$ s3 }4 E# Y
- {
( K$ M/ B! N1 P' ~8 D6 `: R) F - return 0xff;
5 `) x. y) j( G, w' C - }
; [! N6 j6 r( @5 V" T/ h, }
! M* f$ W# \; l5 G- pinMode(_DataPin, OUTPUT);
# C* O' V& k, _1 q - digitalWrite(_DataPin, HIGH);
! S% e( j, k1 u1 `, |: d
/ A& t: z, f5 Q+ |. l; }" N" c/ E, T% X, ^- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0]))) w) `- U8 Y W
- {% t3 F* X( \0 J. c- U8 W0 V
- old_data = Sensor_Data[0];
: O# W$ P3 `1 c - return Sensor_Data[0];" F; i3 D" y: Y2 t3 @+ a6 ~
- }
4 N# |. a3 C) b: @ - else
' x- [, B3 m- S8 A' s! M - {3 f. E P: W3 E9 h& O( b+ J
- return old_data;+ B% U/ Q; P/ U+ M8 ?8 [5 A
- }- I" d3 W$ O$ y9 M: M* V+ a9 \
- }7 q3 l' R4 D$ K! b' R7 S" v
複製代碼 & @1 ]4 B9 K1 V: J
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
9 ~. s& X* G8 H( u! m, r, W. C' U2 y* D# H: S& s
|
|