5 Q% ~, I5 A2 `0 k
#include <Arduino.h>
) Z5 A/ B8 m$ Z1 q9 ^! y #include <MeAuriga.h>! J5 n% h" n& f0 {2 Z
#include "MeLineFollowerArray.h"
$ N% R6 {# H+ z1 U& V 2 s/ A5 K3 a& o# W- s- u* T
MeEncoderOnBoard Encoder_1(SLOT1);
% e" K4 W" v# W0 C# E* U, m+ U MeEncoderOnBoard Encoder_2(SLOT2);
2 q8 _. w z- F3 c/ H MeLightSensor lightsensor_1(12);
& ^/ y- T7 ]4 c2 ^# s; N MeLightSensor lightsensor_2(11);
2 ]$ p6 U* b) x* c4 Z/ m1 m6 @ MeBuzzer buzzer;
* N: p; c) B2 M) j* P MeLineFollowerArray linefollower(PORT_6);$ M. t/ b% x5 v+ c
( j& |5 x) g4 w, { #define Error1 1
# I; t0 A1 r0 X. g h #define Error2 2+ E$ Y1 n6 r5 O8 R+ f& m
#define Error3 3) a% g6 G! M/ d- z0 l
. @/ _0 X: S) B8 c6 s
#define Kp 15+ i* L" K0 s, p7 v
#define Ki 0.157 D2 X+ `( L% j E
#define Kd 0.031 [5 k' A# \! w- s# B
0 V' ~1 G x8 E* i6 B- U" f- E uint8_t sData;; y& W8 U6 @1 s8 H
uint8_t D1;
+ K0 I! c* g: W0 g! I uint8_t D2;+ G7 j4 a, d$ m0 J
uint8_t D3;4 f. _# s( R4 h
uint8_t D4;+ w& e; T( l1 r( R Y5 G
uint8_t D5;
% ^2 [" O3 b) t- H* \ uint8_t D6;8 ?! k2 O( R$ S1 q+ l: x. v
( u& I( x8 r2 {' L9 n
float previous_error = 0;/ i$ h( H$ I# }! O- Z+ n" b4 U
float integral = 0;
+ r8 f! c1 l$ X float derivative = 0;
# O4 D* M- u7 R) Z3 M int Speed = 160;
8 b! P- _. m/ t. O: M. U6 |5 f float output;
; Q3 l# q, r( U* u% z $ ]5 _+ W# L# v
byte Left;9 X+ }) E; y [" s- R2 R2 i
: |2 m, U" X- f void setup()
- J1 S1 t- [/ V$ I( L {1 M6 D& ~* z: t3 m1 Z& L4 K+ P
//Set PWM 8KHz
0 I1 x& H3 r _) K7 J TCCR1A = _BV(WGM10);8 u# ^$ m6 n8 K) r( p
TCCR1B = _BV(CS11) | _BV(WGM12);
4 G, J7 s7 S' N4 U TCCR2A = _BV(WGM21) | _BV(WGM20);
/ Z) c" h6 N, F$ u, q1 D TCCR2B = _BV(CS21);! `4 \# f8 |# E$ y- k3 A8 r
Serial.begin(9600);
5 P7 G! m/ X$ J7 B$ a3 P6 @( U/ U buzzer.setpin(45);9 W j0 ?9 i6 x, [3 F9 R
}
: G y7 p4 r+ g8 c+ l- c# B
. q$ t/ e$ `; t( E, Q) }" o' c5 g void loop()
) I7 b0 q& O7 ^& V( Y# M: G+ T7 _ {+ z8 [, I8 N" N8 Q7 q/ i& F! H- ?# h
while(!((lightsensor_1.read()) < (10)));
& M: u/ S4 G# @1 f! t& |8 }; z buzzer.tone(1047, 500);+ B% I$ Q1 N4 q6 v2 g( m
delay(500);9 M/ @# Z" A3 g+ `2 G
do3 r# i+ \6 h( x& b
{* F0 |9 \! W; w3 M3 L _
int Err = getErr();
' }6 B" n5 Z+ A7 ~ if(D1 == 1)
* J& h& t; b( z$ |3 s, v; D {
. c7 R- u. s" D5 v) r Left = 1;
1 T w: H* ?. R: V% O }8 z' t% A' }0 X
if(D6 == 1)
) e) t- ~1 Q5 W8 N7 D {+ s1 y% M$ m9 X' S+ [
Left = 0;
5 _: P1 m, v# ]# {: V }' r4 c2 Z0 n) ~& D- E9 ~7 w
if(Err == 99) t3 e+ O! J$ U: L) E3 i) L
{
' N% z0 Q' Z1 }2 D if(Left == 1)
$ E( P$ x5 l: l: J+ `3 n" e {: k& c) x" Y! k/ y4 O5 S
Speed -= 5;
* a9 \4 Q. J1 e D2 J: ?4 e moto(0,Speed);
* `$ H) R$ [0 H1 `" p# }! f! N do
V3 R& F$ I- S. e2 @' L7 W {8 {: K$ |: B1 D$ O& _% r
Err = getErr();
: q, y7 i- [7 E. A }while((D1+D6) == 0);! e2 S$ K; s" l+ o$ `3 h! j x* P) k
}% [& q: r. z' K; @
else' G$ g" E; w- o7 x8 Q: U
{
5 _1 y. z- o1 R, t' k- d5 w Speed -= 5;
% d; V6 F* j: T( m+ T/ @$ S: V# x3 M8 G moto(Speed,0); ( }8 Y2 e$ Y9 P% o+ M! v
do1 s' |6 K8 A% Y, Z0 g2 \- a
{! D! {, u! I# \( I6 L# L( w9 G
Err = getErr();
6 |% k4 o* v9 o- |3 g! V! F }while((D1+D6) == 0);
/ o# |9 p3 d9 P/ C; w* G$ b9 d" C }; [+ Q# c8 `( B: ^0 V) R
}2 |3 O. F3 s3 w2 Z
else8 k9 S( S- e" I3 l0 y
{
1 i! w/ x. l4 @9 s) q) r if((Speed < 160) & (Err < 2)) Speed+=1;
" b8 m* o+ X3 w" P# l if((Speed > 100) & (Err > 2)) Speed-=2; 9 [) e/ x( b: k: p' G
integral = integral + Err;
- K# ~( R. @' c; | derivative = Err - previous_error;! d' r0 o5 i% e D2 K3 A4 t' j
output = Kp*Err + Ki*integral + Kd*derivative;/ }- \% l8 g/ `
moto(int(Speed-output),int(Speed+output));
6 j" N4 j5 { y @. i previous_error = Err;( R T! T0 U" L& R
}
2 _1 @5 k+ Q* E) [- M/ V& F" P }while(!((lightsensor_2.read()) < (10)));; c1 R* n, S" M7 T7 ^
moto(0,0);0 q r K" @) C$ o" M
delay(500);1 b ?+ @ m: M' R
buzzer.tone(262, 500);9 [ h+ t5 P& D3 W6 h
} N" d: B/ u4 h/ w
) ?' i) o/ E( c0 w int getErr()6 s" Z/ s* [2 a7 y( i# ]4 u
{
& K) ?3 u4 }2 }6 } sData = linefollower.getValue();
, T9 s3 I5 v4 b, ]" c D1 = ~(sData>>0)&1;
* r; Q" z7 S; b6 O, c6 ^ D2 = ~(sData>>1)&1;. \0 u: M: h \# ^1 x' K! {- ?
D3 = ~(sData>>2)&1;0 N) u; k V$ w2 Z
D4 = ~(sData>>3)&1;0 K; C5 A) R0 i- i: Y' {# Q: w
D5 = ~(sData>>4)&1;
5 u, b4 d K2 U D6 = ~(sData>>5)&1;4 G6 @* R- V) N) _
int downD = D1+D2+D3+D4+D5+D6;
; V7 Z0 d- Y# E+ f' E int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
) e' o5 H x& Q' O if(downD == 0)
! R- H6 G; E2 }5 | {
% \# G7 v5 j+ I+ D2 `/ A$ ] return 99;
: U' u; o( A' M2 d9 Z }/ s1 h& l' m' d
else
* s6 ~7 n9 _# T- { {
! C% ^. N" A; |0 B( Q4 N4 c& {, N return upD/downD;
! p) w7 q' ^# d }5 r |3 r! W( ]4 j" {$ j0 T
}
. I8 ?+ c; R: X3 O% x
+ o, Q& y4 n9 D, s void moto(int Speed_L,int Speed_R)$ d+ g/ d0 F% q& j2 h
{) B1 r4 i2 u* i: T
Encoder_2.setMotorPwm(Speed_L);
( c4 @2 @! {5 ]! T3 m* |( } f Encoder_1.setMotorPwm(-Speed_R);
( r7 I8 m4 r- O* _& i6 y } 複製代碼
+ d( F% R: b! \" J! ?' o MeLineFollowerArray.cpp
3 {; K) Z2 i8 {1 c* o! L1 a #include "MeLineFollowerArray.h"& a' c* M! ^& G
: I: P/ T; c c Q! G& V #ifdef ME_PORT_DEFINED
) }9 o3 k6 k7 o5 A! y0 U. h MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)2 { |& H, n7 d# p
{
- L9 u: g6 ?5 Z( y S$ u# q! y
; ~2 U% S. d: c1 y }- M7 W7 D0 l: Y) a, I
MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
! x1 X6 b3 h! U8 h. [9 ^! N9 U {
/ F' }- m" c% W- H9 _6 [& g _DataPin = mePort[port].s2;
0 J) a4 A9 ?$ A1 q- q1 i pinMode(_DataPin, OUTPUT);
1 u( T" |+ u: \) o0 n* z( ~' L digitalWrite(_DataPin, HIGH);
/ Z5 Y' s \1 z" m }
9 k+ d P# S( b* g, m. c #else // ME_PORT_DEFINED
# p9 `: j5 Q2 T% x) C MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
" ]* H: \% C6 b1 Q- S& R {
; q/ V7 D$ t4 g6 D _DataPin = pin;
! j$ j; o4 R& T: H E, A pinMode(_DataPin, OUTPUT);
: A2 |, P Q6 [$ `/ o: { digitalWrite(_DataPin, HIGH);
5 V* ~+ }9 Z' K B; | }
) O" O' D. C3 ?0 ? #endif // ME_PORT_DEFINED& ?4 K3 w# q# F$ Z" L3 i X! O; z
# ]) A0 Q( v& V" h
. X _- x- \, P R$ w void MeLineFollowerArray::setpin(uint8_t pin)
8 _, Y8 ~% y- x- L3 n, \ {( X" H8 ^0 Q7 C( H8 K1 Z: B# `9 r
_DataPin = pin;
/ m) @) o2 _: B9 v7 H3 F pinMode(_DataPin, OUTPUT);
% M7 E% w; I4 @1 I digitalWrite(_DataPin, HIGH);
9 x' |/ ~; U" M + T& ^+ D/ s; R# P2 B
#ifdef ME_PORT_DEFINED- {( V- Z! Y' L7 x
s2 = pin;
6 @+ l$ c8 {* a, j0 s #endif! z9 t. E" O- ]! G, x
}
6 c; W F& J q! x8 D# o1 @
$ ]* n- u" n- Q2 ^ uint8_t MeLineFollowerArray::getValue()
* d, [7 [7 ~$ n5 R- ^ {( z7 C/ y& J- h5 L" z+ L/ k: X+ ]
uint32_t LOW_level_read_time;! T; O; G; n# R1 ~$ p: E! @* z
uint32_t HIGH_level_read_time;& H n( G* I, m' H, C6 O9 V) T( T% m
uint32_t time_out_flag;
E! j+ x! M3 J1 W5 ? uint8_t Sensor_Data[3];! `; K% C# |% B8 ]$ o' p
static uint8_t old_data = 0xff;- H! w0 ^ f; V9 w) f
" ]. F3 N0 Q) O% t pinMode(_DataPin, OUTPUT);
0 u' X) B m" ?+ |% a. c digitalWrite(_DataPin, LOW);
6 W3 U/ O7 C* W# k* p delayMicroseconds(980);$ A8 @4 p* F% q4 {4 }9 c- o
digitalWrite(_DataPin, HIGH);
. L. m. {" o( L8 h# _6 j" R& o; b & }* h6 u3 Y$ D* Z4 o- x! Y6 N
pinMode(_DataPin, INPUT_PULLUP);+ q% L) o! X' E
delayMicroseconds(10);
, p2 {3 x+ E r$ } 1 t. D2 V+ \4 O2 C; U8 x5 t' I( v3 H
time_out_flag = millis();
8 o7 i+ s9 p; H# ? while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* K- O1 {8 G* b. z
3 R6 [8 k( X, W& o7 Z0 P: g
LOW_level_read_time = micros();
$ t7 K3 T M7 | if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
' M& |$ r; r) J7 ]; ?1 v {2 v! S6 A: |2 [
return 0xff;
F1 R" c$ W) s* f5 r( ~ {( A }1 M& q- }+ @, s6 Q/ N
+ P5 k# f9 L' z1 W& l0 T while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
, ?" } J4 h5 G0 W" h+ b! C% L
; H) z! h& H& j9 g& J& \% X0 ? HIGH_level_read_time = micros();
" H7 v1 a+ f% E; U4 W H LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
' ~6 n2 n3 m0 Q" f/ i( p4 N
/ x# r" G$ B) _ if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out) @, l9 K. @- y+ F& K
{
4 u# k0 y$ L$ z) |1 }# a* } return 0xff;8 k. d$ y1 F5 U/ F/ F T
}
7 `. ?- u9 e, a' P 2 Y. g! z# ]! Z; c8 Y2 ~
if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))) `/ U% C* S6 H0 g; u7 q
{6 v/ `2 J0 l \1 P+ f9 O( T
return 0xff;
! {# k' \) k' U0 x2 j }
: e H9 A. ^1 b! G0 a; w' [) S
& U! }' R4 P/ L; f7 T while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 q8 t b/ P7 I! m: [- E LOW_level_read_time = micros();" U& H; t g1 k7 w4 `2 M. v3 M
HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
, h( m& s; n4 c% M 1 ^- {' k+ k4 t+ ^, X( ]
if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
) }9 |( {' P' h# I5 G; g {. S; x6 J; j' S# V
return 0xff;
# y& K5 P* F. M: l# H }( T! e: Z3 k, C( G; G W: l
, V7 y# m9 O) e% l" W! h- c' f
if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
8 d5 l" j( \, ~1 s# l {2 B( ^5 a3 C+ s9 c
return 0xff;
1 y1 N0 @. _0 [. c* |: F% `! x$ d }3 H1 i3 e5 z3 {# D/ u
0 W( a0 E& O1 s& t+ k- q
for(uint8_t k=0; k<3; k++)
$ \9 _) v; k0 A7 D( \# `( b4 [ {3 D- t5 j0 j4 {/ r
Sensor_Data[k] = 0x00;3 O& n% t* j" I9 ], s% r
8 b! O+ D6 l1 W5 ` for(uint8_t i=0;i<8;i++)
4 y: k, s* m5 q2 l7 K {! m7 \" {8 a' B
while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level$ {' E E& s. c) L" a, I1 g7 D
HIGH_level_read_time = micros();
9 T* j) b# G* s2 L LOW_level_read_time = micros() - LOW_level_read_time;
9 }5 r0 z# T! v! S! Q
+ X3 u+ v9 Q' A if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
& K- a4 R9 B6 I% x {+ K# z) G& v" [/ {
return 0xff;* N8 m, ^% G2 T, [' A; v
} e' b: _% Q1 \; k( o: H
. O' [2 W- b) [) u2 {( Y' H while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. n8 x c* ]* ~" t1 ]9 _% d LOW_level_read_time = micros();' F. U' {, g+ L
HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
8 L/ z6 t* P) w# W2 [1 U8 I ^
5 w/ z$ M: ?1 K$ c if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 15 K4 P7 } l* r. }' r E
{
/ u Q- @" ?7 h! h6 H- ~, A Sensor_Data[k] |= (0x80 >> i);
1 k# {' ~& O. Y$ P/ c }3 ?& h& x6 {4 x% n$ r. f5 { X( ^4 M
else if(HIGH_level_read_time >= 100)( G, T7 x" X+ Y
{
0 X. X' X+ r2 S) i# ?$ I return 0xff;7 a9 Z( J0 _; j3 E6 q6 k
}7 R0 f, `4 b: X8 f% j. `: Q
! E- ~1 T" v9 G( u' u if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out), O; G# O1 ~5 I+ R, K1 B! M
{
, @( b' n* }1 w+ g return 0xff;9 e) j8 g8 W9 Y8 u+ w' Z
}- j8 l; K. g1 j! N9 ^7 }4 B0 {
}, u% e6 U! Z3 C l, l- G
}
) Y. l5 J6 ~; A6 @& ^. a + z( A/ t+ B/ |- a0 C* o5 U
while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level/ x$ q: m; \) M$ L4 m# {9 B
HIGH_level_read_time = micros();9 d+ s; W3 h1 [0 J, T) J9 u! w1 Q ?
LOW_level_read_time = micros() - LOW_level_read_time;
/ M: V- a d. ~( M% s, `# x 0 H/ D6 ^' T2 h& l' s1 }
if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
4 N( ~0 v2 g! U- Z {
6 l R5 z+ J+ P4 }) W2 @ return 0xff;" Y2 k2 I7 ~/ ?6 ^3 s0 \. B" A' t
}. ]/ ]$ `/ ~ ?8 B
( S* M2 Y. {( X4 U pinMode(_DataPin, OUTPUT);
8 ?7 Q! k9 T2 @8 F6 v digitalWrite(_DataPin, HIGH);
/ J' Q# M* [/ I) C6 }# Z! D- C- V/ w : V4 |) Z" s' E# c3 Q. b
if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))4 x* p$ e) j. a" r3 v
{9 X1 a, k0 g: t
old_data = Sensor_Data[0];4 y" O2 m6 }& p2 i" K q( ^& v) u
return Sensor_Data[0];
& s7 D7 v* k' n4 _- t }# [/ D1 h! }$ v/ p
else
9 c# z6 X* v; I7 s7 C1 w7 | {
% D/ M' S M5 {5 J return old_data;
3 w/ w$ ~0 ^0 D: g7 b, D$ x }. x( e3 w/ i j
}" u0 W E& J5 g& U0 X
複製代碼 ( F, S2 Z' ]7 m1 a
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
! c) i% O- }1 |. H3 t # `% r- T5 ^4 t