|
|
+ U9 l7 G( T5 l7 G ^1 v
- #include <Arduino.h>% R" s% q+ E4 D# M! k$ ]' T# S
- #include <MeAuriga.h>6 v! j. V$ T8 m
- #include "MeLineFollowerArray.h"
1 ^$ _" X$ _+ Y) w. q
1 [& b/ N& t. L) x7 O- MeEncoderOnBoard Encoder_1(SLOT1);4 F9 L0 ^$ X' X Z. [
- MeEncoderOnBoard Encoder_2(SLOT2);7 l( H3 V- J8 J5 ?
- MeLightSensor lightsensor_1(12);
, g1 N e# |( T* j" c7 v; ] - MeLightSensor lightsensor_2(11);
4 S- Y9 N! X: B; O1 {1 \+ H; t - MeBuzzer buzzer;
+ J2 L* Z6 Y- h6 D5 f0 k) |( P - MeLineFollowerArray linefollower(PORT_6);
. Q* I! L3 Z5 B0 y - 0 l$ S2 T3 s: P. H2 u/ M
- #define Error1 1* i3 U1 T1 `4 T7 H) i7 {
- #define Error2 2
8 W4 Z, m# Y6 v: P' e# D - #define Error3 3
& m! ], ~$ }3 v( K& J - . C6 F" W! \2 c
- #define Kp 15' s" a) h7 t$ \0 D8 @9 R' i' @9 g
- #define Ki 0.15
3 i5 A/ Y' J! a - #define Kd 0.03
$ X' l9 @) u; L
* [% {; h/ X# Q) ^( ?. e- d- uint8_t sData;
) N) U% P6 S8 {( i - uint8_t D1;) t, |% h" U3 B6 T
- uint8_t D2;4 y) V/ F% t- N" [4 x3 W
- uint8_t D3;0 x4 h4 @& }; m: B/ A: q
- uint8_t D4;; u6 @9 B1 M3 E' t' W4 Q8 N, R& Y) R
- uint8_t D5;
@1 L& M3 h3 H: I6 o$ F! Z! r; K - uint8_t D6;) M# w. Z0 t& U+ }3 i- `+ T
- ! L7 d& h { Z# `* `
- float previous_error = 0;) `* G; s. e* S& ]# E
- float integral = 0;
* j3 a, ]0 [( A1 {& E* n* o- x! d6 l - float derivative = 0;
: g9 x( e9 p* Z+ I+ q, i - int Speed = 160;
9 v, N5 }7 r) w; Q6 n. T9 R - float output;
0 M/ f2 O7 r z
5 M9 r; H: A$ o% q- byte Left;
$ X+ c) X9 f# ]; ^- B
& t+ Q, S# ~" `' | N- void setup() 5 N5 M* K( w$ Z
- {' {' Z# n E- f+ l; W
- //Set PWM 8KHz5 n5 k3 P" [& s4 V
- TCCR1A = _BV(WGM10);
) a' L% }/ R+ F$ q8 h - TCCR1B = _BV(CS11) | _BV(WGM12);2 x* n) g, Q5 s* v: C
- TCCR2A = _BV(WGM21) | _BV(WGM20);
1 a' u) H; j7 r! U - TCCR2B = _BV(CS21);
# i. q5 y6 C* P4 i - Serial.begin(9600);
& t% z9 T* S2 ~0 j - buzzer.setpin(45);8 [- D# [% ]( X: U0 k+ K
- }$ [; }: a. }+ Q4 v4 S( p
! Q& d; N& b b! U) H- void loop()2 H) c' e/ c% G. l( \
- {
- c7 e, l5 V; e$ ^- ]- h, s K: P - while(!((lightsensor_1.read()) < (10)));
3 Q* ]+ M x) z7 m: M! T0 m. @' | - buzzer.tone(1047, 500);
1 M3 ^% S1 G+ K( M - delay(500);+ h4 S4 y! k- a3 D4 Y
- do1 {& q# n7 y3 @/ R5 i2 Q
- {
+ p) y( a6 j9 @7 L }2 h% T3 c( c - int Err = getErr();
. y% o1 B# P, Y: v$ w3 m - if(D1 == 1)
/ o4 q6 m) M6 W - {! [& P; @- @! i8 H8 h4 q! U5 ^
- Left = 1;5 ?- @2 k' x) }7 n- g. g& M
- }
( z: X3 t7 F# H- ]. \: l - if(D6 == 1)% }3 L) e& ]4 V" n$ O" t. B
- {
, N A; T8 t; `* M/ X - Left = 0;
4 d+ b( r( N9 N, q, C - }- o v# K m* n! y4 y* ^5 _- e! `! U
- if(Err == 99)
/ |9 N. g: N! `5 h) D: L - {- F' q: J1 ^0 M
- if(Left == 1)
4 E* Q( H# |% h - {$ A4 F7 v0 R( U: T# K. k4 W+ m+ Q& N
- Speed -= 5;
/ g4 p* C3 T% {: J2 W* T - moto(0,Speed);
: D+ h2 D, Z& N+ M( \ - do
3 Z. o9 o6 r4 f, x9 |4 _ - {/ @& w( b9 K- b2 T# o. G. G
- Err = getErr();& H: Z7 V8 o2 e! ?$ n/ J
- }while((D1+D6) == 0);
7 V6 D4 w; R! n! t% m - }
7 J4 A; x$ T8 v5 W% @/ s2 K - else M( J6 F( y) E" U
- {
' d. z) Y2 z8 @4 B( a8 F9 Y8 t+ T - Speed -= 5;
& P! E, d) a. ~' M8 H - moto(Speed,0); % e/ }$ J6 w1 w, o; S
- do
5 a- D2 I" }0 c" F S/ z: B - {: g5 Y; q: d+ b# E. s; T
- Err = getErr();
6 F! N. X4 ]# S - }while((D1+D6) == 0);' I2 h$ J1 W8 f& K" t$ S* z
- }# D" J" y3 b m
- }
$ y6 g0 z( I& e) E5 x - else
/ R* o7 h! Y6 a% T3 E# t - {
! ~5 Z' N1 c- e% N" b: Q" u - if((Speed < 160) & (Err < 2)) Speed+=1;
1 V0 N: H' v! m0 z: D+ Y! {' Y8 t - if((Speed > 100) & (Err > 2)) Speed-=2; ' O* T" h7 t8 \% a$ Q$ [
- integral = integral + Err;
& s1 F5 Z+ u8 \ - derivative = Err - previous_error;8 g9 S6 f5 A* z, z, @. r0 `
- output = Kp*Err + Ki*integral + Kd*derivative;
4 ?! c: r3 w x! c7 A - moto(int(Speed-output),int(Speed+output));! m$ G4 [: j9 g) J' ?- i- {" m
- previous_error = Err; m1 Z8 w( A' [( r; ]7 ]& D6 B+ k& [
- }
- R, v7 A6 ~& ^& l0 g - }while(!((lightsensor_2.read()) < (10)));2 L- ^, }# s' F( m. @4 W
- moto(0,0);) J( b1 f. n6 j9 g- p9 Q) l& @
- delay(500);
* S) }: i& M1 T- g - buzzer.tone(262, 500);
5 \9 N4 j! t2 l - }0 F, m# M/ H+ j* y% k5 ]
- 1 g4 s& J/ r- D* M- i
- int getErr()
^; _: Y3 n2 h+ { - {
/ ]. Y; R: \/ K; O - sData = linefollower.getValue();
+ n9 U7 d3 @8 I% F - D1 = ~(sData>>0)&1;
. Z' ?: [5 B) k9 T - D2 = ~(sData>>1)&1;5 l. n1 I6 |9 B" Q$ {9 d; }
- D3 = ~(sData>>2)&1;5 k. H" I% o3 a, }! I) j
- D4 = ~(sData>>3)&1;0 k+ d {- b, t+ r+ D7 U0 P, K9 B
- D5 = ~(sData>>4)&1;
# C. M+ z4 Y: q; J( |/ o1 E - D6 = ~(sData>>5)&1;
: U' `% g# Q/ p/ [& [ x - int downD = D1+D2+D3+D4+D5+D6;
* d4 A8 v( d5 y4 i5 O i - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
. L7 f6 i r) Z8 B - if(downD == 0)
' Q5 o# ^/ T9 \ G& Q - {
4 N x, N+ j$ w) o, ] - return 99;
; A" H+ A9 _! N5 Y - }$ T7 C$ a g; h- W5 I
- else; M) p% `1 l9 g8 J5 n, @
- {1 D4 E5 ~+ i1 z
- return upD/downD;' L# M! C+ z; {( ?
- }# T$ d) ^: V) x. y
- }9 o& A/ R% i* S" }: n
- - b( ^; q) F6 a3 `' I1 r
- void moto(int Speed_L,int Speed_R)
8 K4 R0 I, a5 _' e& @ - {
( w" i- C1 @+ ?. t - Encoder_2.setMotorPwm(Speed_L);2 \ G+ |4 ?/ b# W
- Encoder_1.setMotorPwm(-Speed_R);
/ Y$ ~+ P$ Q4 J3 B+ b - }
複製代碼
0 X4 r6 X& `9 p3 j, ?6 C" L/ OMeLineFollowerArray.cpp
+ }: \+ k+ v! D/ y0 K" Q- #include "MeLineFollowerArray.h"
- g& \6 E$ b$ c- |) ]8 h& G) z - : I5 B( O7 W3 J' }* w& i
- #ifdef ME_PORT_DEFINED u$ e# B/ `: v1 q7 g1 m. b
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
1 L9 q; c' {4 M' ~ - {
7 I7 w+ a+ v# w2 X+ h& O- q- u$ `
( m" j: Q( L6 \9 r. Q8 b/ {: z- }4 A; K4 H8 Z# w
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
% a0 C6 L* A* B; `& Y/ F& ~- S - {
8 T& ^8 q; F }" b% ^$ [: p - _DataPin = mePort[port].s2;
\& ~. h7 r4 u, {& t - pinMode(_DataPin, OUTPUT); . H5 L( c1 X/ Z% f
- digitalWrite(_DataPin, HIGH);
/ h7 Q k" B/ Z0 z# k( S2 M1 y - }
, b& o4 w' d% \3 v8 x - #else // ME_PORT_DEFINED: `3 z, U% k: T) M6 I, i
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)) I% a, {' h; x. L w
- {- v' p+ {/ n" D n- e
- _DataPin = pin;
9 E8 A; H$ b" y! l - pinMode(_DataPin, OUTPUT);
) W5 A; W" |9 X, l5 q R2 Q9 t - digitalWrite(_DataPin, HIGH);
- N4 T1 E3 |, a9 x - }( G1 z! k! V- R
- #endif // ME_PORT_DEFINED
2 e& r e; M, V7 W7 P
s4 A' c+ r& h- m) L6 x2 L; U- 3 c+ f6 S0 b6 J, a8 f4 ]. ~
- void MeLineFollowerArray::setpin(uint8_t pin)
" `9 |% y" @8 i+ c - {2 Y O5 c9 m* p; I* N" o
- _DataPin = pin;7 h; n& z3 l# b) _
- pinMode(_DataPin, OUTPUT);
* W3 Y5 A- x6 p; I6 l1 d7 ^! B; G - digitalWrite(_DataPin, HIGH);5 [0 K! r c) E' v7 W
- ' h/ W+ G* T1 w- _
- #ifdef ME_PORT_DEFINED6 S7 K2 I: W5 J4 v/ g" a
- s2 = pin;- w1 E i {2 p( W+ b* r" g
- #endif
+ }$ E9 B; {9 D0 W" Q - }
8 C2 f5 ~5 L* o6 p3 y
! s1 F4 d/ ^1 o1 d- uint8_t MeLineFollowerArray::getValue()
8 _# Z7 k& r! ` v) l E' L6 I4 a - {
0 Z$ e. M* V. j3 b7 `. z' R - uint32_t LOW_level_read_time;5 E8 o; O+ k8 t% ^9 m3 W5 d
- uint32_t HIGH_level_read_time;
) H% R* B5 Z0 o' q - uint32_t time_out_flag;0 Z. V& K f8 E
- uint8_t Sensor_Data[3];; {" ~) D/ w2 o' K* k
- static uint8_t old_data = 0xff;
- |" m* [$ b. Y/ A. t+ h8 h - 0 y |6 g4 o# L
- pinMode(_DataPin, OUTPUT);$ V/ j0 G; N- r
- digitalWrite(_DataPin, LOW);; J0 `' p) _+ X/ [ M/ W! Y. _6 S/ i7 F
- delayMicroseconds(980);! b! ]3 o, Y& q& K
- digitalWrite(_DataPin, HIGH);
/ T: M1 ?, z; n z k. ~+ o8 Y' s8 i
1 E1 W( m5 I: I* g/ ?0 Y+ f' @! x- pinMode(_DataPin, INPUT_PULLUP);
8 ?/ O3 e$ ?9 ^+ I2 B - delayMicroseconds(10);
8 ] J/ b0 j; y/ J
" u- l G" t+ k3 M- time_out_flag = millis();5 J2 U q5 Y s0 G- G
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- o1 ^% L+ A9 [5 g9 z2 \- r - ' W& t* q- _0 b, Y
- LOW_level_read_time = micros();6 [9 ~1 `2 z% e3 h8 i+ ~
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out: v/ y7 B) S% c a( s& x! I
- {
, u) F# ^3 V7 Y) R - return 0xff;5 v& L. M1 I! [, l7 x1 v' [
- }
: q& B* Q+ M. ?2 V: a - ( H! U" u* f7 {* W& O9 |
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" I: E6 p; t `0 s1 o5 @& Y
! q. I5 i8 k% e: W- HIGH_level_read_time = micros();, t0 F1 j, z3 i& E- P0 S
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
2 L5 q& Z) H( Z7 Q- o
& s3 X C( T+ }. T* q$ G; ~- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out {/ V4 q( t3 u! c- l: A8 ?0 m
- {
7 i3 }$ e' u6 C9 _* W) n8 `! j - return 0xff;
. h/ }% H" O/ |/ y3 E( ` - }' T: E& v# |$ T, x) Q# ? ^
- " `0 Q# f" s0 H4 z' p$ H) S
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)), W5 W4 O' g r+ V
- {) K k: E- Q( I8 O
- return 0xff; B$ F- l/ w* J7 F
- }
; j( T, x) h3 V9 A# r: w; h& H - : h5 l f8 A( Z1 I% z9 U8 c
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: M# Z' [" j! f+ A. g1 C% r
- LOW_level_read_time = micros();
5 N" X1 F% F2 b5 w' [0 [ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level! ?: q \# O& T* S
- 8 \4 H8 u( j/ ]" p" o7 j
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
( m0 c2 n+ v- R - {
8 I+ l, M( d. g. C! V2 @5 K - return 0xff;
4 d# g O) U1 J. b: O: x - }' d& { \4 ?$ z# G: g/ _
6 S9 W, m2 P3 X, j7 t* _: H- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
1 l' P. I* W, B; ?& U O. h - {9 ?) s" @: H# a
- return 0xff;, i$ V& a7 v+ F& z5 Z' f/ h
- }5 L; K% x% h# l, i; O$ S! Z! L. L
- % ~+ q) E8 G3 x5 a" W
- for(uint8_t k=0; k<3; k++)
1 L! u" o: g2 H& _; k8 D/ H - {
+ A- o# G" a6 C: o" t4 h6 l) Z - Sensor_Data[k] = 0x00;2 w" R1 y( }& G: Y3 V
- $ G% v; {. X% ~6 O7 e. u
- for(uint8_t i=0;i<8;i++)& H x6 a) y9 n
- {
( ~) L1 c; F/ w+ L$ Q) t8 L - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
* j1 C) v/ l; h7 C$ [ - HIGH_level_read_time = micros();
: E H( @9 z- }2 [; N! @# u5 y - LOW_level_read_time = micros() - LOW_level_read_time;
! l) T1 k6 z. v7 t - , D/ W4 ]' ?7 J6 L: f) i) a& O* x" J* c
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
n# |. r- N( ^ - {$ [8 n- ]! J5 v
- return 0xff;
4 |' ?. ^1 Y, ] - }: F, j' l. Q' v0 f/ e1 V; s* z
8 P: O5 m1 u- U1 G" k7 Q, t- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 m# j& @- W0 J& t( j0 x- C' f
- LOW_level_read_time = micros();
" s4 X) \" V5 {1 O! S - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
2 x# L; c. T% W w/ G9 R; b - ) z) w' B# X* F, V) A5 i5 U
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1- f# i# r# G5 P
- {
: b1 w- O0 z0 X9 ]: Z N- ^ - Sensor_Data[k] |= (0x80 >> i);. g' e8 h' \0 A2 L$ C
- }- Q, H& r! J& F( ?( n4 x! h/ O
- else if(HIGH_level_read_time >= 100)# D0 w8 l3 v* D5 E
- {
6 G7 E& x5 R7 X! \) t4 E; } - return 0xff;& W. w% ]5 i# W) e/ g% B5 E
- }; P! h" T9 Y' g$ G# o& ^$ f
- - ^6 E+ w5 T7 t! v" }( x
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)5 F i7 S2 C* Q( ~# E
- {
& U* k4 f+ d9 }( s u* V! K. S - return 0xff;
) i0 t+ R# L9 b+ Z, O. P - }$ \6 k3 f' r N4 Z; Z% H
- }' q7 a) l: x4 @0 V9 ]" u/ n$ b
- }
! A! P! R: E7 w, l5 x1 S" k - 3 a! ~* S4 O) M3 O
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level4 v. j: p# S- K- ]9 F5 F( L
- HIGH_level_read_time = micros();0 A: Z9 D. j6 u3 q3 `: R3 k
- LOW_level_read_time = micros() - LOW_level_read_time;
: d: X5 u2 B- h0 o8 m
% B) k; a6 o) L- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
9 U* z T2 ]9 F3 f3 I: b - {
1 ]* L, ^! z' d+ w" K0 p - return 0xff;
$ E, w& I7 P/ z" C - }$ i' e0 L. h+ P- Q& j4 k
- . h- Z1 f8 w( m2 z% F6 x
- pinMode(_DataPin, OUTPUT);: j v- D+ ?. Q- ~
- digitalWrite(_DataPin, HIGH);
7 @. n; }- |4 v; E - - V# m+ z1 S- Y) M/ b, @
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
: p4 k( S+ T/ v4 ~5 m, ?$ E - {
9 K& Z I* r5 m/ Z0 W7 m - old_data = Sensor_Data[0];
" f* r2 H! N/ s" K' B, k - return Sensor_Data[0];
7 P a8 L/ M2 e8 n( T( N - }( n4 |1 Q( G) c! O; D/ c1 R, V2 R
- else5 o; D5 l. w/ U6 J q6 H
- {
, V: x6 V% P$ B9 j - return old_data;
$ ?& \( [' Z6 f* s- o9 O. I+ Y% k j - }$ D( ?) B4 S/ Y7 l' X% G
- }
2 |0 u2 ^& ~! }: h& w; l5 I) \
複製代碼
* t$ v0 N. o$ d3 U; Y4 B2 gMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
% r+ x: N, p7 O
z+ P/ y o, T/ [) X4 O( t5 \3 I |
|