|
|
" q l; G+ K- q9 G
- #include <Arduino.h>
2 \) B9 n! Z- s* |, k - #include <MeAuriga.h>
% }9 V4 h; f3 ~$ I - #include "MeLineFollowerArray.h"% v3 R- ]8 l- N
; g0 e$ u: C+ I/ P1 D6 |- MeEncoderOnBoard Encoder_1(SLOT1);% }8 }5 i+ s9 C- O$ |* x
- MeEncoderOnBoard Encoder_2(SLOT2);
: B* C% b, I8 l1 v$ \ - MeLightSensor lightsensor_1(12);: s" U H" P4 U' C4 ]
- MeLightSensor lightsensor_2(11);
5 L: S! u' T. v; G( R$ ~+ E0 S: k - MeBuzzer buzzer;- ]( Q" {! d8 O3 b: c8 T; K
- MeLineFollowerArray linefollower(PORT_6);9 q9 s s. f. w* F% p
# C( l. E9 K: H% u7 E- #define Error1 1
3 Z) S7 r6 c; `7 c! m0 T - #define Error2 2
0 a3 j4 B0 P7 [ g) `+ f - #define Error3 3
$ A- V) f( b) h) U6 R1 I% @! r - 5 w/ w6 E0 [/ z: p$ j A2 k
- #define Kp 15
3 U1 Y" l. p; T$ @ - #define Ki 0.15
# S2 O- R( j* x8 k2 ?; f( @! V - #define Kd 0.03, R1 [$ d' B9 y$ X/ O# ^9 U
- 5 X7 j8 i! |6 [7 E; O
- uint8_t sData;; D. U: d. w' C+ r: V$ ?* g
- uint8_t D1;
1 C: K4 q, ?4 j- M( \ - uint8_t D2;
: H% l7 v% M9 P+ F! V0 H - uint8_t D3;
2 }3 U% q- Y3 b5 N# L6 x ]( Q - uint8_t D4;7 Y+ W! A/ s# j" {* i
- uint8_t D5;
* G a3 B' g) }; a% Z3 h8 k" D - uint8_t D6;+ i. K0 @6 w+ c3 R0 E( K7 V) s9 U
' s, W( v# P0 `3 r- float previous_error = 0;# U t( u4 J O. X' t
- float integral = 0;0 e- `# z# v2 `7 S& S# b4 l- P
- float derivative = 0;" P5 c4 W- X1 g8 C |" d
- int Speed = 160;# ?. e" K, f' u2 P
- float output;7 C0 \3 m6 O" ]9 G( r' D/ ]1 A
- / j6 z% l2 j1 h( W0 r+ F* ~
- byte Left;
% ]- D7 d; ]5 f
4 C# J& @% b0 S6 ^ n, S/ `& {9 u- void setup()
* f7 I4 _, w) T6 M& I5 ? - {
* z& s3 W7 ?: {4 n L# M, K e; z - //Set PWM 8KHz
! U- X( Q9 I2 l# ~5 I - TCCR1A = _BV(WGM10);
. |7 Y% M6 p1 R" R2 u! n - TCCR1B = _BV(CS11) | _BV(WGM12);
$ j* [: K5 L2 [ - TCCR2A = _BV(WGM21) | _BV(WGM20);
) g8 H- ?" ]: H - TCCR2B = _BV(CS21);( f- V( `# z4 Q Y2 \3 @+ c
- Serial.begin(9600);5 Q9 C9 p# _4 R$ _
- buzzer.setpin(45);
h5 V l2 l7 j+ T, L - }. |, t4 a+ i& I' {( B
- % [2 W p; g) O" n7 j# s9 m3 l1 ~
- void loop(). I' H. m( w. t. F0 q
- {! z/ `: O# ]5 R/ A1 Q8 d
- while(!((lightsensor_1.read()) < (10)));
# ^( j/ T- r3 J- m% s* p - buzzer.tone(1047, 500);9 h! Y* w. X9 ^3 f' k
- delay(500);% ?4 S3 {5 z$ w1 f, u
- do
6 M6 }2 w! h' b7 ]" T - {$ Z/ h G B d# S- A% M" r
- int Err = getErr();- C; X' J, [, p" S1 W1 p' M0 T% P
- if(D1 == 1)0 ?" j4 M/ r- j, M
- {4 J: R+ t" o4 x3 {5 t
- Left = 1;
& J- ~* i! V$ f/ M: A& A - }
4 u6 s# P4 ^( n8 r- N2 {, m6 r - if(D6 == 1)
" R9 s! k( q+ D4 l+ T9 e: E2 F - {* f0 v: F8 u# }0 i+ z8 M7 Z% `. j4 I
- Left = 0;- D/ z( q7 |' P# r& Y. t
- }0 E8 A( f/ D# X$ a8 n
- if(Err == 99)
1 U h2 @) E9 T$ I$ U$ l6 y+ F9 e - {
y \0 c+ P4 o% F3 f6 d6 G# |" B - if(Left == 1): h o& `: v% h* i' L" S, A8 p+ O
- {- l* S! ^- X( w
- Speed -= 5;$ {4 r8 B. h- @6 ^' j6 D' C! Y
- moto(0,Speed);
_+ \: J; T, S( l2 V/ I - do: I c, p; Z+ m- j% ]7 [6 V
- {. j- J3 L6 a1 x6 r& C: p
- Err = getErr();( ]. N0 t; L4 h; \/ J2 k: }7 b9 V* `1 \
- }while((D1+D6) == 0);
; V# H: A1 n' q7 m; r - }
6 b/ I; n* U/ n6 ? - else2 i* b" ^. Q( r# H( l' x
- {- d" ]( |. V0 d1 v# ^) d! T4 D. r
- Speed -= 5;
/ L3 t9 ^- ^ X+ G. U/ U5 l4 R - moto(Speed,0);
. e# G, R( S$ m: A1 w" I9 W - do+ ]/ C- b, i8 Q
- {
% V- W5 q2 L- c2 V0 `9 _( x - Err = getErr();, n: [. x0 O; L5 |3 R& ?
- }while((D1+D6) == 0);( C, c4 ^# n0 r7 [# Z
- }
~. e" m# e. U" ^3 c - }
K; f5 o2 Z3 H. B7 {% o F - else" t, m3 w( _) B" ? N
- {
! h9 ]- Q! P4 `3 h& V. A - if((Speed < 160) & (Err < 2)) Speed+=1; . ]2 }- Z/ R5 `7 H
- if((Speed > 100) & (Err > 2)) Speed-=2;
$ B2 }" O3 z4 c( [( J X - integral = integral + Err;
6 O6 V. r5 v7 ]6 c) ] - derivative = Err - previous_error;2 ~$ r8 h& n0 s
- output = Kp*Err + Ki*integral + Kd*derivative;1 r3 T* D7 T$ d( L F
- moto(int(Speed-output),int(Speed+output));
$ R( l- \9 c: ~& x8 p - previous_error = Err;
3 u+ l" P- p# e+ l - }
1 J/ H1 w8 G' r) G( l% S; B1 n4 F - }while(!((lightsensor_2.read()) < (10)));
6 ^- O7 {1 H* k/ J/ ]. _- m - moto(0,0);
' J2 _, k [* p' q+ H Y: w( O - delay(500);0 `7 ]. ^8 a+ Z; U: C
- buzzer.tone(262, 500);
" {5 t+ g# G5 M2 S7 n4 n0 {( \" R, D - }
! x9 u; j" T* f( B& V# ~4 P9 y
0 V, j( M$ L+ I& Z6 t4 z, K7 Y3 n- int getErr()
1 P. }: Z( Q2 ^: J/ ~; |" t/ E2 @ - { ) D, ?4 q6 c# z; l4 C
- sData = linefollower.getValue();
* `2 @% u& V& |' a0 e - D1 = ~(sData>>0)&1;# h, d3 O5 k6 _# O3 J! y4 N- c* J
- D2 = ~(sData>>1)&1;
9 D( r. E P0 o: g( d0 R( d - D3 = ~(sData>>2)&1;: E u. ~% y) `0 [& x
- D4 = ~(sData>>3)&1;1 O0 L& g$ k& Z7 e( n* ?
- D5 = ~(sData>>4)&1;) B. B E1 M' M3 b7 n$ s
- D6 = ~(sData>>5)&1;
' A* {3 @& N( h* N7 H& B# f - int downD = D1+D2+D3+D4+D5+D6;+ @, j2 z w- W% q& U
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
2 F3 I7 x" X" n, Y - if(downD == 0)
/ W" u! m4 P- d; k0 C5 a2 f - {
: q9 L- E N" g `+ S - return 99;, r# n0 R6 k! [# j! R4 _
- }' }0 T4 J6 g' R0 R
- else
# v/ ]9 F Y0 i5 T* ?+ q - {- X) m$ \/ S1 L
- return upD/downD;
0 v( X, V4 M+ v& @7 c4 l' e - }
: x+ g/ K8 c# d |. } - }
) C: H/ T, |) w$ b; W0 [
& f0 }, L6 s, z3 h8 e, A1 G; ?" d# X- void moto(int Speed_L,int Speed_R)
* ~; y, N0 o5 K. @ - {* X7 p* j6 u, o* c
- Encoder_2.setMotorPwm(Speed_L);9 e* n* a# X4 _$ a8 P
- Encoder_1.setMotorPwm(-Speed_R);
* {3 ~* l5 w2 w% o- F - }
複製代碼 ( v8 G& x5 i- \, j/ Y- Z0 @' T
MeLineFollowerArray.cpp. L7 y3 _$ N) l1 S9 U- f
- #include "MeLineFollowerArray.h"6 U. ?8 B( U, u! H6 L
- - A* [/ L3 k9 X! T6 c8 k* G
- #ifdef ME_PORT_DEFINED7 M5 r# Z7 H w, k2 P7 P
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
# t x9 p2 q# r" _( E - {
+ b T2 o" g* X0 W9 g8 \8 m - ) H, f/ ^* S, g5 E+ T/ |
- }
6 d/ ~8 @, W; \/ E9 F - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
3 w. d: W6 U5 |" a - {# ^/ G% k! b0 ~3 z
- _DataPin = mePort[port].s2;
6 E* L; N% I0 t - pinMode(_DataPin, OUTPUT);
; Y, A' g$ e- W+ x; y - digitalWrite(_DataPin, HIGH);
: y4 E. |3 P! o# n- q# c9 s- u - }
0 f' x$ N" R$ y" p% m& ~ - #else // ME_PORT_DEFINED- {$ }. p; |" I0 [' H+ M& Y
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)8 Z6 G. O' ~# h z$ Q1 K
- {
9 x. a8 ]' P6 C: v) t - _DataPin = pin;! O9 N# G$ e# {6 N/ _
- pinMode(_DataPin, OUTPUT); % D3 a4 S' B6 Z/ O
- digitalWrite(_DataPin, HIGH);/ i. o: K' l9 p% A7 K
- }
8 [& x" Z; x& i7 T: J$ d5 N - #endif // ME_PORT_DEFINED/ A3 E! S6 e) {8 C5 W0 v5 f
, k, s( T4 u4 _( ?0 o4 Z
! H/ `, R7 a/ p5 }8 g8 ~- void MeLineFollowerArray::setpin(uint8_t pin)& [4 Q3 Y1 S+ y7 k+ p$ S4 ^9 x
- {
7 G9 `$ `9 ~! e5 Q - _DataPin = pin;+ I: A$ e5 p8 y
- pinMode(_DataPin, OUTPUT);
: a0 Z+ p9 |! o N( v - digitalWrite(_DataPin, HIGH);, k( ~- p# R) Y% `7 ]/ J) P
- 1 e$ Q" I( e! L0 Y
- #ifdef ME_PORT_DEFINED+ {9 R; x, z* y8 U
- s2 = pin;$ M; A" l3 P0 J0 N; D; w9 d
- #endif+ Q+ m( _3 X' x/ N
- }* J, U S6 h5 |6 M# v! a: y% h
9 x2 M& I5 P3 `' {! w/ e- uint8_t MeLineFollowerArray::getValue()
4 f. d) }* T' _; T - {8 g7 a. V4 Q- Z% V# }
- uint32_t LOW_level_read_time;
1 q6 I! ^8 ^' ?- @% s - uint32_t HIGH_level_read_time;+ {4 i, K& a/ ?2 \# [: _$ [
- uint32_t time_out_flag;% I' T0 K$ v1 o2 |7 v
- uint8_t Sensor_Data[3];
5 ^. |2 |8 L, L - static uint8_t old_data = 0xff;
+ O1 e/ J) d- ^' f- l" x# x
$ B, U+ B) s* y9 O* [- pinMode(_DataPin, OUTPUT);1 Q$ E5 O! [' O. y
- digitalWrite(_DataPin, LOW);
9 s: o/ i+ e+ h3 l" l - delayMicroseconds(980);
' t, A* r0 g7 D - digitalWrite(_DataPin, HIGH);# w- E" X7 X) b, {3 [* X, j
- ( C# U1 A# n( @2 R) t* B; `
- pinMode(_DataPin, INPUT_PULLUP);
( \1 o4 n. r: K* v/ W4 |- t! h$ z+ J - delayMicroseconds(10);
2 d' S& C9 F/ C" O6 ^3 \; ` - 0 a* ~. b' ~4 Q6 [, k3 q* \# T
- time_out_flag = millis();, o% b) {: |0 C4 p) ~0 p% u w E- U
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' b# e ^/ L3 y; l; \ @
+ M- R+ f; \/ O t4 p3 c& }- LOW_level_read_time = micros();
: k- |7 X; S" l; A3 m& \ - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
+ _4 i- x9 f0 ]; ]' d - {
' l e# n% A: j* ^ - return 0xff;5 [. p: U/ I8 K) U: j2 B
- }
0 s: L# F8 |* [
9 n! Q, c f" {! t- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );6 H p) g" Q* P7 }& t- T
- : I4 a4 X& y/ ^8 X9 B8 d
- HIGH_level_read_time = micros();
% Q$ Q: J8 w& Y/ s5 c - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
+ e( C7 z7 C j6 k) i: J
' i% j. c+ y% |- p2 Q+ [% s: K- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out; L) V9 ^( A" a: e2 A3 P
- {
) B9 _" }0 v7 [' d3 N, s# p - return 0xff;
. y& I; O' l6 A1 V - }
/ F% S. H" a( E: s' i
( @' Q: u' G! z" Z1 W! P7 T- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))& o& v# L! Z* w$ W- I
- {$ i* y. x, Z' A: K- o; F# j3 h6 q
- return 0xff;% Y v' w1 a' ]4 l; I# k2 F
- }
# a( }3 K' o9 ]: n% d t+ K - ( l* P" N4 V1 U8 F& d+ m. V( w
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; D5 ]2 t/ O# j. c1 J8 U/ V$ E - LOW_level_read_time = micros();
0 h- ^' Y) J) i; w* o - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level8 y- l4 t% Z9 V+ y1 D
% } H; G% v5 W$ q- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out+ Q7 b* e5 _, t: q+ t: \, n
- {
, j( p+ b3 G* g6 ~ - return 0xff;, z) O& A+ }9 O. c- M1 H) D
- }; {& L4 g3 {8 [1 Q. l" Y& ^
$ T2 r- Y8 _+ i- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))" e+ O* ^. y& r: Q2 O
- {
) A1 K- Z" a, i& Z! k - return 0xff;
5 l4 Q/ D) t2 i; Y2 ` - }# f/ _% s$ q8 u$ Z D
- 3 c/ e+ p2 O1 k! E: t4 F
- for(uint8_t k=0; k<3; k++)* D8 O: X& q# k" L
- {( m& m2 ~# h: m- S% \! c( A$ K
- Sensor_Data[k] = 0x00;
9 Y7 m, h* R6 r; |
1 `9 J6 L8 R' x# J, h( p# [- for(uint8_t i=0;i<8;i++)
! n3 J" E* V% y/ A" R0 P - {
: R3 V7 g z6 L9 `( J - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
% {) K" b0 p/ J/ S* B - HIGH_level_read_time = micros();$ b* o z. |7 [1 Y; m. F
- LOW_level_read_time = micros() - LOW_level_read_time;7 ~ X$ M, Z, w7 g) T8 l
% E' @# ^8 x3 A$ D E- |8 ~: Q- h! S+ ]- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ), X' E' I1 p& w- k7 S. [0 X
- {- M0 T& }% G: ^" c# P
- return 0xff;
} n: ~. \: B% C x% S- s - }, {( x7 {* _( w8 E5 I; w( b8 C
, C4 b& I. j' M' n* x% U( V7 C2 ]' l9 L- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
' m i: {* x; I0 \* ]) R3 E1 O - LOW_level_read_time = micros();. Z$ }/ c: j/ D [1 v7 f$ }2 E4 s
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level! p: D# u# N/ x9 C6 v% y
- - ?8 _1 O( A1 x
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
, _& @1 ^6 B. }+ ^ - {
6 x# ?; ~) |: z: C( b: W+ c - Sensor_Data[k] |= (0x80 >> i);
( P5 o# h' |" k - }& f- S7 j, P6 Q" N
- else if(HIGH_level_read_time >= 100)
+ T9 E% |8 Z W, P# x# p: F8 Q. o - {. a' K- A8 Z- Y5 m
- return 0xff; Y4 G, w" f$ I* W5 v4 F/ b7 B
- }1 ]6 P% R% M6 ]* p r6 Q
- . |5 t0 Z5 _- D. }7 P1 f
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
8 J2 D( H, }# C- n7 ] - {. `9 y$ W' g2 S! {3 o5 Q0 G
- return 0xff;: s+ P1 H8 e; K; M! l0 D
- }, H. z9 Q4 E: P: S& f
- }4 m" L% W, v+ |! s
- }
- T! D8 ]. D% J
# u7 e7 o; u* }- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
9 Y- h5 b7 c' N/ G3 Y+ Q - HIGH_level_read_time = micros();9 x x# j9 W; w/ l
- LOW_level_read_time = micros() - LOW_level_read_time;
$ X3 L+ N, B) T- o: y5 Z3 d - : }, \# K$ [8 ` l: z# y
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
& [/ |, A/ a7 T8 E. } - {
9 m: ~. p6 m# `3 | - return 0xff;- I, V& C, V. S3 M( ~' O
- }
. b8 c) ~9 Y/ R, p5 M
n6 v: H, w" m1 y# ~, v- pinMode(_DataPin, OUTPUT);9 F% a) W! T6 J) M; G$ W
- digitalWrite(_DataPin, HIGH);! K' E! s, s1 y, ~* ]5 l# m
- ! ~: F# |7 H7 V- h
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
; m7 k1 o- u% s3 H) C0 a - {, Y& x4 M/ U4 R' Q {
- old_data = Sensor_Data[0];
2 P* I8 B3 J; C# d: } - return Sensor_Data[0];8 d6 M+ O8 I( e/ K
- }
. R" I! v C( |- F - else
3 H5 v6 {* s. m; d2 @* [0 x - {
7 X$ i, l) u9 K( g - return old_data;
9 D; M* x2 r3 k7 T - }7 p4 @% w) p( q7 T7 X/ U" {
- }
@& Z. ]8 Q) A/ `' d
複製代碼 6 c5 W" {! I( W: T- s
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
" b- r; H6 [/ _$ P7 h& t6 g; E
' B( P6 r4 T+ F$ Q* E |
|