|
|
1 B" i; @7 C+ W8 _
- #include <Arduino.h>
l' g8 d5 w A8 N8 ^, g1 H - #include <MeAuriga.h>
. z, c5 x8 s2 f( j# k( u. k |7 B+ \ - #include "MeLineFollowerArray.h"
! v, D5 }! ]! x - 9 ^! \/ Q' u: l: U0 |
- MeEncoderOnBoard Encoder_1(SLOT1);3 N: U' h! Z, i
- MeEncoderOnBoard Encoder_2(SLOT2);" a& K1 B* s7 b! [7 J; q
- MeLightSensor lightsensor_1(12);8 T; u M# r7 i7 [
- MeLightSensor lightsensor_2(11);
1 [' ~5 j$ P: p) t/ k3 h2 ]0 Y - MeBuzzer buzzer;
4 p& _( o3 C/ Z - MeLineFollowerArray linefollower(PORT_6);
0 g% ?3 M5 h! @: ?! a3 D R - : B6 n" B6 o8 h* d9 S% _3 M! d
- #define Error1 1( M% d4 g }3 t: p
- #define Error2 2
% N5 @6 }1 [+ w1 P - #define Error3 3
6 R; J9 y* {( Y3 a' f7 P K
0 r, L$ t. i; s) ~- #define Kp 159 g; U' f+ I; c7 M& {" w
- #define Ki 0.15
0 o0 S; Q2 i: ^7 G" D0 x- @ - #define Kd 0.03
9 r- Q! S; [9 g- z" B" v - " m: k! w4 C8 y0 z: r- e! h. d' m7 A
- uint8_t sData;; X! Y: ?7 Y0 ]0 a+ C
- uint8_t D1;
1 V& T2 B* J, n( }8 m+ t- t( R4 s - uint8_t D2;6 a9 J' s: d% {3 k' |
- uint8_t D3;/ K7 I" I, s7 t0 i* b& K5 s
- uint8_t D4;
5 s1 e" D% @- N% A; U; Z - uint8_t D5;
; L) a; }9 ~( A! z - uint8_t D6;9 @( v9 L) y) ]3 H" W! \) M0 u) l3 E" M
0 d- B8 d5 T6 B6 m; t9 z. v# j/ D- float previous_error = 0;& s& p4 V7 ?, M h! P! u
- float integral = 0;1 E ~$ y6 y: @4 d
- float derivative = 0;
: b& J: Q" G) c+ O3 z - int Speed = 160;
. q/ `9 l: x' | w1 d - float output;
: b6 @2 d% a; W. J
" ] ~+ ]. {% H5 o4 u0 N- byte Left;4 Y4 L# Q4 n, A& d4 b3 M: F* f: ]
% o" p, o4 \$ h. K- void setup() 6 H7 x! L! V2 c& N4 x1 U% d
- {
0 r" x6 I+ r! i - //Set PWM 8KHz
6 B* b$ L* x2 h ? - TCCR1A = _BV(WGM10);/ o1 l: W4 ?9 B
- TCCR1B = _BV(CS11) | _BV(WGM12);
A$ D; K' }% ^9 H - TCCR2A = _BV(WGM21) | _BV(WGM20);
/ C' n8 X3 s% S f4 J c - TCCR2B = _BV(CS21);7 Q$ {9 l$ p8 s! F7 Y0 Z
- Serial.begin(9600);8 _& B/ U+ V w8 {$ P
- buzzer.setpin(45);" C& Y% _* M8 a: ], ^$ ?0 I6 i
- }
+ {- m% ] ~/ }" x) _
3 f6 I2 i5 r* Z) t( T2 Z- void loop()3 D D) H1 l! ^. u% I
- {( Q4 F7 ^6 h* h0 ^
- while(!((lightsensor_1.read()) < (10)));
% h/ @3 r, \9 t( N" K |* u - buzzer.tone(1047, 500);) H- u" B' k* `) I# E- K# d
- delay(500);) R' K) f4 ]* c1 g3 P: [! y* ^7 ]. o
- do
5 o8 @& c7 c- j: ~; i+ U - {3 A3 Y9 N( t9 X* K
- int Err = getErr();
0 V: O' Y" q8 ~ - if(D1 == 1)4 A$ W- \4 A: } B `1 v0 A
- {( J# h( w* `& L* b4 ?
- Left = 1;) Y) x3 u2 Z C q4 K* j
- }
2 d/ ]; C/ d1 R+ a - if(D6 == 1)
; o0 {* s" G! ~9 \' A - {
* q' N( B4 l- T; W4 ]3 C: S6 d - Left = 0;7 [& d8 b" l/ I# x: ]: h% d6 ?5 k
- }
( B; f( l l" q1 z2 g3 v - if(Err == 99)* z6 J. C( p' W- m
- {8 [8 r' `7 a! G
- if(Left == 1)
; G& g" ]& i7 j, c- k - {' _. {! K% T( F; G& Q/ M
- Speed -= 5;9 g2 e+ K% w; }
- moto(0,Speed);
- Z0 ?* M6 |5 } - do
3 C3 T, Q2 q' G) @& [4 Y) | - {
/ t' A* y- J' g4 b0 R+ I - Err = getErr();& A9 w2 i5 a& }0 W8 m+ X6 D
- }while((D1+D6) == 0);
+ u7 m2 a, l8 q9 q8 q1 ~+ G! ~ - }( y* _: M4 S; w6 U
- else. F9 u) W" k8 n9 F! |- j8 {
- {
; H: E+ G G9 A3 X$ {- E - Speed -= 5;
9 R" V6 l% ] O, h - moto(Speed,0);
4 \8 D A# |3 f/ i! f - do
( _. j {) `/ Y* Z - {) E3 l3 m% y" Q( U# Y5 e- E" k1 F
- Err = getErr();0 Y/ k* G' M5 H7 E6 I4 r
- }while((D1+D6) == 0);5 ^& R$ O8 k5 C! q8 B2 f% Q4 D: j
- }
. u9 s1 V/ W! M - }# |3 s3 l$ q5 B. f' R
- else) C4 ^+ V3 @, j5 L
- {1 R- c2 d7 }5 G3 W/ `* S
- if((Speed < 160) & (Err < 2)) Speed+=1; 1 }9 C$ \7 F) {! f
- if((Speed > 100) & (Err > 2)) Speed-=2;
: {# ?$ H( V7 t8 P! b - integral = integral + Err;# W, V5 k' ^0 X
- derivative = Err - previous_error;
4 n: N; v$ b' u: q$ F( n - output = Kp*Err + Ki*integral + Kd*derivative; W: O+ z' X z% y5 y
- moto(int(Speed-output),int(Speed+output));1 [: N9 L2 j5 I! o; h' A: X1 L1 |% `
- previous_error = Err;
7 h! \/ @, z8 F% l - }
" O6 M) V8 U2 F! r4 `, M - }while(!((lightsensor_2.read()) < (10)));6 u, L- f9 p. l6 _
- moto(0,0);8 }, s$ G* N+ a6 i
- delay(500);2 b% @/ Q- j; G$ ~! l% C) ~
- buzzer.tone(262, 500);8 ~+ N, X, }# f1 v. G5 Z* K
- }" h: o/ b7 p! |- Q
- 6 b3 Y+ P; O+ V! l
- int getErr()' \/ J! U$ J$ G/ ]$ ?* F
- { 9 E$ x% F- v& H6 D2 s
- sData = linefollower.getValue();3 e# Z9 T! j/ [1 u2 H) h" w# m
- D1 = ~(sData>>0)&1;
8 i9 @( z3 `! v - D2 = ~(sData>>1)&1;
8 M. p9 a" ]8 a1 d% G - D3 = ~(sData>>2)&1;2 q- a( ^1 o4 P2 I3 H3 b* V, X
- D4 = ~(sData>>3)&1;
+ p. j5 D$ _( X1 g# C, G) S - D5 = ~(sData>>4)&1;6 j( t* @% C% J# ^0 G; \
- D6 = ~(sData>>5)&1;( P# u& E& Z( g4 ?$ c
- int downD = D1+D2+D3+D4+D5+D6;
9 w7 q5 x+ m, \% k - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);4 x! E1 I c3 r4 B
- if(downD == 0). } q$ q; h% V3 j
- {
) `. N, P! D( X8 X) U - return 99;
+ }% ^$ i: [6 P - }4 h1 a3 q" u, A2 Y
- else
/ Y- L+ a( v. {) L6 O' @ - {1 T8 a2 _5 i c5 ^' f& C
- return upD/downD;0 N2 C8 R; J# e2 E4 J, t$ k$ R
- }
: n# o+ n1 b0 ^' F* m8 w$ n0 a$ b - }" X$ l' x8 A& T2 b
- : m9 [" O e7 E" h2 b/ `' {$ s# R
- void moto(int Speed_L,int Speed_R)
0 m! C! A" l! g. B* K - { @6 n- G& D6 W6 }$ \" J* O
- Encoder_2.setMotorPwm(Speed_L);
+ [! [8 n' j8 U6 W( W3 f# l. ^ - Encoder_1.setMotorPwm(-Speed_R);9 }( H% l5 z, Q& X3 S
- }
複製代碼
! o; ^- [( a/ E( _MeLineFollowerArray.cpp
! k4 S7 z9 t9 k! F z7 V- #include "MeLineFollowerArray.h"
" W0 I+ E8 B9 {; x F - + q8 b3 n/ J1 r( W+ }% B
- #ifdef ME_PORT_DEFINED; m, p% |, `4 v+ q3 q
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0), m& B4 m# z, Y4 K
- {% R. H- r4 o2 y4 {: `0 r
- x2 m. v Z. n; ]4 |6 P0 G: Q- }
) ~6 F) q" h/ f4 [; ]; ] - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port). A [ r$ [) {1 b! i
- {
t9 d6 K1 P; }# R - _DataPin = mePort[port].s2;
2 t8 g( o; o; g: | - pinMode(_DataPin, OUTPUT);
& F' q: u( Z2 Y$ t# o - digitalWrite(_DataPin, HIGH);. O/ H9 ^6 q2 Y F1 y U' z* i
- }' W+ L, J1 c* X7 b7 x; |8 o
- #else // ME_PORT_DEFINED3 R' p2 p) l) R1 k( o+ I8 J
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
# N w5 P, x) @9 r/ ~, _; e - {
7 p5 C: U; D( X* M - _DataPin = pin;
# d0 E/ o8 F# b - pinMode(_DataPin, OUTPUT);
3 i. `3 i0 e. F4 l - digitalWrite(_DataPin, HIGH);
8 V8 o6 {/ X1 _5 g& o/ z) J y4 ?: B - }8 O$ c9 E4 m7 K
- #endif // ME_PORT_DEFINED
# @) c O! |" _! h3 K* z
3 d& V" a) `6 i
5 X2 F$ m7 }1 W9 V) \: z- void MeLineFollowerArray::setpin(uint8_t pin) _/ P0 S P9 ^5 d$ u0 {$ w
- {( h* x: a0 a+ i* s) W
- _DataPin = pin;
+ @0 d( s# g' q - pinMode(_DataPin, OUTPUT); " ^ @" l% ^$ f2 I0 ]: j
- digitalWrite(_DataPin, HIGH);3 K2 g7 P' e# D- t
- 3 N& V; a. J% {+ A7 f: F
- #ifdef ME_PORT_DEFINED9 V* l! r4 g3 ]7 T+ W% r9 ~; M
- s2 = pin;9 l) H$ c6 T2 k0 |! t
- #endif$ M) |) `" x" J, Y/ g7 [6 p! b
- }
! |0 b- m1 Q/ v/ T* r( W+ h - / @9 b( ]7 o/ }( P
- uint8_t MeLineFollowerArray::getValue(), L- O' `" P& s6 n& e, x6 }" L
- {
/ `% G* X4 Y4 u) h+ w* z - uint32_t LOW_level_read_time;
! @3 M# [2 @2 P: P - uint32_t HIGH_level_read_time;) s( L$ [! P* i1 @7 j
- uint32_t time_out_flag;) B$ w+ O" {/ i$ U; S
- uint8_t Sensor_Data[3];
& w( p" O' f4 G' l- E - static uint8_t old_data = 0xff;; A I4 A( {% B* `; B j: I& @
- / B. X* {$ G9 z' E; u7 O+ j5 R$ ]
- pinMode(_DataPin, OUTPUT);/ v( y i0 C* f& [1 }
- digitalWrite(_DataPin, LOW);
6 X1 n1 ^9 i7 m% B - delayMicroseconds(980);/ J. y: T. ^4 ?2 F
- digitalWrite(_DataPin, HIGH);
3 Z9 w/ T/ D$ Z
2 I' t9 [; d1 d! N3 D- pinMode(_DataPin, INPUT_PULLUP);6 \8 L |) p! `, }
- delayMicroseconds(10);
$ C3 x6 ^- e1 g4 K
7 }) _" x/ u; c; J2 g9 t9 L8 ]- M- time_out_flag = millis();
+ H9 w3 B0 T. b- l - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );3 s4 C, w& T8 s
- 6 P& s. a. h1 h$ g0 @
- LOW_level_read_time = micros();
1 N; h! ^, Q2 M- }$ Z% U# W2 w! B/ h - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
! a6 k: r6 O+ h& w4 l( I }( { - {
3 ]* d8 p. v% _& i - return 0xff;
0 E/ I# a& ^; l; J# t: r - }
- [$ d. f& D, d% o
) B0 ?3 u( @4 x3 `$ U4 O% _ M- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, R c9 L& U; G# q$ [2 Q d
- D+ c, l8 h8 p$ r9 N* U+ Z! U
- HIGH_level_read_time = micros();
: z( a3 E" u x - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level% q: I8 k0 k2 ~
- ; `9 _. |' C6 K, s
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out" P8 ~3 h3 W2 r+ Y8 x# X. p! C9 F
- {. p5 ~% [$ g3 g; Y( w
- return 0xff;' v0 m3 s- N7 d7 E7 r# h
- }
' X% v; ^* i" u" x: V - - T6 e5 t- u4 Y4 [1 o. c7 v" ?
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))4 y6 C. d3 U, G* k# D9 K
- {
# U$ p ?; J* y - return 0xff;
' q1 ~$ n2 O$ V( } - }+ K6 ~, L: h0 L( z e
3 t a" }1 o. M2 k- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );3 E; d! C2 j% n" G/ E
- LOW_level_read_time = micros();
& I' [( i3 Q) L" Y+ {; u: ~ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level# n2 z- i9 Q# o7 O
4 [, d2 M( ^% d9 P" w& ]" H# F- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
8 g( R0 p4 I7 m4 M5 ]6 @! N - {
9 n L5 [9 c+ i - return 0xff;
+ r* ~6 L+ t6 `6 D/ W- M) c - }
5 B% t& I# x+ ^' }" j
" o9 s5 d0 D+ a* \* _% h- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))! p' p; u" u# @# b) E; ^: A
- {7 b$ m7 [2 a' d4 R
- return 0xff;1 E2 K% ]1 I* Q- p' M& a& j% u
- }. | x+ d5 [! K& P, p$ Q
- 1 a* s& E% m& y7 j
- for(uint8_t k=0; k<3; k++)$ d- A9 O$ g: @: i
- {; M3 m6 t2 O& M# ^4 l
- Sensor_Data[k] = 0x00;
, R8 j8 [8 T( U/ g3 b; {+ b* P - . H- P/ X/ }6 b4 l5 y/ \( T( ~& R$ p' G
- for(uint8_t i=0;i<8;i++)
9 f' a; V. O7 V; q( x3 z0 m - { v3 `4 {9 Y, O9 x+ v) I0 W
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
; V" G; r9 h5 u8 f* E4 m - HIGH_level_read_time = micros();' {/ k6 `- H0 @% e( J9 M
- LOW_level_read_time = micros() - LOW_level_read_time;
0 F" P# Y! ~/ }; |0 e% }' ?8 T E
( d) S4 ~% E& y- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
" K& g4 h. u" Z. U7 T+ C9 g) W+ C - {( a' j- s) \( Y2 v$ S& f; m- p
- return 0xff;$ s: D% `+ }" d# j; _7 E' t+ u
- }! Z# Y& R6 Y X7 e! w! N
- _4 I; R- x/ o4 O" l
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );4 J( V3 q# ? m8 o$ K) A
- LOW_level_read_time = micros();
5 m0 N% h' m* o' z - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level2 v7 p" ?! r& `; k, X
5 q! _5 N2 W; W9 v' _6 l9 k- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1+ _* j: E* A( T4 n
- {
( s1 z ^% H# R$ p u* K2 y) c6 L" E - Sensor_Data[k] |= (0x80 >> i);# t6 F$ T$ o3 l2 }# P% }0 E
- } y; A7 H. p/ t7 o: Z
- else if(HIGH_level_read_time >= 100)
# g+ b' ]! `$ j9 p2 Q% `5 p - {2 e" a s+ M( D& i; v: K
- return 0xff;/ W: T/ m# v; y6 l( @+ ]$ t& Z4 [
- }
( q+ p% n* r% R
0 O/ H% b& V" q+ N# I) @* q& n- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
1 Q% h/ N. k; X2 P/ `; c: e0 K - {- u: I; q$ n" Y: W: m+ ]
- return 0xff;
4 E+ D% s+ U& ` - }
2 K+ y) r( P& \$ n+ @7 ~ - }
) ?' O4 M& ]2 K7 ^# I - }
5 l+ _7 Z( L" h( g/ c - ; v5 ?- e2 {+ P
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level* f1 ^% ?8 j9 I1 A9 {. l
- HIGH_level_read_time = micros();$ @+ O; A$ C# u7 R3 U0 ?/ R C
- LOW_level_read_time = micros() - LOW_level_read_time;
2 @% \/ F5 |8 ?7 W" u3 T2 J - # w- v _6 Q1 [8 r
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )9 k5 |# M' C# g+ }- G
- {$ D. a7 M1 S; j0 r: q+ w
- return 0xff;- V) F; E5 m! p
- }9 X4 E i0 d6 R, z* I
- 7 A- r" B+ F8 g! G# q. h& K
- pinMode(_DataPin, OUTPUT);. z4 Z) S% L) Y4 Q: S( e$ a
- digitalWrite(_DataPin, HIGH);
u* }7 ~1 i1 N# ^, f - ( E2 L7 o$ h* R4 K, W. V5 d& E
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
% q1 }( c2 e, F- d9 p - {
% H2 X8 V: g6 P7 f - old_data = Sensor_Data[0];
- A/ f# x- m; L0 h' H - return Sensor_Data[0];
8 t9 t5 d3 T' r, C* \* N3 G - }
- I9 N- B3 [8 a" s: h% z - else
8 k7 b1 L+ T8 \/ i# \ - {3 g9 E* }& c$ I8 ~" i6 H" ^5 y& S* ?
- return old_data;% |0 L$ `0 K. E
- }; A2 Z6 _2 a: H. `/ u4 U. a- C: f
- }
/ F( m2 R ?6 j: A) ?* s
複製代碼 8 }0 ]6 [! F6 [1 c' @ L
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
" W/ Q* K6 v' E4 O5 r# l/ F$ Z
) _7 j0 V! _( S, b, S |
|