|
5 u) R* s* j9 d( S7 F- #include <Arduino.h>
y1 R9 z% A9 W) H; _7 S I% E% d - #include <MeAuriga.h>8 n$ t8 i$ v+ ^# B8 o
- #include "MeLineFollowerArray.h"
; C# M- S+ w# u7 b% _ - ; Q9 p" H1 u/ q3 N, Y! U
- MeEncoderOnBoard Encoder_1(SLOT1);
$ G& O w2 T4 p9 U$ ]- v! {. E2 c - MeEncoderOnBoard Encoder_2(SLOT2);" t8 c( r% o8 G4 w* i* I
- MeLightSensor lightsensor_1(12);7 `7 f/ ^- B p+ |1 S( Z" k
- MeLightSensor lightsensor_2(11);) X& Y% J9 i: B
- MeBuzzer buzzer;
; q; M/ |& ^" a) t - MeLineFollowerArray linefollower(PORT_6);
, A/ w$ I1 ~' a/ l - & w- f( z3 }. Q W3 c
- #define Error1 1) x- s2 ?( |8 E
- #define Error2 2" S& J/ P- ~) R' R Z
- #define Error3 3
3 |2 U: Z) J9 ^9 U
- R2 ~0 C9 X9 W- #define Kp 15
) z7 {& o7 l$ I9 ?3 u9 _( T - #define Ki 0.15
+ { G. o. t6 s' G$ I+ s: O2 a0 X - #define Kd 0.03
: u" D, [3 c) Q+ j4 \! R' L - 4 b; `: \* X% j
- uint8_t sData;
* X+ E4 q+ ~+ v, ~$ b - uint8_t D1;
, y5 o4 p: ]& w, M: |# ~ - uint8_t D2;
: e. A1 t2 t; b; F3 [' s - uint8_t D3;
/ y, k2 O c4 q - uint8_t D4;
/ X* I$ W, s2 }& t) F - uint8_t D5;
; e- @7 }4 e4 j$ H! N' L4 Y - uint8_t D6;1 r+ W# R# L. |& L
- z& ^9 [5 E6 V3 J" w- float previous_error = 0;3 y" i7 c% ?! P3 h# F( @7 o# u4 Q/ n ?
- float integral = 0;
& f/ i# [; U3 w' O1 R+ | - float derivative = 0;& t1 }( @( K! _
- int Speed = 160;; }% u' G, ^3 f2 P( L7 a+ `2 Q/ w
- float output;
1 j: R6 @; S0 \% X! ?( c% h
, E: X! g8 Q4 J3 V+ a) P. B8 B- byte Left;
: R& A6 |: d/ s) t! p4 T1 ?7 W. B/ o - I7 V( v" u; l% _0 ^
- void setup() 9 r6 W L2 x; n3 N$ z
- {
# R! v+ s0 H& [* P, v$ U/ B - //Set PWM 8KHz
9 Y( e$ @; U, }6 _ - TCCR1A = _BV(WGM10);
1 j4 [) |. |, h9 J - TCCR1B = _BV(CS11) | _BV(WGM12);
& A9 y. K9 m/ v% b7 J - TCCR2A = _BV(WGM21) | _BV(WGM20);
: c$ i1 C! z7 _/ _6 E) B - TCCR2B = _BV(CS21);
( e. }6 I3 u) |9 F: N - Serial.begin(9600);
+ C# F. G- Z& F! [4 @ - buzzer.setpin(45);
: k; K" `+ h; J% v - }/ D, z+ y& P" F7 k; l' z3 c4 o2 G
6 k, k) S- Y" s9 P1 [- void loop()
6 I$ x* a; h3 z( [' l+ e - {) ^, m- ?$ s: `
- while(!((lightsensor_1.read()) < (10)));
/ c: V- D; } F# h7 m* @ - buzzer.tone(1047, 500);
5 W2 h# k9 v* E' W _- M! P - delay(500);
+ D. }3 Z2 ?. Q; C5 B, [/ ] - do6 K6 F3 N$ `1 P1 ?1 x6 ^, U
- {
Z% ]8 M4 G" w - int Err = getErr();# s5 b8 l! O' _& B) v6 D
- if(D1 == 1)
+ ]; R5 a$ |& U- h; |2 ^ - {$ E9 M/ X2 [9 m6 F
- Left = 1;8 s* Y G. h, g5 D: b
- }. S; q J Z9 Y+ w$ O8 m
- if(D6 == 1)
5 ~9 C& k+ X9 K5 W - {" U7 L- O" u4 Y b. e, C; V5 A
- Left = 0;
" m& `0 y4 ^1 B; h - }; k- `& c9 l) C" R
- if(Err == 99)
. K1 \+ U8 B% T. L - {9 J, a O8 t7 E! D9 @2 V
- if(Left == 1)
( B `, A- H$ O) A' | - {# w. G. k' u( ?# v4 H/ E7 M3 _9 d2 \
- Speed -= 5;
% U5 {2 J& h- {3 _% j- R - moto(0,Speed);0 ^+ j0 \) H* K7 E6 M
- do
1 D# D# T; F _, }, i' H+ E - {- n3 l- Q. B3 X" X0 j
- Err = getErr();* [* V: w9 `5 p6 @2 I$ \
- }while((D1+D6) == 0);
7 Z7 Y5 P- w0 }$ g) I) H1 q6 [" u9 D0 o - }
6 w4 ~8 R3 |1 z( `4 U! B; o9 z - else
& b' k! b% `9 v - {
0 j: Q. {: C# z; O3 n - Speed -= 5;
4 q) C& ?$ @6 O4 \ - moto(Speed,0); $ H& s! W. d7 ~4 C+ I: |' j
- do
. F W0 F- f* f( V! r) K0 J - { k. t, |. p0 I
- Err = getErr();
" r$ A3 H% X7 a& F; Q+ K- { - }while((D1+D6) == 0);
) S; j$ ]9 U% g2 H3 c( I3 z9 n - }
. k$ l% p+ Z3 ]1 a, H, k5 J - }
+ `4 i3 u8 V. ^( D/ _ - else
* R& A! l* d3 f7 L - {4 a0 A2 D" q( }- Q+ P9 [4 n* T; h
- if((Speed < 160) & (Err < 2)) Speed+=1;
& Z( K# \, T9 |! c% o - if((Speed > 100) & (Err > 2)) Speed-=2;
9 R6 c7 p* w6 \) r4 h5 y% C$ [ }$ N - integral = integral + Err;: A' ^& w0 \$ o$ C2 u) B, R: _1 {
- derivative = Err - previous_error;
; N/ X7 Z1 l( t - output = Kp*Err + Ki*integral + Kd*derivative;
6 W$ N1 k4 E6 [( n - moto(int(Speed-output),int(Speed+output));# M" p0 G; ?( t. Q+ l# c
- previous_error = Err;' Z3 _) V E1 t; Y
- }
( j8 [- m, p( q0 L - }while(!((lightsensor_2.read()) < (10)));
2 G$ C8 U* ^+ j; f5 x - moto(0,0); I/ v6 I) s9 _3 Y$ B8 T
- delay(500);+ A$ l2 {- q0 W8 w
- buzzer.tone(262, 500);- W- `1 e3 U- R
- }
. Z- H( s: z3 l. R
" C6 z) m1 d. U- int getErr()
, ~, [- F* I! B - {
$ }8 Q- v; T0 W - sData = linefollower.getValue();, J' `7 B7 P) ~: `
- D1 = ~(sData>>0)&1; @' v" `" o' j
- D2 = ~(sData>>1)&1;
* W. w& P, k/ | - D3 = ~(sData>>2)&1;
! Y7 k# M' R: T4 L$ t j0 a9 n/ ] - D4 = ~(sData>>3)&1;
5 j% n- o! D, g( k. W" T, C - D5 = ~(sData>>4)&1;1 V, T x8 j8 d
- D6 = ~(sData>>5)&1;
0 l6 R4 s, O* Y# F - int downD = D1+D2+D3+D4+D5+D6;8 H( E1 V' R: e& p
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);& ?; M: e( b& @/ Q- s; Z% N
- if(downD == 0), g: E" u, y, w0 G
- {
V6 _' E( T5 M* v - return 99;
4 D' n7 u* d% X# g - }4 D! S6 d+ q* G: y2 s4 n7 ?
- else- }( y. x% ^) W* X9 m8 M% a
- {
2 O3 U/ W& _' {' I4 A$ \2 ^# x5 A - return upD/downD;
2 ^. e8 O, H4 l9 f" Q! p. k - }5 R" V- H" R/ n7 E& {$ L
- }$ x7 I' i1 ]) D7 i" o" K* j
- + L* e% Q* Q. X# u& K
- void moto(int Speed_L,int Speed_R)" c7 A& u( ~) B7 _
- {
' f. j' m0 Q1 U; t4 ~ - Encoder_2.setMotorPwm(Speed_L);$ D3 m# J" t! ?9 I% z( P2 U
- Encoder_1.setMotorPwm(-Speed_R);
. i' j& m0 o+ W0 D+ y$ h - }
複製代碼 $ r( m4 }& j, _
MeLineFollowerArray.cpp
; r' J. ]6 [2 b2 j3 w) r$ Z- #include "MeLineFollowerArray.h"6 ]6 h+ \* \9 h( d6 a
- 1 b' y- ?! k) K
- #ifdef ME_PORT_DEFINED
% S( S; F- b* o - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)1 s. g+ l g% w# y: u
- {% m0 c; ?* T' C$ r- V3 D
! \7 h- r6 A& G6 f1 o# q8 K8 S- }
$ y7 z+ V8 d) z7 e/ v: | ` - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port); D0 n" F' J! n. Q4 o0 n
- {1 P7 E9 J+ r% q, o
- _DataPin = mePort[port].s2;0 o1 a: z3 a6 t: T3 q- I
- pinMode(_DataPin, OUTPUT);
. O H! U) H" H+ ^, G. _/ w - digitalWrite(_DataPin, HIGH);5 k. S! z! ~" p P6 B
- }4 }$ e9 G6 q0 V3 P1 m/ c
- #else // ME_PORT_DEFINED
- m Y. Z( G9 q - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
! M- U' l: H' H - {* `7 e0 q# M! s2 O3 g8 `
- _DataPin = pin;0 J) d) b9 T; M6 D* M( c& E; R2 ?! i
- pinMode(_DataPin, OUTPUT); 9 n: g- F* R( d% u6 D- z u
- digitalWrite(_DataPin, HIGH);
- C9 _% T6 E8 h! {" E/ ]8 H - }8 c9 R# |5 u7 A" ~( B
- #endif // ME_PORT_DEFINED
9 `1 A1 X: H' r' k - 7 Y. |; o) e' m# b% {
4 @0 s5 F0 h- O- h- void MeLineFollowerArray::setpin(uint8_t pin): ` d/ i1 u( v6 B$ w% K5 C
- {" d& t) ^* g: G/ q* J
- _DataPin = pin;
) d- I0 d( H- A$ p* z - pinMode(_DataPin, OUTPUT); , O$ U3 R7 f6 |3 U, F9 k6 K- ~2 _( t
- digitalWrite(_DataPin, HIGH);
/ ~ Y1 _9 K- X5 [, ] d$ g5 {: Y - 8 Z' H- p3 P! @5 p" R [
- #ifdef ME_PORT_DEFINED
q# U2 [3 k6 p' R9 E5 @$ b: w3 ~. J - s2 = pin;
8 e4 G" g5 W8 Z! A4 d: D; [ - #endif
9 k0 M1 Y4 Y; ^( F n4 d - }
+ d8 g% K. k2 E - $ P3 w# `: i! T6 Y ?
- uint8_t MeLineFollowerArray::getValue()
' w1 K0 y& O, l$ o: H5 r+ A7 h - {
8 p, Q$ u6 ]' k2 h) a) X - uint32_t LOW_level_read_time;
( A0 ^& p4 p9 b; m% i. D( ? - uint32_t HIGH_level_read_time;
I* T V, K$ F$ [: h. `" m - uint32_t time_out_flag;
* E% Y' n% d: \ o0 r - uint8_t Sensor_Data[3];- I" w7 l7 }+ O
- static uint8_t old_data = 0xff;
/ f$ N& k+ @+ S0 ^ Z) N2 V. m - 6 |* q" d3 }. k( t- y- h
- pinMode(_DataPin, OUTPUT);+ `. U8 Z/ X9 A9 I: Y6 l! y( b
- digitalWrite(_DataPin, LOW);/ o C D8 B. l( k
- delayMicroseconds(980);' Q+ i; p6 G8 @& h0 N8 m5 O
- digitalWrite(_DataPin, HIGH);5 I% E7 j( ~7 }) N
- / ?5 s" @$ S# O: A" X
- pinMode(_DataPin, INPUT_PULLUP);
7 v7 \) Y. H B, G$ F" T - delayMicroseconds(10);
4 [: S6 x# M7 s0 h, ^7 [" \ - , H8 Y M$ J' O0 w, t, ~' u/ j- `
- time_out_flag = millis();
( O5 r' C% _8 y w* c - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; f L: M+ O; I/ @ - , s* M1 M' Y: D) U1 j
- LOW_level_read_time = micros();
( r! M/ l$ _6 r! [# @6 i - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 Q4 B0 s$ k) ^0 R - {
4 w+ @, `" O* @, [5 |, b' t - return 0xff;& p- `0 p; k) X
- }1 J# J* G7 U0 G q' O2 @, ]
- + o) n% ]9 K% O! | L O
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( ]3 U6 k# S& S/ z! c5 o5 m) G
$ M; o( q2 \6 g- u. `7 z" D- HIGH_level_read_time = micros();5 u+ u0 b7 L n, @% g
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level/ H$ C& D6 n3 P! ?
- 9 _* ~1 V( @9 A4 h
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
( ]4 l7 ]* x% o - {
: R3 C! G' J# m k& k9 h9 C8 | - return 0xff;
7 X4 t/ j& R) w# i: T! d - }
$ c; D2 j- i7 w: x - ) ^& I0 j0 [8 z6 a) U: Q
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
9 \- l! q( F; F* \. n/ f - {
# n! c4 Y, c* w8 X# P - return 0xff;: D0 Q" ^3 y7 O# ~4 x! n7 D1 D9 {
- }4 X: B" O j' ~# k/ G/ N
- ) A0 B4 n/ E) M$ v4 v7 W
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ j. H$ z# T5 q4 \( x( g
- LOW_level_read_time = micros();8 g5 {+ o# R$ u4 @
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
% p T5 E0 l5 ^1 h% O
( O0 L6 E; f6 @" r5 q& X- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out6 z, D; n6 ^: h9 M% Z
- {
* v" G: _0 Y3 H- Y' | - return 0xff;
7 \& C( c0 T4 F; O! H9 s7 @ - }$ s# T1 |& A! x4 I$ |! @
- " ` `, Y8 M+ k4 B7 m- I- t$ x
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
( f- ]7 D# ^2 b) F6 t8 k& c - {
- z- }* c2 @0 ?( ] - return 0xff;: ~/ J Q/ i2 K5 t: _
- }
, Z$ z6 I8 K6 [# {* _
. r# @/ Y( r! n R6 L+ ^- for(uint8_t k=0; k<3; k++)
, ~: e* r/ }; ?/ E* y - {
/ I" W( i8 j+ Y- @ - Sensor_Data[k] = 0x00;2 k+ N. z \- n+ v3 R) ~; H8 Q
- 7 `9 ]1 `" I1 m. e0 K; }0 ~
- for(uint8_t i=0;i<8;i++)
9 T6 a+ e6 h/ o2 {6 V, u; D - {/ h3 e% W' ], m. p9 `, e
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
' f! W( o$ L3 t - HIGH_level_read_time = micros();
1 K: w- J2 a: E9 k; ^- ^ - LOW_level_read_time = micros() - LOW_level_read_time;( s! _3 L4 A$ g1 t9 v ]) |
- % b* r% {2 V7 w' l- y
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
`( S% r4 y: t" ^ - {) i. N9 O3 ]7 Z$ ~5 \' O) @0 e' e( u
- return 0xff;
5 E5 w9 ?( V5 [: ^* ] - }
3 I$ U2 h+ Y0 ~8 C- z K - $ s& d9 B5 A0 H9 P
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( @ F a0 [; @( i% Q
- LOW_level_read_time = micros();9 q; a/ I" f0 \- w/ x, b
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level! z+ I) C4 o+ e- t
- / ]: I# j& k' l/ _2 B7 v
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1' m, q" s7 i4 S# P& K+ O
- {
3 V7 T. x" m/ [/ g) C7 B - Sensor_Data[k] |= (0x80 >> i);) M+ V7 v% v% z; m) ~2 ]0 h
- }
' ]2 _" M" I' t, r - else if(HIGH_level_read_time >= 100)
' k2 J) M) q. l5 b1 w* d7 d* c - {
. N5 i0 i; [5 r6 V- u* } - return 0xff;
$ P# h5 ~8 S L9 I7 x3 F) E3 v3 h - }5 N0 u) }" r R$ r7 t
% }7 k1 x+ d+ ]0 k" U3 f, K- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)* U2 L4 m: [) W4 G
- {
; f' [5 O* S1 G1 E6 w) L - return 0xff;
3 G1 f/ a n. l# U5 U - }/ J& S. I" T8 ^1 j1 m2 S% e2 u
- }
8 @7 o" a! p* I+ g# d1 P$ _ - }) E- {1 u) u" k2 s1 O
- 6 e/ f% }$ b+ M7 f* F
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level5 h6 \; g: d( E0 |7 G2 ?2 n6 F
- HIGH_level_read_time = micros();% s- m! G+ N3 w9 U
- LOW_level_read_time = micros() - LOW_level_read_time;2 l# a: W9 e: O. R/ ?& U5 {; N* t$ ]
- 5 Y6 i! S. d7 U. b
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )' }4 d2 y6 u/ T7 y# z6 o+ T/ }
- {7 q3 k1 p2 t" X* G5 H
- return 0xff;
S% f. _6 l3 q - }
2 b9 q* }/ B4 Z5 h1 p: S& q1 T
; `0 k' a" T5 X5 F" R- pinMode(_DataPin, OUTPUT);% [$ J2 l9 G9 o6 x9 A0 ^( l# P* f+ v
- digitalWrite(_DataPin, HIGH);! P8 P0 v+ p3 {$ K8 O' i
- $ _' S( K4 M# T: |# r6 w
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
+ g+ _& {% G5 e! ?5 L6 H2 s - {- q% V# O% w4 d% k
- old_data = Sensor_Data[0];
4 \# E0 O7 i7 {# c. k - return Sensor_Data[0];
" c% n3 ~3 j9 f) J$ c6 ^ - }
7 |! k7 n1 P# T$ a - else, Y d; v1 {8 D- L0 p
- {
6 ~- D, W7 u& R8 W$ p2 F, u - return old_data;6 u; u6 a7 {3 Q- K( B- A2 t
- }
5 O0 X# l% q7 r; D& J K, ` - }' u/ k4 ^6 ]+ C' Q' _4 Q
複製代碼
" E1 F- y: G" O$ ?! }, r' W- H& SMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
# m/ w* x! K9 i( l3 U# ~. u: d0 D B# ]: `
|
|