POLITECHNIKA
WR
OCA
WSKA
WYDZIA
ELEKTR
ONIKI
Kier
unek:
Automat
yk
a
i
Rob
ot
yk
a
Specjalno±¢:
Rob
ot
yk
a
PRA
CA
D
YPLOMO
W
A
MA
GISTERSKA
Implemen
tacja
zac
ho
w
a«
w
stero
wniku
maªego
rob
ota
mobilnego
Implemetation
of
b
eha
viors
on
a
small
mobile
rob
ot's
con
troler
A
utor:
Ark
adiusz
Materek
Pr
o
w
adz¡
cy
pra
c¦:
dr
in».
Marek
W
n
uk,
I-6
Opiekun:
dr
in».
Marek
W
n
uk,
I-6
Ocena
pra
cy:
WR
OCA
W
2006
Spis
tre±ci
3
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4
7
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
7
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
7
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
9
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
9
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
10
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
11
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
12
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
12
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
14
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
15
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
15
.
.
.
.
.
.
.
.
.
.
.
15
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
17
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
17
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
22
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
24
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
32
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
32
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
32
37
.
.
.
.
.
.
.
.
.
.
.
.
37
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
37
.
.
.
.
.
.
.
.
.
.
.
.
.
.
38
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
40
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
40
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
42
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
43
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
44
47
2
SPIS
TRECI
49
53
57
63
67
68
69
Rozdziaª
1
W
st¦p
W
latac
h
40-t
yc
h
Issac
Asimo
w
w
sw
oim
op
o
wiadaniu
s-f
Zaba
w
a
w
b
erk
a
p
o
raz
pierw-
szy
u»yª
termin
u
rob
ot
yk
a
oraz
sform
uªo
w
aª
trzy
pra
w
a
rob
ot
yki:
1.
Rob
ot
nie
mo»e
zrani¢
i
skrzywdzi¢
czªo
wiek
a
ani
przez
zaniec
hanie
dziaªania
dopu±ci¢,
ab
y
czªo
wiek
p
oniósª
szk
o
d¦.
2.
Rob
ot
m
usi
sªuc
ha¢
danego
m
u
rozk
azu,
c
h
yba
»e
k
oliduje
on
z
Pierwszym
Pra
w
em.
3.
Rob
ot
m
usi
c
hroni¢
sam
siebie,
dop
óki
nie
k
oliduje
to
z
Pierwszym
lub
Drugim
Pra
w
em.
Dzisia
j
p
o
j¦cie
rob
ota
jest
znane
k
a»dem
u,
istnieje
wiele
jego
ró»n
yc
h
de nicji.
Przy-
kªado
w
o
w
edªug
Rob
otics
Industry
Asso
ciation
(RIA)
rob
ot
to
reprogramo
w
aln
y
,
wielo-
funk
cyjn
y
manipulator
zapro
jekto
w
an
y
do
przenoszenia
materiaªó
w,
cz¦±ci
narz¦dzi
lub
wysp
ecjalizo
w
an
yc
h
urz¡dze«
p
oprzez
wyk
onanie
programo
w
alnego
ruc
h
u
w
celu
wyk
o-
nania
ró»noro
dn
yc
h
zada«
(Jabªo«ski
i
P
osey
1985).
T
a
de nicja
nie
ob
ejm
uje
jednak
rob
otó
w
mobiln
yc
h,
dot
yczy
t
ylk
o
rob
otó
w
przem
ysªo
wyc
h.
De nicj¡
rob
ota,
która
b
¦-
dzie
ob
o
wi¡zyw
a¢
w
dalszej
cz¦±ci
pracy
jest:
rob
ot
to
maszyna
p
otra ¡ca
wyci¡
gn¡¢
informacje
z
oto
czenia
i
u»y¢
sw
o
jej
wiedzy
o
±wiecie,
ab
y
p
orusza¢
si¦
lub
inne
obiekt
y
w
b
ezpieczn
y
i
celo
wy
sp
Inspiracj¡
jak
i
przykªadami
do
na±lado
w
ania
zac
ho
w
a«
rob
ota
s¡
wszelkiego
ro
dza
ju
form
y
»ycia,
o
d
t
yc
h
na
jbardziej
prymit
ywn
yc
h
do
na
jbardziej
in
teligen
tn
yc
h.
Zwierz¡tk
a
o
d
uro
dzenia
ma
j¡
zaprogramo
w
ane
zac
ho
w
ania
(inst
ynkt
y).
S¡
to
t
yp
o
w
e
zac
ho
w
ania
o
druc
ho
w
e
np.
gdy
zwierz¦
ukuje
si¦
w
ªap
¦
to
o
druc
hem
jest
nat
yc
hmiasto
w
e
cofni¦cie
ªap
y
,
jak
te»
bardziej
zªo»one
wymaga
j¡ce
przewidyw
ania
na
jbli»szej
przyszªo±ci
oraz
pami¦tania
przeszªo±ci
np.
zdob
yw
anie
p
o»ywienia,
p
olo
w
anie
czy
rozmna»anie.
W
stero
wnik
ac
h
rob
otó
w
wyró»niam
y
dwie
arc
hitektury
zac
ho
w
a«:
o
druc
ho
w
e
(reac-
tiv
e
systems)
oraz
rozm
y±lne
(deliv
erativ
e/Hierarc
hical
con
trol
systems).
Odruchowe
-
b
ez
pami¦tania
przeszªo±ci,
prymit
ywne
zac
ho
w
ania
sp
o
w
o
do
w
ane
b
o
d¹-
cem.
S¡
to
system
y
o
bardzo
szybkiej
reak
cji.
T
yp
o
wymi
algorytmami
reprezen
tuj¡cymi
te
zac
ho
w
ania
s¡
np.
meto
da
p
óª
p
otencjaªó
w
oraz
wszelkiego
ro
dza
ju
stero
wniki
t
ypu
JEELI
b
o
dziec
TO
reak
cja
np.
stero
wnik
F
uzzy
.
R
ozmy±lne
-
stero
wnik
rob
ota
skªada
si¦
z
wielu
w
arst
w,
w
arst
w
a
na
jwy»sza
o
dp
o
wiada
za
realizacj¦
celu
gªó
wnego,
k
a»da
z
ni»szyc
h
w
arst
w
jest
o
dp
o
wiedzialna
za
p
o
dcel
zadan
y
przez
wy»sz¡
w
arst
w
¦.
Nieraz
niezb
¦dne
s¡
informacje
o
oto
czeniu
i
jego
zmienno±ci
lub
te»
pami¦tanie
p
oprzednic
h
stanó
w
rob
ota,
budo
w
anie
map
y
oto
czenia
czy
adaptacyjne
1
tªumaczenie
z
ksi¡»ki
R.
C.
Arkin
Beha
vior-based
rob
otics
4
W
st¦p
zac
ho
w
ania
przystoso
wuj¡ce
rob
ota
do
zmian
y
oto
czenia
lub
uczenia
si¦.
S¡
to
algorytm
y
dziaªa
j¡ce
w
olno,
wymaga
j¡ce
du»o
pami¦ci,
t
ymczaso
w
ej
jak
i
trw
aªej,
p
ozw
ala
j¡
jednak
na
wyk
on
yw
anie
sk
omplik
o
w
an
yc
h
zada«.
Jak
o
pierwszy
tego
t
ypu
zac
ho
w
ania
rozwijaª
R.
Bro
oks
w
latac
h
80-t
yc
h
na
uniw
ersytecie
w
Massac
h
usetts
(st¡d
arc
hitektura
ta
nosi
ró
wnie»
nazw
¦
arc
hitektury
Bro
oks'a).
W
ynikiem
p
oª¡czenia
opisan
yc
h
p
o
wy»ej
dw
ó
c
h
arc
hitektur
jest
arc
hitektura
h
ybry-
do
w
a,
która
dziaªa
tak
jak
Bro
oks'a,
z
t¡
ró»nic¡,
»e
na
na
jni»szyc
h
p
oziomac
h
p
osiada
jeszcze
system
o
druc
ho
wy
.
W
o
dniesieniu
do
natury
jej
przykªadem
jest:
gdy
czªo
wiek
c
hce
zªapa¢
przedmiot
robi
to
z
caªk
o
wit¡
±wiadomo±ci¡,
ale
je±li
przedmiot
ok
a»e
si¦
gor¡cy
nat
yc
hmiasto
wym
o
druc
hem
jest
puszczenie
go.
1.1
Cel
i
zakres
pracy
P
o
dsta
w
o
wym
celem
tej
pracy
b
yªo
zilustro
w
anie
mo»liw
o±ci
programo
w
ania
o
druc
ho
wyc
h
zac
ho
w
a«
w
okre±lon
y
sp
osób,
przepro
w
adzenie
bada«
i
eksp
erymen
tó
w
z
wybran
ymi
za-
c
ho
w
aniami
i
ic
h
dokumen
tacja.
Praca
ma
c
harakter
eksp
erymen
taln
y
,
zadaniem
b
yªo
zbudo
w
anie
rob
ota
oraz
zaimplemen
to
w
anie
zac
ho
w
a«
i
przepro
w
adzenie
eksp
erymen
tó
w.
W
ramac
h
pracy
napisano
ró
wnie»
oprogramo
w
anie
na
k
omputer
PC
uªat
wia
j¡ce
de nio-
w
anie
zac
ho
w
a«
oraz
aplik
acj¦
do
monitoro
w
ania
i
k
on guro
w
ania
rob
ota.
1.2
Realizacja
Rob
ot
do
eksp
erymen
tó
w
zostaª
st
w
orzon
y
z
wyk
orzystaniem
jak
na
jwi¦kszej
ilo±ci
go-
to
wyc
h
elemen
tó
w
w
celu
p
o±wi¦cenia
wi¦kszej
ilo±ci
czasu
na
gªó
wn
y
cel
pracy
-
imple-
men
tacj¦
zac
ho
w
a«.
Jak
o
k
onstruk
cj¦
rob
ota
wyk
orzystano
p
o
dw
ozie
samo
c
ho
du
mo
de-
larskiego.
Sensory
oraz
ukªady
wyk
ona
w
cze
ró
wnie»
b
yªy
w
goto
w
ej
do
u»ycia
formie.
Nast¦pnie
wybrano
arc
hitektur¦
zac
ho
w
a«
oraz
sp
osób
implemen
tacji.
T
uta
j
wybrano
zac
ho
w
ania
o
druc
ho
w
e
oparte
na
F
uzzy
Logic,
p
oniew
a»
stero
w
anie
rozm
yte
przyp
omina
sp
osób
w
jaki
m
y±l¡
»yw
e
istot
y
.
Jak
o
mikropro
cesor
do
stero
wnik
a
rob
ota
wybrano
Motorola
S12,
idealnie
nada
j¡cy
si¦
do
tego
celu
ze
wzgl¦du
na
sprz¦to
w
e
wsparcie
dla
F
uzzy
Logic,
wyda
jno±¢,
du»e
zasob
y
pami¦cio
w
e
oraz
ró»noro
dno±¢
ukªadó
w
zewn¦trzn
yc
h.
P
o
zbudo
w
aniu
rob
ota
oraz
wyk
onaniu
elektroniki
zostaª
oprogramo
w
an
y
stero
wnik
rob
ota
oraz
napisane
oprogramo
w
anie
ma
j¡ce
wsp
omó
c
p
ó¹niejsze
de nio
w
anie
zac
ho
w
a«.
W
ostatniej
cz¦±ci
pracy
zde nio
w
ano
proste
zac
ho
w
ania
(±ledzenie
obiektu,
omija-
nie
przeszk
ó
d)
oraz
zebrano
niezb
¦dne
c
harakteryst
yki
do
udokumen
to
w
ania
dziaªania
i
wyci¡
gni¦cia
wniosk
ó
w.
W
rozdziale
opisana
zostaªa
k
onstruk
cja
mec
haniczna,
elektroniczna
oraz
p
ok
azane
zostaªy
przykªady
implemen
tacji
funk
cji
stero
wnik
a
rob
ota.
W
rozdziale
zde nio
w
ane
s¡
wybrane
przykªady
zac
ho
w
a«
oraz
wykresy
z
eksp
ery-
men
tó
w
przepro
w
adzon
yc
h
na
rob
o
cie.
K
o
dy
¹ró
dªo
w
e
doª¡czone
s¡
do
pracy
w
formie
elektronicznej,
w
pracy
zna
jduj¡
si¦
jedynie
ic
h
fragmen
t
y
.
Do
programo
w
ania
mikrok
on
trolera
rm
y
S12
wyk
orzystano
zin
tegro
w
ane
±ro
do
wi-
sk
o
programist
yczne
Co
deW
arrior
rm
y
F
reescale
4.5
for
HC12,
które
w
w
ersji
z
ogra-
1.2
Realizacja
5
niczeniami
jest
dost¦pne
za
darmo.
Jak
o
programator/debugger
do
tego
mikropro
cesora
wyk
orzystano
darmo
wy
(op
en
source)
TBDML
(T
urb
o
BDM
Ligh
t).
Jak
o
narz¦dzia
do
programo
w
ania
mikrok
on
troleró
w
rm
y
A
tmel
wyk
orzystano
darmo
w
e
±ro
do
wisk
o
pro-
gramist
yczne
A
vrStudio
4
z
k
ompilatorem
a
vr-gcc.
Oprogramo
w
anie
do
k
omputera
PC
zostaªo
napisane
w
j¦zyku
JA
V
A
w
±ro
do
wisku
programist
yczn
ym
NetBeen
5.5.
W
szel-
kiego
ro
dza
ju
sc
hemat
y
elektroniczne
oraz
pro
jekt
y
pªytek
druk
o
w
an
yc
h
wyk
onano
w
dar-
mo
wym
(op
en
source)
programie
KiCad.
Programo
w
anie
zac
ho
w
a«
w
stero
wniku
fuzzy
realizo
w
ano
za
p
omo
c¡
wªasnor¦cznie
napisanego
program
u
HC12F
uzzyEditor.
Rysunek
1.1:
Zdj¦cie
rob
ota
6
W
st¦p
Rozdziaª
2
Budo
w
a
rob
ota
2.1
K
onstruk
cja
mec
haniczna
Rob
ot
zostaª
zbudo
w
an
y
na
mo
delarskim
p
o
dw
oziu
TT-01
(rysunek
P
o
dw
ozie
stan-
dardo
w
o
ma
nap
¦d
na
cztery
k
oªa,
jednak
zrezygno
w
ano
z
nap
¦du
na
t
yª,
ab
y
w
miejsce
p
óªosiek
zamon
to
w
a¢
k
o
dery
magnet
yczne.
Rob
ot
nap
¦dzan
y
jest
silnikiem
mo
delarskim
klasy
540,
stero
w
anie
skr¦tem
k
óª
realizuje
standardo
wy
serw
omec
hanizm
mo
delarski.
W
zesta
wie
b
yª
ró
wnie»
stero
wnik
do
silnik
a
(mostek
H).
Rob
ot
zasilan
y
jest
sze±cioma
ogniw
ami
Ni-Cd
o
p
o
jemno±ci
1900mAh.
Z
przo
du
rob
ota
zostaªy
zamon
to
w
ane
dw
a
dal-
mierze
rm
y
Sharp,
ic
h
o±
widzenia
jest
lekk
o
o
dc
h
ylona
o
d
osi
rob
ota.
Umiejsco
wienie
pªytki
z
elektronik
¡
oraz
dalmierzy
wida¢
na
zdj¦ciu
Rysunek
2.1:
P
o
dw
ozie
TT-01
2.1.1
Kinemat
yk
a
Kinemat
yk
a
rob
ota
klasy
(1,1)
mo»e
b
y¢
wyra»ona
wzorem
Na
rysunku
wido
czn
y
jest
lok
aln
y
ukªad
wsp
óªrz¦dn
yc
h
rob
ota
(X, Y )
,
globaln
y
(X
0
, Y
0
)
oraz
wybrane
w
ektory
8
Budo
w
a
rob
ota
pr¦dk
o±ci
i
k
¡tó
w.
Stero
w
aniami
s¡:
pr¦dk
o±¢
linio
w
a
k
óª
przednic
h
v
p
(±rednia
pr¦dk
o±¢
lew
ego
i
pra
w
ego
k
oªa)
oraz
pr¦dk
o±¢
zmian
y
skr¦tu
k
óª
przednic
h
ζ
.
X
0
Y
0
d
l
X
Y
ș
Į
x
y
v
t
V
p
Rysunek
2.2:
Mo
del
kinemat
yczn
y
p
o
dw
ozia
samo
c
ho
du
˙x
˙y
˙θ
˙α
=
cos
(α)· cos(θ) 0
cos
(α)· sin(θ) 0
sin
(α)
d
0
0
1
·
v
p
ζ
,
(2.1)
gdzie:
(x, y)
p
oªo»enie
rob
ota,
θ
orien
tacja
rob
ota,
α
k
¡t
skr¦tu
k
óª
rob
ota,
d
o
dlegªo±¢
osi
k
óª
przednic
h
o
d
osi
t
ylnic
h,
v
p
pr¦dk
o±¢
linio
w
a
przednic
h
k
óª
rob
ota,
ζ
pr¦dk
o±¢
zmian
y
k
¡ta
k
óª
skr¦tn
yc
h.
W
rob
o
cie
jednak
nie
ma
p
omiaru
pr¦dk
o±ci
zmian
y
skr¦tu
k
óª,
dlatego
z
p
o
wy»szego
wzoru
nie
mo»na
b
ezp
o±rednio
sk
orzysta¢.
Za
p
omo
c¡
k
o
deró
w
zamon
to
w
an
yc
h
na
t
yl-
nic
h
k
oªac
h
jest
mo»liw
o±¢
p
omiaru
pr¦dk
o±ci
linio
w
ej
rob
ota
i
pr¦dk
o±ci
obroto
w
ej
rob
ota
wi¦c
kinemat
yk
a
rob
ota
jest
przeksztaªcona
do
p
ostaci:
˙x
˙y
˙θ
=
cos
(θ) 0
sin
(θ) 0
0
1
·
v
t
ω
,
(2.2)
gdzie:
v
t
pr¦dk
o±¢
linio
w
a
rob
ota
t
ylnic
h
k
óª
rob
ota,
ω
pr¦dk
o±¢
obroto
w
a
rob
ota.
Natomiast
pr¦dk
o±ci
obliczane
s¡
ze
wzoró
w:
v
= r
ω
r
+ ω
l
2
,
(2.3)
2.2
K
onstruk
cja
elektroniczna
9
ω
= r
ω
r
− ω
l
l
,
(2.4)
gdzie:
ω
r
pr¦dk
o±¢
obroto
w
a
pra
w
ego
t
ylniego
k
oªa
rob
ota,
ω
l
pr¦dk
o±¢
obroto
w
a
pra
w
ego
t
ylniego
k
oªa
rob
ota,
r
promie«
t
ylniego
k
oªa
rob
ota,
l
rozsta
w
k
óª
t
ylnic
h
rob
ota.
Rzeczywiste
wymiary
rob
ota:
d
= 265mm
l
= 152mm
r
= 32.315mm
2.2
K
onstruk
cja
elektroniczna
Ogóln
y
sc
hemat
blok
o
wy
elektroniki
rob
ota
przedsta
wion
y
zostaª
na
rysunku
wida¢
tam
ró
wnie»
cze±¢
zwi¡zan¡
z
blok
ami
programo
wymi,
te
zostan¡
opisane
w
p
o
drozdziale
K
on
troler
rob
ota
-
MC9S12A64
rm
y
F
reescale,
wybrano
z
kilku
p
o
w
o
dó
w:
dost¦pna
jest
pªytk
a
druk
o
w
ana
mo
duªu
z
t
ym
mikrok
on
trolerem,
ma
wbudo
w
an
y
BDM
(debugger),
ma
sprz¦to
w
e
wsparcie
dla
F
uzzy
Logic,
dost¦pna
jest
w
ersja
zin
tegro
w
anego
±ro
do
wisk
a
programist
ycznego
Co
deW
arrior
4.5
rm
y
F
reescale
z
limitem
do
32kB
k
o
du.
Na
pªy-
cie
gªó
wnej
zdecydo
w
ano
si¦
umie±ci¢
mo
dem
radio
wy
(zmo
dy k
o
w
ana
w
ersja
pro
jektu
w
celu
monitoro
w
ania
rob
ota,
k
on guro
w
ania
oraz
programo
w
ania
stero
wnik
a
fuzzy
b
ezprzew
o
do
w
o.
2.2.1
Dalmierze
Firma
Sharp
pro
dukuje
dalmierze
serii
GP2Dxx
o
zakresie
p
omiaro
wym
o
d
30cm
do
150cm.
Dziaªa
j¡
one
na
zasadzie
p
omiaru
przesuni¦cia
dobitej
o
d
obiektu
wi¡zki
±wiatªa
p
o
dczerw
onego.
Cec
h
uj¡
si¦
bardzo
maªym
wpªyw
em
k
oloru
oraz
re eksyjno±ci
obiektu
na
wynik
p
omiaru.
W
ynik
p
omiaru
jest
p
o
da
w
an
y
analogo
w
o
lub
binarnie.
W
adami
t
yc
h
czujnik
ó
w
s¡:
stosunk
o
w
o
du»a
strefa
mart
w
a,
maªa
cz¦stotliw
o±¢
p
omiaró
w
(ok.
20-25Hz),
nielinio
w
a
c
harakteryst
yk
a
wyj±cia.
P
arametry
p
oszczególn
yc
h
mo
deli
przed-
sta
wiono
w
tab
eli
a
c
harakteryst
yk
¦
wyj±cia
jednego
z
nic
h
na
rysunku
Mo
del
zasi¦g
W
yj±cie
GP2D150
15cm
z
histerez¡
2.5cm
binarne
GP2D120
4-30cm
analogo
w
e
GP2D15
24cm
z
histerez¡
+-
3cm
binarne
GP2D12
10-80cm
analogo
w
e
GP2D
Y0A02
20-150cm
analogo
w
e
T
ablica
2.1:
P
oró
wnanie
parametró
w
dalmierzy
rm
y
SHARP
10
Budo
w
a
rob
ota
Sterownik robota
MC9S12A64
Radiomodem
Modu
á CC1000
ATMEGA8
UART
TX
BUFF
255
bajtów
RX
BUFF
255
bajtów
STEROWNIK
CTS/RTS
DTR
Enkodery
ECT
Serwomechanizmy
Sterownik silnika
PWM
ATD
Dalmierze GP2D12
Kontroler
FUZZY
Interfejs u
Īytkownika
S
C
I
Obs
áuga
sensorów
Sterownik
uk
áadu
jezdnego
Monitor
Rysunek
2.3:
Sc
hemat
blok
o
wy
stero
wnik
a
rob
ota
2.2.2
K
o
dery
Do
k
o
deró
w
wyk
orzystano
ukªady
AS5040
rm
y
Microsystems.
S¡
to
10
bito
w
e
k
o
dery
magnet
yczne.
P
o
dsta
w
o
w
e
wªasno±ci
ukªadu:
•
Programo
w
alna
rozdzielczo±¢
7,
8,
9,
10
bit
•
P
omiar
b
ezwzgl¦dn
y
k
¡ta
(wyj±cie
PWM
oraz
SPI).
•
W
yj±cia
inkremen
tacyjne:
kw
adraturo
w
e,
impuls-kierunek,
3
fazo
w
a
k
om
utacja
sil-
nik
ami
BDLC.
•
Maksymalna
pr¦dk
o±¢
obroto
w
a
10
000
obr/min
Ab
y
zap
ewni¢
p
opra
wn¡
prac¦
ukªadu
niezb
¦dn
y
jest
magnes
o
±rednicy
4-6mm
umiesz-
czon
y
nad
(lub
p
o
d)
obudo
w
¡
ukªadu
w
o
dlegªo±ci
0.5-1.8mm.
Nale»y
jeszcze
zwró
ci¢
u
w
ag¦
ab
y
o±
obrotu
magnesu
b
yªa
dokªadnie
na
osi
ukªadu
(pro
ducen
t
sugeruje
toleran-
cje
0.25mm).
Szczegóªo
w
e
informacje
zna
jduj¡
si¦
w
no
cie
pro
ducen
ta
Dom
y±lnie
2.2
K
onstruk
cja
elektroniczna
11
0,00
0,50
1,00
1,50
2,00
2,50
3,00
0
10
20
30
40
50
60
70
80
x [cm]
U
[
V
]
Rysunek
2.4:
Charakteryst
yk
a
czujnik
a
GP2D12
ukªady
ma
j¡
usta
wion¡
rozdzielczo±¢
10bit,
a
jak
o
wyj±cie
inkremen
tacyjne
dom
y±lnie
jest
kw
adraturo
w
e.
Usta
wienia
mo»na
zmieni¢
program
uj¡c
je
przez
in
terfejs
szerego
wy
.
Ukªad
p
osiada
ró
wnie»
dw
a
wyj±cia
(MagINCn,
MagDECn),
które
inform
uj¡
czy
magnes
jest
umieszczon
y
pra
widªo
w
o.
2.2.3
Serw
omec
hanizm
steruj¡cy
skr¦tem
k
óª
Do
stero
w
ania
skr¦tem
k
óª
wyk
orzystano
standardo
w
e
serw
o
mo
delarskie
rm
y
Hitec
HS-
325HB.
Zasilane
jest
ono
napi¦ciem
5V,
stero
w
anie
o
db
yw
a
si¦
dªugo±ci¡
impulsu
w
stanie
wysokim,
zakres
dªugo±ci
impulsu
wynosi
o
d
1ms
o
d
2ms,
a
p
oªo»enie
neutralne
to
1.5ms.
Stero
w
anie
p
o
wtarzane
jest
co
10-20ms.
Rysunek
2.5:
Widok
serw
omec
hanizm
u
HS325HB
12
Budo
w
a
rob
ota
2.2.4
Stero
wnik
silnik
a
W
wyp
osa»eniu
samo
c
ho
du
zna
jdo
w
aª
si¦
dwukierunk
o
wy
regulator
obrotó
w
TEU-101BK.
Jest
on
stero
w
an
y
dokªadnie
w
ten
sam
sp
osób
jak
serw
omec
hanizm
y
mo
delarskie
(patrz
Maksymaln
y
pr¡d
wynosi
60A
(w
przó
d
i
w
t
yª).
Stero
wnik
da
je
mo»liw
o±¢
hamo
w
ania
silnikiem
(przez
zw
arcie
jego
wypro
w
adze«)
t
ylk
o
w
przypadku,
gdy
p
o
je¹dzie
do
przo
du
zostaªa
usta
wiona
jazda
do
t
yªu.
Usta
wienie
jazdy
do
t
yªu
nast¦puje
p
o
hamo
w
aniu
a
nast¦pnie
wyzero
w
aniu
stero
wnia
i
usta
wienia
jady
w
t
yª.
T
a
cec
ha
stero
wnik
a
sk
omplik
o
w
aªa
jego
obsªug¦
przez
mikrok
on
troler
i
nie
jest
dost¦pne
hamo
w
anie
silnik
a
przez
zw
arcie
jego
wypro
w
adze«.
Rysunek
2.6:
Widok
stero
wnik
a
silnik
a
2.2.5
Mikrok
on
troler
Gªó
wn
ym
mikrok
on
trolerem
steruj¡cym
rob
otem
jest
MC9S12A64
rm
y
F
reescale.
Ro-
dzina
16bit
mikrok
on
troleró
w
S12
jest
nast¦p
c¡
8bit
mikrok
on
trolera
serii
HC11.
Seria
S12
cec
h
uje
si¦
wi¦ksz¡
wyda
jno±ci¡
ni»
p
oprzednicy
oraz
du»¡
ilo±ci¡
zin
tegro
w
an
yc
h
ukªadó
w
w
ej±cia/wyj±cia
(timeró
w,
przet
w
ornik
ó
w
A
C,
p
ortó
w,
in
terfejsó
w
szerego
wyc
h).
W
stosunku
do
p
oprzednik
a
rozszerzono
zesta
w
instruk
cji
pro
cesora
(m.in.
o
in
terp
olacje
linio
w
e,
fuzzy
logic)
oraz
wpro
w
adzono
in
terfejs
uruc
homienio
wy
BDM,
który
p
ozw
ala
na
uruc
hamianie
program
u
w
ukªadzie
do
celo
wym.
Dzi¦ki
w/w
zaletom
mikrok
on
trolery
S12
s¡
cz¦sto
stoso
w
ane
w
rob
ot
yce
jak
i
w
przem
y±le
samo
c
ho
do
wym.
W
rob
o
cie
wyk
orzy-
stano
mo
duª
z
wymienion
ym
wy»ej
mikrok
on
trolerem
opisan
ym
w
P
o
dsta
w
o
w
e
wªasno±ci
mikrok
on
trolera:
•
16
bit
j¡dro
HCS12
K
ompat
ybilno±¢
z
MC68HC11
K
olejk
a
instruk
cji
Rozbudo
w
ane
tryb
y
adreso
w
ania
20
bit
jednostk
a
arytmet
yczno
logiczna
(ALU)
Instruk
cje
F
uzzy
Logic
•
Mo
duª
CR
G
P
¦tla
fazo
w
a
(PLL)
2.2
K
onstruk
cja
elektroniczna
13
Generator
zegara
systemo
w
ego
i
monitoro
w
anie
jego
stan
u
W
atc
hdog
(COP)
System
resetu
wyzw
alan
y
przez
wyj¡tki
p
o
dczas
dziaªania
program
u
Przerw
anie
czasu
rzeczywistego
(R
TI)
•
8
bito
wy
i
4
bito
wy
p
ort
przerw
a«
zewn¦trzn
yc
h
z
cyfro
w
¡
ltracj¡
oraz
k
on guracj¡
zb
o
cza
wyzw
alania
•
P
ami¦¢
64KB
pami¦ci
FLASH
1KB
pami¦ci
EEPR
OM
4KB
pami¦ci
SRAM
•
10
bito
wy
przet
w
ornik
analogo
w
o
cyfro
wy
•
16
bito
wy
o±mio
k
anaªo
wy
timer
ECT
z
funk
cjami
Input
Capture
i
Output
Compare,
oraz
cztery
8
bito
w
e
lub
dw
a
16
bito
w
e
liczniki
impulsó
w
•
Puls
width
mo
dulation
PWM
Osiem
8
bito
wyc
h
lub
cztery
16
bito
w
e
k
anaªó
w
Osobna
k
on guracja
(okres
i
wyp
eªnienie
dla
k
a»dego
k
anaªu)
T
ryb
symetryczn
y
Center
align
i
asymetryczn
y
L
eft
align
W
ej±cie
wyª¡czania
a
w
aryjnego
•
In
terfejsy
szerego
w
e
Dw
a
mo
duªy
async
hronicznej
transmisji
SCI
Mo
duª
transmisji
sync
hronicznej
SPI
Mo
duª
I2C
1Mbps
CAN
•
Wbudo
w
an
y
regulator
napi¦cia
2.5V
•
In
terfejs
uruc
homienio
wy
BDM
•
Maksymalna
cz¦stotliw
o±¢
pracy
pro
cesora
50MHz,
magistrali
25MHz
•
Obudo
w
a
80
pin
oraz
112
pin
14
Budo
w
a
rob
ota
2.2.6
Radiomo
dem
USB
Do
b
ezprzew
o
do
w
ej
k
om
unik
acji
z
rob
otem
wyk
onano
mo
dem
radio
wy
p
o
dª¡czan
y
do
k
omputera
PC
przez
USB.
Mo
dem
pracuje
w
w
olno
dost¦pn
ym
pa±mie
433MHz.
Roz-
wi¡zanie
jest
k
on
t
yn
uacj¡
pro
jektu
Jak
o
k
on
w
erter
USB-RS232
zastoso
w
ano
ukªad
FT232BM.
Zalet¡
wyk
orzystania
USB
jest
to,
»e
niewymagane
jest
zasilanie
zewn¦trzne.
Mikrok
on
trolerem
steruj¡cym
przebieg
transmisji
radio
w
ej
jest
A
TMega8,
do
k
om
unik
acji
radio
w
ej
wyk
orzystano
mo
duª
z
ukªadem
CC1000
(szczegóªy
w
Caªy
mo
dem
mie±ci
si¦
w
obudo
wie
60x50mm,
wymaga
an
ten
y
o
dªugo±ci
ok.
16.4cm
(an
tena
p
o
winna
mie¢
imp
edancj¦
50
Ω
).
An
tena
zostaªa
wyk
onana
z
k
a
w
aªk
a
drutu
(takie
rozwi¡zanie
spisuje
si¦
bardzo
dobrze).
Mo
dem
k
om
unikuje
si¦
z
urz¡dzeniem
wyk
orzystuj¡cym
go
przez
in-
terfejs
RS232
z
pr¦dk
o±ci¡
115200
b
o
dó
w,
z
obsªug¡
p
ot
wierdze«
R
TS/CTS,
linia
DTR
jest
usta
wiana,
kiedy
mo
dem
jest
goto
wy
do
transmisji
dan
yc
h.
Dane
tec
hniczne
radiomo
dem
u:
•
parametry
RS232:
115200
baud/s,
8
bit
dan
yc
h,
brak
parzysto±ci,
p
ot
wierdzenia
(handshaking)
R
TS/CTS
•
cz¦stotliw
o±¢
pracy:
433MHz
•
parametry
transmisji
radio
w
ej:
76,8kbaud/s
FSK,
k
o
do
w
anie
Manc
hester.
•
bufor
nada
w
ania:
255
ba
jtó
w
•
bufor
o
dbiornik
a:
255
ba
jtó
w
•
maksymalna
dªugo±¢
ramki:
96
ba
jtó
w
•
liczba
prób
nada
w
ania
ramki:
5
Obsªuga
mo
dem
u
ogranicza
si¦
do
o
dp
o
wiedniego
k
on guro
w
ania
p
ortu
COM
k
omputera
PC.
Nast¦pnie
wystarczy
wysyªa¢
dane
przez
p
ort
COM,
ab
y
p
o
ja
wiªy
si¦
z
drugiej
stron
y
.
Nale»y
jednak
pami¦ta¢
o
ograniczonej
przepusto
w
o±ci
dan
yc
h
przez
radio.
Sc
hemat
ide-
o
wy
,
pro
jekt
pªytki
druk
o
w
anej
oraz
spis
elemen
tó
w
zna
jduje
si¦
w
do
datku
Rysunek
2.7:
Widok
radiomo
dem
u
USB
2.3
Oprogramo
w
anie
rob
ota
15
2.2.7
Pªyta
gªó
wna
rob
ota
Rob
ota
wyp
osa»ono
w
pªyt¦
gªó
wn¡,
która
ma
za
zadanie
zin
tegro
w
a¢
wszystkie
elemen
t
y
elektroniczne
rob
ota.
Zna
jduj¡
si¦
na
niej
stabilizatory
napi¦cia
na
5V
oraz
3.3V,
mo
dem
radio
wy
(opisan
y
w
p
o
dpunk
cie
b
ez
in
terfejsu
USB
oraz
mo
duª
z
16
bito
wym
mi-
krok
on
trolerem
MC9S12A64
który
p
eªni
rol¦
gªó
wnego
stero
wnik
a
rob
ota.
W
szelkie
sygnaªy
niezb
¦dne
do
p
o
dª¡czenia
czujnik
ó
w,
elemen
tó
w
wyk
ona
w
czyc
h
i
zasilania
zostaªy
wypro
w
adzone
przez
zª¡cza
t
ypu
gold-pin.
Sc
hemat
blok
o
wy
pªyt
y
gªó
wnej
p
ok
azano
na
rysunku
Mikrok
on
troler
A
TMega8
(szczegóªy
w
zasilan
y
napi¦ciem
3.3V
jest
o
dp
o
wiedzialn
y
za
przesyªanie
dan
yc
h
drog¡
radio
w
¡
przy
p
omo
cy
ukªadu
CC1000
(szczegóªy
w
K
om
unik
acja
z
gªó
wn
ym
k
on
trolerem
o
db
yw
a
si¦
przy
p
omo
cy
in
ter-
fejsu
UAR
T
oraz
p
ot
wierdze«
CTS/R
TS.
Linia
DTR,
gdy
jest
w
stanie
niskim
oznacza
goto
w
o±¢
mo
dem
u
(p
opra
wne
zainicjo
w
anie).
P
oniew
a»
ukªad
MC9S12A64
pracuje
na
na-
pi¦ciu
5V
niezb
¦dne
jest
u»ycie
translatora
napi¦¢
logiczn
yc
h.
W
t
ym
celu
wyk
orzystano
ukªady
MAX3377
oraz
MAX3372.
Linie
RxD
i
T
xD
z
mo
dem
u
p
o
dª¡czono
o
dp
o
wiednio
o
d
sygnaªó
w
T
xD0
oraz
RxD0,
a
linie
CTS
i
R
TS
do
T
xD1
oraz
RxD1.
T
akie
p
o
dª¡czenie
sygnaªó
w
zap
obiega
uzale»nieniu
k
om
unik
acji
z
rob
otem
t
ylk
o
przez
radio.
W
przypadku
braku
mo
duªu
radio
w
ego
mo
dem
usta
wia
linie
k
om
unik
acyjne
w
stan
wysokiej
imp
edancji,
dlatego
mo»na
przewidziane
na
mo
dule
z
mikrok
on
trolerem
MC9S12A64
zª¡cze
p
o
dª¡czy¢
przez
k
on
w
erter
TTL-RS232
b
ezp
o±rednio
do
k
omputera.
Do
datk
o
w
o
w
celu
monitoro
w
a-
nia
zmian
y
sygnaªu
R
TS
z
mo
dem
u/k
omputera
(w
przerw
aniu)
linie
t¦
p
o
dª¡czono
na
pin
PTP4.
Sygnaªy
kw
adraturo
w
e
z
k
o
deró
w
na
jwygo
dniej
(ze
wzgl¦du
na
prostot¦
obsªugi)
p
o
dª¡czy¢
do
mo
duªu
ECT
(Enhanced
Capture
Timer),
który
ma
niezale»ne
przerw
a-
nia
o
d
k
a»dego
z
o±miu
k
anaªó
w
IC
(Input
Capture),
k
a»dy
z
k
on guro
w
aln
ym
zb
o
czem
wyzw
alania
przerw
ania.
Dalmierze,
p
oniew
a»
ma
j¡
wyj±cia
analogo
w
e
p
o
dª¡czone
s¡
do
w
ej±¢
zin
tegro
w
anego
w
MC9S12A64
przet
w
ornik
a
A/C.
Stero
w
anie
serw
omec
hanizmem
steruj¡cym
skr¦tem
k
óª
oraz
stero
wnikiem
silnik
a
o
db
yw
a
si¦
p
oprzez
dªugo±¢
impulsu.
P
oniew
a»
mo
duª
ECT
jest
ju»
za
j¦t
y
na
dek
o
do
w
anie
sygnaªu
kw
adraturo
w
ego,
sygnaªy
wypro
w
adzono
z
mo
duªu
PWM.
W
ypro
w
adzono
t
ylk
o
4
sp
o±ró
d
7
dost¦pn
yc
h
k
anaªó
w
PWM,
p
oniew
a»
t
ylk
o
t
yle
mo»e
praco
w
a¢
w
trybie
16
bito
wym,
który
jest
niezb
¦dn
y
do
stero
w
ania
elemen
tami
wyk
ona
w
czymi
z
o
dp
o
wiedni¡
rozdzielczo±ci¡.
W
szystkie
nieu»y-
w
ane
sygnaªy
gªó
wnego
k
on
trolera
zostaªy
wypro
w
adzone
w
celu
ew
en
tualnej
rozbudo
wy
rob
ota.
Na
rysunku
p
ok
azano
widok
pªyt
y
gªó
wnej
z
opisan
ymi
wypro
w
adzeniami.
Sc
hemat
ideo
wy
oraz
pro
jekt
pªytki
druk
o
w
anej
oraz
spis
elemen
tó
w
umieszczono
w
do-
datku
2.3
Oprogramo
w
anie
rob
ota
Narz¦dziem
do
t
w
orzenia
oprogramo
w
ania
stero
wnik
a
b
yª
Co
deW
arrior
for
HC12
4.5.
Jest
to
zin
tegro
w
ane
±ro
do
wisk
o
programist
yczne,
w
skªad
którego
w
c
ho
dzi
edytor,
k
ompilator,
debugger
i
link
er.
P
omo
c¡
w
pisaniu
oprogramo
w
ania,
a
w
szczególno±ci
obsªugi
p
eryferii,
b
yª
p
o
dr¦cznik
u»ytk
o
wnik
a
dla
mikrok
on
trolera
MC9S12DJ64
2.3.1
Organizacja
k
o
du
¹ró
dªo
w
ego
stero
wnik
a
rob
ota
Oprogramo
w
anie
zostaªo
p
o
dzielone
na
mo
duªy:
16
Budo
w
a
rob
ota
Rysunek
2.8:
Opis
wypro
w
adze«
na
pªycie
gªó
wnej
•
main.h,
main.c
-
doª¡cza
biblioteki
dla
mikrok
on
trolera
MC9S12A64
oraz
de niuje
t
yp
y
dan
yc
h,
za
wiera
p
¦tl¦
gªó
wn¡
program
u
oraz
obsªug¦
niemask
o
w
aln
yc
h
prze-
rw
a«.
•
cr
g.h,
cr
g.c
-
k
on guruje
cz¦stotliw
o±¢
pracy
pro
cesora,
cz¦stotliw
o±ci
przerw
ania
R
TI,
za
wiera
makra
do
obsªugi
W
atchdo
g'a
oraz
za
wiera
implemen
tacj¦
funk
cji
usta-
wienia
zegara
pro
cesora
oraz
obsªug¦
przerw
ania
R
TI.
•
sci.h
sci.c
-
k
on guruje
mo
duª
SCI
oraz
za
wiera
implemen
tacj¦
funk
cji
do
k
om
uni-
k
acji
szerego
w
ej.
•
e
epr
om.h,
e
epr
om.c
-
za
wiera
implemen
tacj¦
funk
cji
do
obsªugi
zapisu
dan
yc
h
w
pami¦ci
EEPR
OM.
•
ash.h,
ash.c
-
za
wiera
implemen
tacj¦
funk
cji
do
obsªugi
zapisu
dan
yc
h
w
pami¦ci
FLASH.
•
atd.h,
atd.c
-
k
on guruje
przet
w
ornik
A/C
oraz
za
wiera
implemen
tacj¦
funk
cji
ob-
sªuguj¡cyc
h
go.
•
servo
drv.h,
servo
drv.c
-
za
wiera
funk
cje
p
ozw
ala
j¡ce
stero
w
a¢
serw
omec
hanizmami
przy
u»yciu
mo
duªu
PWM.
•
qde
c.h,
qde
c.c
-
za
wiera
implemen
tacj¦
przerw
a«
dek
o
duj¡cyc
h
sygnaªy
kw
adratu-
ro
w
e
przy
u»yciu
mo
duªu
ECT.
2.3
Oprogramo
w
anie
rob
ota
17
•
tryg.h,
tryg.c
-
za
wiera
implemen
tacj¦
obliczania
funk
cji
SIN
i
COS
na
liczbac
h
caªk
o
wit
yc
h.
•
cmd_pr
o
gr
essing.h,
cmd_pr
o
gr
essing.c
-
za
wiera
obsªug¦
in
terfejsu
u»ytk
o
wnik
a.
•
fuzzy.h,
fuzzy.c
-
za
wiera
implemen
tacj¦
stero
wnik
a
opartego
na
F
uzzy
Logic.
•
auto.h,
auto.c
-
za
wiera
obsªug¦
sensoró
w,
regulator
PID
oraz
inne
elemen
t
y
zwi¡-
zane
z
samo
c
ho
dem.
2.3.2
Algorytm
dziaªania
rob
ota
Start
pracy
rob
ota
zaczyna
si¦
inicjalizacj¡
zmienn
yc
h
oraz
p
eryferii
mikrok
on
trolera.
Nast¦pnie
rob
ot
co
1ms
próbkuje
napi¦cie
na
przet
w
ornik
ac
h
A/C,
liczy
sum¦
p
omiaró
w
oraz
wybiera
maksymaln¡
oraz
minimaln¡
w
arto±¢
z
prób
ek.
Co
1ms
s¡
ró
wnie»
spra
w-
dzane
k
omendy
u»ytk
o
wnik
a.
Co
10ms
z
w
cze±niej
zebran
yc
h
prób
ek
z
A/C
wyliczana
jest
±rednia
arytmet
yczna
p
o
o
drzuceniu
dw
ó
c
h
skra
jn
yc
h
w
arto±ci
prób
ek.
Na
p
o
dsta
wie
liczb
y
impulsó
w
naliczon
yc
h
na
k
o
derac
h
w
ci¡
gu
ostatnic
h
10ms
wyliczana
jest
pr¦dk
o±¢
linio
w
a
oraz
obroto
w
a
rob
ota.
Na
p
o
dsta
wie
pr¦dk
o±ci
obliczana
jest
p
ozycja
rob
ota
zgo
d-
nie
z
kinemat
yk
¡
Przet
w
orzone
p
omiary
s¡
zapami¦t
yw
ane
w
k
olejce
w
celu
wysªania
ic
h
na
»¡danie
u»ytk
o
wnik
a.
K
olejk
a
pami¦ta
ostatnie
300ms
czyli
30
prób
ek
k
a»dego
z
p
omiaru.
Nast¦pnie
w
zale»no±ci
o
d
wybranego
trybu
stero
w
ania
stop,
man
ual
lub
fuzzy
wyznaczane
jest
stero
w
anie
(v-pr¦dk
o±¢
linio
w
a
rob
ota,
alpha
-
k
¡t
skr¦tu
k
óª):
1.
Stop
-
silnik
zatrzyman
y
,
przednie
k
oªa
prosto.
2.
Man
ual
-
stero
w
anie
r¦czne
za
p
omo
c¡
k
omend
in
terfejsu
u»ytk
o
wnik
a.
3.
F
uzzy
-
rob
ot
dziaªa
autonomiczne,
stero
wnik
F
uzzy
dziaªa
z
wybran
ym
blokiem
reguª.
Przed
wyk
onaniem
iteracji
stero
wnik
a
F
uzzy
zmienne
s¡
sk
alo
w
ane
do
zakresu
0-255,
a
p
o
z
zakresu
jednego
ba
jta
do
rzeczywist
yc
h
w
arto±ci.
P
o
wyznaczeniu
stero
w
ania
za
wyj¡tkiem
trybu
stop
w
arto±¢
pr¦dk
o±ci
zadanej
p
o
da-
w
ana
jest
na
regulator
PID
(je±li
ta
op
cja
jest
wª¡czona),
zadana
w
arto±¢
stero
w
ania
skr¦tem
k
óª
przek
azyw
ana
jest
b
ezp
o±rednio
na
serw
omec
hanizm,
co
zam
yk
a
p
¦tl¦
dzia-
ªania
rob
ota.
Baza
wiedzy
dla
stero
wnik
a
fuzzy
przec
ho
wyw
ana
jest
w
pami¦ci
FLASH.
K
on guracja
rob
ota
(nasta
wy
PID,
staªe
sk
aluj¡ce,
w
arto±ci
zero
w
e
dla
serw
omec
hani-
zmó
w)
zapisana
jest
w
pami¦ci
EEPR
OM.
2.3.3
In
terfejs
u»ytk
o
wnik
a
W
rob
o
cie
zostaª
zaimplemen
to
w
an
y
in
terfejs
u»ytk
o
wnik
a
p
ozw
ala
j¡cy
na
r¦czne
ste-
ro
w
anie
rob
otem,
k
on guracj¦
rob
ota,
wyb
ór
algorytmó
w,
oraz
monitoro
w
anie
stanó
w
rob
ota.
K
om
unik
acja
o
db
yw
a
si¦
za
p
omo
c¡
in
terfejsu
SCI
(UAR
T).
Dom
y±lnie
rob
ot
wyk
orzystuje
k
om
unik
acj¦
radio
w
¡,
ale
istnieje
mo»liw
o±¢
p
oª¡czenia
z
k
omputerem
PC
do
p
ortu
RS232
z
u»yciem
ukªadu
do
translacji
p
oziomó
w
napi¦¢
np.
MAX232.
18
Budo
w
a
rob
ota
á
Tryb sterowania
Start
Inicjaliza
cja robota
Czekaj na
przerwanie
RTI (1ms)
Zbierz
pomiary z
ATD
rti_cnt
>=10
rti_cnt:=0
rti_cnt++
Nie
Tak
Przetw.
info. z
sensorów
Fuzzy
Przetw.
Komend
u
Īytkownika
Stop
Manual
PID
Rysunek
2.9:
Algorytm
dziaªania
rob
ota
Rob
ot
p
o
otrzymaniu
k
omendy
(2
ba
jtó
w)
czek
a
1s
na
argumen
t
y
(o
ile
takie
dana
k
omenda
p
osiada),
p
o
t
ym
czasie
k
omenda
zosta
je
zignoro
w
ana.
Maksymaln
y
czas
o
d-
p
o
wiedzi
na
k
omend¦
jest
ró
wn
y
ok
25ms
(za
wyj¡tkiem
k
omend
zapisu
i
czyszczenia
pami¦ci
FLASH,
gdzie
jest
wi¦kszy).
P
oni»ej
zostaª
przedsta
wion
y
zesta
w
k
omend
dost¦pn
yc
h
w
rob
o
cie
wraz
z
ic
h
argu-
men
tami
i
sekw
encjami
o
dp
o
wiedzi.
PING
(0x0001)
K
omenda
spra
wdza
ª¡czno±¢
z
rob
otem.
A
r
gumenty:
brak
Odp
owie
d¹:
Iden
t
y k
ator
0x00FE
FLASH_ERASE
(0x0010)
K
omenda
usu
w
a
za
w
arto±¢
pami¦ci
FLASH
w
oknie
0x8000-0xBFFF.
A
r
gumenty:
brak
Odp
owie
d¹:
2.3
Oprogramo
w
anie
rob
ota
19
P
ot
wierdzenie
(1
ba
jt)
-
0x00:
OK
-
0xFF:
FLASH_A
CCES_ERR
OR
-
0xFE:
FLASH_PR
OTECTION_V
OLA
TION
FLASH_READ
(0x0011)
K
omenda
czyta
okre±lon¡
liczb
¦
ba
jtó
w
z
pami¦ci
FLASH
z
zakresu
adreso
w
ego
0x8000-
0xBFFF.
A
r
gumenty:
A
dres
(2
ba
jt
y)
Liczba
ba
jtó
w
do
przeczytania
(2
ba
jt
y)
Odp
owie
d»:
P
ot
wierdzenie
(1
ba
jt)
-
0x00:
adres
p
opra
wn
y
-
0xFF:
niepra
widªo
wy
adres
lub
liczba
ba
jtó
w
Dane
(n
ba
jtó
w):
je±li
adres
i
liczba
ba
jtó
w
b
yªy
p
opra
wne.
FLASH_WRITE
(0x0012)
K
omenda
zapisuje
okre±lon¡
liczb
¦
ba
jtó
w
do
pami¦ci
FLASH
z
zakresu
adreso
w
ego
0x8000-0xBFFF.
A
r
gumenty:
A
dres
(2
ba
jt
y)
Liczba
ba
jtó
w
do
zapisania
(2
ba
jt
y)
Odp
owie
d¹:
P
ot
wierdzenie
(1
ba
jt)
-
0x00:
adres
p
opra
wn
y
-
0xFF:
niepra
widªo
wy
adres
lub
liczba
ba
jtó
w
A
r
gumenty
(je±li
otrzymano
0x00):
Dane
(n
ba
jtó
w)
Odp
owie
d¹:
P
ot
wierdzenie
(1
ba
jt)
-
0x00:
OK
-
0xFF:
FLASH_A
CCESS_ERR
OR
-
0xFE:
FLASH_PR
OTECTION_V
OLA
TION
-
0xF
C:
FLASH_NOT_ERASED_EER
OR
20
Budo
w
a
rob
ota
A
UTO_RESET
(0x0020)
K
omenda
zeruje
czas,
p
oªo»enie,
orien
tacj¦
oraz
pr¦dk
o±ci
rob
ota.
A
r
gumenty:
brak
Odp
owie
d¹:
brak
GET_ST
A
TUS
(0x0100)
K
omenda
p
o
w
o
duje
wysªanie
przez
rob
ota
stanó
w
zapami¦tan
yc
h
o
d
czasu
otrzymania
ostatniej
k
omendy
lub
maksymalnej
liczb
y
pami¦tan
yc
h
ostatnic
h
30
stanó
w
(300ms)
A
r
gumenty:
brak
Odp
owie
d¹:
liczba
rek
ordó
w
(1
ba
jt)
n
rek
ordó
w
(k
a»dy
20
ba
jtó
w)
-
teta
(4
ba
jt
y):
orien
tacja
rob
ota
[0.01
◦
]
-
x
(4
ba
jt
y):
wsp
óªrz¦dna
X
p
oªo»enia
rob
ota
[mm]
-
y
(4
ba
jt
y):
wsp
óªrz¦dna
Y
p
oªo»enia
rob
ota
[mm]
-
Ll
(2
ba
jt
y):
o
dlegªo±¢
o
dczytana
z
lew
ego
dalmierza
[mm]
-
Lr
(2
ba
jt
y):
o
dlegªo±¢
o
dczytana
z
lew
ego
dalmierza
[mm]
-
_v
(1
ba
jt
y):
pr¦dk
o±¢
rzeczywista
rob
ota
[mm/s]
-
v
(1
ba
jt
y):
pr¦dk
o±¢
zadana
rob
ota
[mm/s]
-
alpha
(1
ba
jt
y):
(k
¡t
skr¦tu
k
óª
przednic
h
(127->
60
◦
)
-
motor
(1
ba
jt
y):
(k
¡t
skr¦tu
k
óª
przednic
h
(127->
100%
mo
cy)
SET_CONFIG
(0x0101)
Zapisuje
usta
wienia
do
pami¦ci
EEPR
OM
A
r
gumenty:
usta
wienia
(19
ba
jtó
w)
-
teta_m
ul
(2
ba
jt
y):
sk
alo
w
anie
o
dczytó
w
z
ek
o
deró
w
na
znormalizo
w
ane
jednostki
-
teta_div
(2
ba
jt
y):
sk
alo
w
anie
o
dczytó
w
z
ek
o
deró
w
na
znormalizo
w
ane
jednostki
-
xy_m
ul
(2
ba
jt
y):
sk
alo
w
anie
o
dczytó
w
z
ek
o
deró
w
na
znormalizo
w
ane
jednostki
-
xy_div
(2
ba
jt
y):
sk
alo
w
anie
o
dczytó
w
z
ek
o
deró
w
na
znormalizo
w
ane
jednostki
-
Serv
o1T
rim
(1
ba
jt):
p
ozycja
neutrum
dla
serw
omec
hanizm
u
nr
1
-
Serv
o2T
rim
(1
ba
jt):
p
ozycja
neutrum
dla
serw
omec
hanizm
u
nr
2
-
Serv
o3T
rim
(1
ba
jt):
p
ozycja
neutrum
dla
serw
omec
hanizm
u
nr
3
-
Serv
o4T
rim
(1
ba
jt):
p
ozycja
neutrum
dla
serw
omec
hanizm
u
nr
4
2.3
Oprogramo
w
anie
rob
ota
21
-
V_Kp
(2
ba
jt
y):
nasta
w
a
Kp
regulator
PID
-
V_Ki
(2
ba
jt
y):
nasta
w
a
Ki
regulator
PID
-
V_Kd
(2
ba
jt
y):
nasta
w
a
Kd
regulator
PID
-
enablePID
(1
ba
jt):
usta
wienie,
czy
regulator
PID
wª¡czon
y
Odp
owie
d¹:
brak
GET_CONFIG
(0x0102)
Odczytuje
usta
wienia
z
pami¦ci
EEPR
OM
A
r
gumenty:
brak
Odp
owie
d¹:
usta
wienia
(19
ba
jtó
w):
znaczenie
patrz
k
omenda
wy»ej
SET_CONTR
OL_MODE
(0x0103)
Usta
wia
tryb
stero
w
ania
rob
otem.
A
r
gumenty:
T
ryb
stero
w
ania
(1
ba
jt):
-
i:
tryb
stero
w
ania
F
uzzy
-
n
umer
zesta
wu
reguª
-
0xFE:
r¦czn
y
tryb
stero
w
ania
-
0xFF:
stero
w
anie
wyª¡czone
Odp
owie
d¹:
brak
GET_CONTR
OL_MODE
(0x0104)
W
ysyªa
aktualn
y
tryb
stero
w
ania
A
r
gumenty:
brak
Odp
owie
dz:
T
ryb
stero
w
ania
(1
ba
jt):
znaczenie
patrz
k
omenda
wy»ej
22
Budo
w
a
rob
ota
SET_CONTR
OL
(0x0110)
Usta
wia
stero
w
anie
r¦czne
A
r
gumenty:
motor
(1
ba
jt):
(-128..127)
stero
w
anie
pr¦dk
o±ci¡
jazdy
alpha
(1
ba
jt):
(-128..127)
stero
w
anie
skr¦tem
k
óª
Odp
owie
dz:
brak
2.3.4
Obsªuga
sensoró
w
Sharp
GP2D12
Do
p
omiaru
napi¦cia
wyk
orzystano
wbudo
w
an
y
w
mikrok
on
troler
10
bito
wy
przet
w
ornik
A/C
z
napi¦ciem
referencyjn
ym
3.3V.W
celu
p
opra
wnego
przet
w
arzania
sygnaªu
analogo-
w
ego
zebrano
c
harakteryst
yk
¦
wyj±cia
dalmierza,
przedsta
wiono
j¡
na
rysunku
W
y-
znaczono
o
dlegªo±ci,
dla
któryc
h
w
arto±¢
z
przet
w
ornik
a
A/C
zmienia
si¦
co
64
jednostki.
W
arto±ci
p
o±rednie
o
dlegªo±ci
obliczane
s¡
przez
in
terp
olacj¦
linio
w
¡.
Na
wykresie
p
ok
azane
zostaªy
wyznaczone
punkt
y
oraz
wykres
p
o
in
terp
olacji.
Przykªad
implemen-
SHARP U(X)
0,00
0,50
1,00
1,50
2,00
2,50
3,00
0
10
20
30
40
50
60
70
80
x [cm]
U
[
V
]
Rysunek
2.10:
In
terp
olacja
o
dcink
ami
linio
w
a
dla
wyj±cia
dalmierza
GP2D12
tacji
funk
cji
przelicza
j¡cej
o
dczyt
z
przet
w
ornik
a
na
o
dlegªo±¢
w
mm
z
wyk
orzystaniem
goto
w
ej
instruk
cji
mikrok
on
trolera
ETBL.
const
int
gp2d12[]
=
{
1600,
930,
640,
430,
320,
250,
205,
170,
146,
125,
110,
100,
85,
65
};
2.3
Oprogramo
w
anie
rob
ota
23
#pragma
NO_ENTRY
#pragma
NO_FRAME
#pragma
NO_EXIT
int
sharp_gp2d12(
int
adc_value
){
asm{
LDX
#64
//
X
=
64
IDIV
//
X
=
D/64,
D
=
D%64
LSLB
//
B=B*2
LSLB
//
B=B*2
XGDX
//
zamian
X<->D
LSLD
//
przesuniecie
D
w
lewo
XGDX
//
zamian
X<->D
LEAX
gp2d12,
X
//
do
X
adres
elementu
i
ETBL
0,X
//
przybli»enie
liniowe
RTS
//
zakonczenie
funkcji
}
}
K
o
dery
K
o
dery
opisane
w
p
o
drozdziale
jak
o
wyj±cie
da
j¡
sygnaª
kw
adraturo
wy
(rysunek
Dek
o
do
w
anie
sygnaªu
kw
adraturo
w
ego
zaimplemen
to
w
ane
jest
przy
u»yciu
mo
duªu
Rysunek
2.11:
Dek
o
do
w
anie
sygnaªu
kw
adraturo
w
ego
ECT
i
funk
cji
Input
Capture
mikrok
on
trolera.
Ka»dy
pin
p
ortu
T
ma
osobne
zewn¦trzne
przerw
anie
z
k
on guro
w
aln
ym
zb
o
czem
wyzw
ala
j¡cym.
Z
rysunku
wynik
a,
»e
przy
obrotac
h
w
pra
w
o
p
o
zb
o
czu
na
k
anale
A
stan
k
anaªu
B
jest
za
wsze
przeciwn
y
,
a
przy
obrotac
h
w
lew
o
taki
sam
i
o
dwrotnie
dla
zb
o
czy
k
anaªu
B.
Wi¦c
wyzw
alanie
przerw
ania
Input
Capture
sk
on guro
w
ano
na
do
w
olne
zb
o
cze
(opada
j¡ce
lub
narasta
j¡ce).
W
prze-
rw
aniu
o
d
linii
A
spra
wdzan
y
jest
stan
linii
B
(je±li
A=B
to
zwi¦ksz
licznik,
w
przeciwn
ym
wypadku
zmniejsz),
a
w
przerw
aniu
do
linii
B
spra
wdzan
y
jest
stan
linii
A
(je±li
A=B
to
24
Budo
w
a
rob
ota
zmniejsz
licznik,
w
przeciwn
ym
wypadku
zwi¦ksz).
Przykªad
implemen
tacji:
/*
KANAL
1A
*/
interrupt
8
void
_qdec0ISR(
void
)
{
if(
PTIT_PTIT0
!=
PTIT_PTIT1
)
qdec[0]++;
else
qdec[0]--;
TFLG1
=
TFLG1_C0F_MASK;
//wyczyszczenie
flagi
przerwania
}
/*
KANAL
1B
*/
interrupt
9
void
_qdec1ISR(
void
)
{
if(
PTIT_PTIT0
!=
PTIT_PTIT1
)
qdec[0]--;
else
qdec[0]++;
TFLG1
=
TFLG1_C1F_MASK;
//wyczysczenie
flagi
przerwania
}
2.3.5
Stero
wnik
F
uzzy
Ogóln¡
zasad¦
dziaªania
stero
wnik
a
opartego
na
F
uzzy
logic
przedsta
wia
rysunek
Ro-
b
ot
p
obiera
za
p
omo
c¡
sensoró
w
p
omiary
z
oto
czenia,
nast¦pnie
zadaniem
prepro
cesora
jest
przygoto
w
anie
zmienn
yc
h
dla
stero
wnik
a
fuzzy
tj.
in
terpretacja
o
dczytó
w
z
sensoró
w,
sk
alo
w
anie.
Dopiero
zmienne
w
tej
p
ostaci
mog¡
b
y¢
przek
azane
do
stero
wnik
a
F
uzzy
.
T
am
k
a»da
ze
zmienn
yc
h
w
p
ostaci
ostrej
jest
p
o
dda
w
ana
rozm
yw
aniu,
tzn.
wyliczane
s¡
w
arto±ci
zmienn
yc
h
rozm
yt
yc
h
na
p
o
dsta
wie
w
arto±ci
zmiennej
ostrej
i
w
ej±cio
wyc
h
funk
cji
przynale»no±ci,
funk
cje
przynale»no±ci
mog¡
b
y¢
np.
trap
ezoidalne,
tró
jk
¡tne,
gausso
w-
skie.
Zmienne
w
p
ostaci
rozm
ytej
tra a
j¡
do
maszyn
y
wnioskuj¡cej,
tam
na
p
o
dsta
wie
reguª
wyliczane
s¡
stopnie
akt
yw
acji
przesªanek.
Je»eli
zdarzy
si¦
tak,
»e
baza
reguª
ma
kilk
a
przesªanek
o
dnosz¡cyc
h
si¦
do
tego
samego
zbioru
rozm
ytego,
wynik
rozstrzygan
y
jest
przez
zde nio
w
an¡
meto
d¦
akum
ulacji
(na
jcz¦±ciej
MAX).
W
niosk
o
w
anie
o
db
yw
a
si¦
w
formie
Je»eli
-
T
o,
do
ª¡czenia
zmienn
yc
h
rozm
yt
yc
h
w
w
arunku
u»yw
a
si¦
op
erato-
ró
w
logiki
rozm
ytej
AND,
OR,
NOT.
W
e
wniosku
reguªy
mo»e
wyst¡
pi¢
wi¦cej
ni»
jedna
rozm
yta
zmienna
wyj±cio
w
a.
P
o
wniosk
o
w
aniu
nast¦puje
wyliczenie
w
arto±ci
zmienn
yc
h
w
p
ostaci
ostrej
na
p
o
dsta
wie
stopni
akt
yw
acji
wyj±cio
wyc
h
funk
cji
przynale»no±ci
oraz
meto
dy
denazy k
acji
(np.
meto
da
wysok
o±ci,
±ro
dk
a
ci¦»k
o±ci,
meto
da
pierwszego,
ostat-
niego
±ro
dk
a
maksim
um).
Gdy
»adna
reguªa
nie
p
okryw
a
danej
zmiennej,
czyli
wszystkie
rozm
yte
w
arto±ci
zmiennej
s¡
ró
wne
zero,
jak
o
w
arto±¢
zmiennej
stosuje
si¦
w
arto±¢
do-
m
y±ln¡.
W
arto±¢
ta
mo»e
b
y¢
te»
in
terpreto
w
ana
jak
o
p
ozosta
wienie
zmiennej
b
ez
zmian.
Mikrok
on
trolery
rm
y
F
reescale
z
serii
HCS12
ma
j¡
sprz¦to
w
e
wsparcie
dla
F
uzzy
logic,
s¡
to
cztery
instruk
cje
asem
blera:
•
MEM
-
instruk
cja
przepro
w
adza
fuzzy k
acj¦
zmiennej
w
ej±cio
w
ej,
dopuszcza
t
ylk
o
2.3
Oprogramo
w
anie
rob
ota
25
ROBOT
Baza wiedzy
Sterownik
Fuzzy
Sensory
Uk
áady
wykonawcze
Rozmywanie
(Fazzyfikacja)
Preprocesor
Wnioskowanie
Rozmywanie
(Defuzzyfikacja)
Postprocesor
Regu
áy Fuzzy
Termy
wej
Ğciowe
Termy
wyj
Ğciowe
Otoczenie
Rysunek
2.12:
Sc
hemat
blok
o
wy
stero
wnik
a
fuzzy
trap
ezoidalne
funk
cje
przynale»no±ci.
•
REV
-
instruk
cja
wyk
on
uje
wniosk
o
w
anie
(mo»e
b
y¢
przerw
ana
przez
przerw
anie).
•
REVW
-
tak
jak,
p
o
wy»ej
ale
z
do
datk
o
wym
u
wzgl¦dnieniem
w
ag
•
W
A
V
-
(±rednia
w
a»ona)
wylicza
w
arto±¢
zmiennej
wyj±cio
w
ej
na
p
o
dsta
wie
p
ozycji
i
w
arto±ci
singletonó
w.
Ograniczenia
HCS12:
•
Dopuszczalne
s¡
t
ylk
o
liczb
y
8
bito
w
e
(zakres
0-255)
•
W
ej±cio
w
e
funk
cje
przynale»no±ci
mog¡
b
y¢
t
ylk
o
trap
ezoidalne
lub
tró
jk
¡tne
(jak
o
szczególn
y
przypadek
trap
ezoidaln
yc
h)
•
W
yj±cio
w
e
funk
cje
przynale»no±ci
mog¡
przyjmo
w
a¢
t
ylk
o
form¦
singletonó
w.
•
Ograniczona
liczba
zmienn
yc
h
rozm
yt
yc
h
(w
ej±cio
w
e
+
wyj±cio
w
e
<
254)
•
W
w
arunku
reguªy
dopuszczaln
y
jest
t
ylk
o
op
erator
AND
Mimo
w/w
ogranicze«
implemen
tacja
stero
wnik
a
fuzzy
w
oparciu
o
sprz¦to
w
e
wspar-
cie
mikrok
on
troleró
w
HCS12
jest
k
orzystna
ze
wzgl¦du
na
szybk
o±¢
dziaªania
stero
wnik
a
(która
jak
p
o
da
je
pro
ducen
t
jest
ok.
50
razy
szybsza
ni»
przy
standardo
w
ej
implemen
ta-
cji
w
j¦zyku
C),
jak
ró
wnie»
oszcz¦dno±¢
pami¦ci
k
o
du
program
u.
W
szelkie
szczegóªo
w
e
informacje
mo»na
znale¹¢
w
dokumen
tacji
pro
ducen
ta
Stero
wnik
fuzzy
k
orzysta
ze
struktury
,
w
której
zde nio
w
ane
s¡
funk
cje
przynale»no±ci
oraz
reguªy
.
Struktura
jest
zde nio
w
ana
nast¦puj¡co:
26
Budo
w
a
rob
ota
typedef
struct
_FUZZY{
struct
INPUT{
unsigned
char
*idx;
//
wska¹nik
do
tablicy
z
ilo±ci¡
termów
dla
ka»dej
//
zmiennej
wej±ciowej
MF
*mf;
//
wska¹nik
do
ci¡gu
danych
opisuj¡cych
kolejne
termy
//
wej±ciowe
}
inp;
struct
OUTPUT
{
unsigned
char
*idx;
//
wska¹nik
do
tablicy
z
ilo±ci¡
termów
//
dla
ka»dej
zmiennej
wyj±ciowej
unsigned
char
*weights;
//
wska¹nik
do
tablicy
opisuj¡cej
termy
//
wyj±ciowe
(pozycje
singletonów)
unsigned
char
*defaults;
//
wska¹nik
do
tablicy
warto±ci
domy±lnych
//
dla
ka»dej
zmiennej
}
out;
struct
RULEBLOCKS
{
unsigned
char
n;
//
Liczba
dost¦pnych
zestawów
reguª
unsigned
char*
rules[MAX_RULES_BLOCKS];
//
tablica
ze
wska¹nikami
//
do
definicji
zestawu
//
reguª
}
rules_blocks;
}
FUZZY;
Stero
wnik
jest
k
on guro
w
an
y
w
pliku
fuzzy
.h,
k
on guracja
p
olega
na
usta
wieniu
ilo-
±ci
zmienn
y
w
ej±cio
wyc
h
i
wyj±cio
wyc
h,
maksymalnej
ilo±ci
zmienn
yc
h
rozm
yt
yc
h
oraz
maksymalnej
ilo±ci
zesta
w
ó
w
reguª.
/*
konfiguracja
sterownika
Fuzzy
*/
#define
INPUT_VARS
16
//
Liczba
zmiennych
wej±ciowych
#define
OUTPUT_VARS
8
//
Liczba
zmiennych
wyj±ciowych
#define
FUZZY_IN
32
//
Maksymalna
liczba
rozmytych
zmiennych
wej±ciowych
#define
FUZZY_OUT
32
//
Maksymalna
liczba
rozmytych
zmiennych
wyj±ciowych
#define
MAX_RULES_BLOCKS
8
//
Liczba
zestawów
reguª
/*
Zmeinne
globale
*/
unsigned
char
inp_vars[INPUT_VARS];
//
Tablica
zmiennych
wej±ciowych
w
//
postaci
ostrej,
przeskalowane
do
//
zakresu
0-255.
unsigned
char
out_vars[OUTPUT_VARS];
//
Tablica
zmiennych
wyj±ciowych
w
//
postaci
ostrej.
Wynik
dziaªania
//
funkcji
defuzzyfy.
Zakres
zmiennych
2.3
Oprogramo
w
anie
rob
ota
27
//
to
0-255.
unsigned
char
fuzzy_inp[FUZZY_IN
];
//
zmienne
wej±ciowe
w
postaci
//
rozmytej.
wyliczane
s¡
przez
//
funkcj¦
fuzzyfy()
unsigned
char
fuzzy_out[FUZZY_OUT];
//
Zmienne
wyj±ciowe
w
postaci
//
rozmytej.
Wyliczane
s¡
przez
//
funkcje
evaluate
extern
FUZZY
fuzzy;
Protot
yp
y
pryw
atn
yc
h
funk
cji
stero
wnik
a
wygl¡da
j¡
nast¦puj¡co:
/*
Funkcja
dokonuje
fuzzyfikacji
zmiennych
wej±ciowych
z
tablicy
inp_vars.
Wynik
umieszczony
jest
w
tablicy
globalnej
fuzzy_inp.
*/
void
fuzzyfy(void);
/*
Funkcja
dokonuje
ewaluacji
reguª.
Wynik
umieszczony
jest
w
tablicy
globalnej
fuzzy_out.
Argumenty:
rules
-
wska¹nik
do
struktury
opisuj¡cej
reguªy
n
-
liczba
rozmytych
zmiennych
wyj±ciowych
*/
void
evaluate(
void
*rules,
u08
n
);
/*
Funkcja
dokonuje
wyostrzenia
rozmytych
zmiennych
wyj±ciowych.
Wynik
umieszczany
jest
w
globalnej
tablicy
out_vars
*/
void
defuzzyfy(
void
);
In
terfejs
z
mo
duªem
stero
wnik
a
F
uzzy
stano
wi
zmienna
fuzzy.
W
opisyw
an
ym
w
tej
pracy
rob
o
cie,
zmienna
ta
jest
umieszczona
p
o
d
adresem
0x8000.
Jest
to
adres
okna
pami¦ci
FLASH.
W
t
ym
oknie
mo»e
b
y¢
wido
czna
jedna
z
4
stron
FLASH
o
adresac
h
0x3F,
0x3E,
0x3D,
0x3C.
W
rob
o
cie
na
baz¦
reguª
przeznaczono
stron¦
0x3E,
p
oniew
a»
jest
to
strona
niewyk
orzyst
yw
ana
przez
k
ompilator
przy
adreso
w
aniu
16bit
(Smal
Memory
Mo
del).
F
unk
cje
in
terfejsu
ze
stero
wnikiem
stano
wi:
/*
Funkcja
iteracji
sterownika
opartego
na
Fuzzy
Logic.
Argumenty:
rule_block_nr
-
nr.
zestawu
reguª
fuzzy
logic
*/
28
Budo
w
a
rob
ota
void
FuzzyLogic(
u08
rule_block_nr
){
u08
j,
fuzzy_out_n
=
0;
//
obliczenie
ilo±ci
zmiennych
wyj±ciowych
for(j=0;
j<OUTPUT_VARS;
j++)
fuzzy_out_n+=fuzzy.out.idx[ j];
fuzzyfy();
evaluate(
fuzzy.rules_blocks.rules[r ule_ bloc k_nr ],
fuzzy_out_n
);
defuzzfy();
}
Przed
wyw
oªaniem
p
o
wy»szej
funk
cji
nale»y
wpisa¢
do
tablicy
inp_vars
przesk
alo
w
ane
zmienne
w
ej±cio
w
e.
P
o
wyk
onaniu
nale»y
wyliczone
zmienne
wyj±cio
w
e
z
tablicy
out_vars
zastoso
w
a¢
do
ukªadó
w
wyk
ona
w
czyc
h.
Instruk
cja
MEM
-
Implemen
tacja
funk
cji
rozm
yw
a
j¡cej
W
ej±cio
w
a
funk
cja
przynale»no±ci
opisana
jest
przez
4
ba
jt
y
(patrz
rysunek
W
j¦zyku
C
trap
ezoidalna
funk
cja
przynale»no±ci
zostaªa
zde nio
w
ana
nast¦puj¡co:
typedef
struct
_MF{
unsigned
char
x1,
x2,
s1,
s2;
}
MF;
Rysunek
2.13:
Reprezen
tacja
trap
ezoidalnej
funk
cji
przynale»no±ci
Instruk
cja
MEM
jak
o
argumen
t
y
przyjm
uje:
Rejestr
X
-
wsk
a¹nik
do
struktury
opisuj¡cej
trap
ezoidaln¡
funk
cj¦
przynale»no±ci
Rejestr
Y
-
wsk
a¹nik
do
rozm
ytej
zmiennej
w
ej±cio
w
ej
2.3
Oprogramo
w
anie
rob
ota
29
Rejestr
A
-
w
arto±¢
zmiennej
p
o
dda
w
anej
fuzzy k
acji.
W
ynikiem
dziaªania
instruk
cji
jest
wyliczenie
jednej
w
arto±ci
rozm
ytej
zmiennej
w
ej±cio-
w
ej.
P
o
wyk
onaniu
instruk
cji
rejestr
X
zwi¦kszan
y
jest
o
4,
a
rejestr
Y
o
1,
czyli
X
wsk
azuje
na
opis
nast¦pnej
funk
cji
trap
ezoidalnej,
a
Y
na
nast¦pn¡
zmienna
rozm
yt¡,
co
uªat
wia
im-
plemen
tacj¦.
Do
implemen
tacji
p
otrzebna
b
¦dzie
informacja
ile
k
a»da
zmienna
w
ej±cio
w
a
ma
zbioró
w
rozm
yt
yc
h.
Przykªado
w
a
implemen
tacja
funk
cji
rozm
yw
a
j¡cej.
/*
Globalne
zmienne
wej±ciowe:
fuzzy.inp.mf
-
wska¹nik
do
struktury
opisuj¡cej
termy
wej±ciowe
fuzzy.inp.idx
-
tablica
zawieraj¡ca
informacj¦
o
ilo±ci
termów
dla
danej
zmiennej
wej±ciowej
unsigned
char
inp_vars[]
-
tablica
zawieraj¡ca
warto±ci
zmiennych
wej.
Globalne
zmienne
wyj±ciowe:
unsigned
char
fuzzy_inp[]
-
tablica
rozmytych
zmiennych
wyj±ciowych
*/
#pragma
NO_ENTRY
#pragma
NO_EXIT
#pragma
NO_FRAME
void
fuzzyfy(void)
{
asm{
LDX
fuzzy.inp.mf
//
X
=
&fuzzy.inp.mf
LDY
#fuzzy_inp
//
Y
=
fuzzy_inputs
LDD
#(0xff00
|
INPUT_VARS)
//
A
=
0xff;
B
=
INPUT_VARS
LOOP1:
INCA
//
A++
CBA
//
Porównaj
A
z
B
BEQ
LOOP1_END
//
skocz
je±li
równe
PSHD
//
zapami¦taj
D
na
stosie
PSHX
//
zapami¦taj
X
na
stosie
LDX
fuzzy.inp.idx
//
X
=
&fuzzy.inp.idx
LDAB
A,
X
//
B
=
X[A]=
fuzzy.inp.idx[A]
LDX
#inp_vars
//
X
=
inp_vars
LDAA
A,
X
//
A
=
X[A]
=
vars[A]
PULX
//
przywró¢
X
INCB
//
B++
LOOP2:
DBEQ
B,
LOOP2_END
//
je±li
--B==0
to
skocz
MEM
BRA
LOOP2
//
skocz
do
LOOP2_END:
PULD
//
przywró¢
D
BRA
LOOP1
//
skocz
do
LOOP1_END:
RTS
//
wyj±cie
z
funkcji
}
}
30
Budo
w
a
rob
ota
Instruk
cja
REV
-
Implemen
tacja
funk
cji
wnioskuj¡cej
Reguªy
dla
instruk
cji
REV
reprezen
to
w
ane
s¡
jak
o
ci¡
g
ba
jtó
w.
W
w
arunku
p
o
da
w
ane
s¡
n
umery
rozm
yt
yc
h
zmienn
yc
h
w
ej±cio
wyc
h,
nast¦pnie
znacznik
o
ddziela
j¡cy
w
arunek
o
d
wniosku
(0xFE),
p
o
znaczniku
wyst¦puj¡
n
umery
rozm
yt
yc
h
zmienn
yc
h
wyj±cio
wyc
h.
W
a»ne
jest,
ab
y
zmienne
w
ej±cio
w
e
i
wyj±cio
w
e
zna
jdo
w
aªy
si¦
w
jednej
tablicy
,
n
umero-
w
anie
zmienn
yc
h
wyj±cio
wyc
h
o
db
yw
a
si¦
wzgl¦dem
p
o
cz¡tku
tablicy
.
P
o
wniosku
reguªy
wyst¦puje
znacznik
0xFF,
który
mó
wi
»e
to
ostatnia
reguªa
i
k
o«czy
instruk
cj¦
REV
lub
0xFE,
który
oznacza,
»e
nale»y
przet
w
arza¢
nast¦pn¡
reguª¦.
Instruk
cja
REV
ma
nast¦puj¡ce
argumen
t
y:
Rejestr
X
-
wsk
a¹nik
pierwszego
elemen
tu
list
y
reguª
Rejestr
Y
-
wsk
a¹nik
do
w
ej±cio
wyc
h
i
wyj±cio
wyc
h
zmienn
yc
h
rozm
yt
yc
h,
pierwsze
w
tablicy
s¡
zmienne
w
ej±cio
w
e
a
nast¦pnie
wyj±cio
w
e.
Rejestr
A
-
m
usi
za
wiera¢
w
arto±¢
0xFF.
Flaga
V
-
m
usi
b
y¢
0.
W
szystkie
wyj±cio
w
e
zmienne
rozm
yte
m
usz¡
b
y¢
wyzero
w
ane.
W
ynikiem
dziaªania
instruk
cji
jest
obliczenie
rozm
yt
yc
h
zmienn
yc
h
wyj±cio
wyc
h
Przykªad
implemen
tacji
funk
cji
wnioskuj¡cej:
/*
Argumenty:
rules
-
wska¹nik
do
listy
reguª
n
-
liczba
wyj±ciowych
zmiennych
rozmytych
*/
#pragma
NO_ENTRY
#pragma
NO_EXIT
#pragma
NO_FRAME
void
evaluate(
void
*rules,
u08
n
)
{
asm{
//
n
jest
w
B
//
zerowanie
zmiennych
wyj±ciowych
(konieczne)
LDY
#fuzzy_out
//
Y
=
fuzzy
LOOP:
CLR
1,
Y+
//
outputs[i]
=
0,
Y++
DBNE
B,
LOOP
//
Je±li
--B!=0
to
skocz
//
wykonywanie
reguª
LDY
#fuzzy_inp
//
Y
=
fuzzy_inp
LDX
rules
//
X
=
rules
LDAA
#0xff
//
A
=
0xFF
i
V
=
0;
REV
RTS
//
wyj±cie
z
funkcji
}
}
Instruk
cja
W
A
V
-
Implemen
tacja
funk
cji
wy
ostrza
j¡cej
Instruk
cja
REV
ma
nast¦puj¡ce
argumen
t
y:
Rejestr
X
-
wsk
a¹nik
do
tablicy
p
ozycji
singletonó
w
Rejestr
Y
-
wsk
a¹nik
do
tablicy
rozm
yt
yc
h
zmienn
yc
h
wyj±cio
wyc
h
2.3
Oprogramo
w
anie
rob
ota
31
Rejestr
B
-
liczba
singletonó
w
dla
danej
zmiennej
W
ynikiem
dziaªania
instruk
cji
jest:
Rejestr
X
-
suma
w
arto±ci
singletonó
w
Rejestr
Y:D
-
24
bito
w
a
suma
ilo
czynó
w
w
arto±ci
singletonó
w
i
p
ozycji
singletonó
w
P
o
wyk
onaniu
instruk
cji
W
A
V
nale»y
wyk
ona¢
instruk
cj¦
EDIV
(dzielenie
liczb
y
32bit
przez
liczb
¦
16bit),
16bit
wynik
jest
w
rejestrze
Y
(w
rzeczywisto±ci
mie±ci
si¦
w
8bitac
h).
W
a»n
ym
elemen
tem
jest
w
arto±¢
dom
y±lna
zmiennej,
mo»e
ona
przyj¡¢
k
onkretn¡
w
ar-
to±¢
lub
NC
(no
c
hange)
czyli
p
ozosta
wi¢
zmienn¡
b
ez
zmian
(w
implemen
tacji
jak
o
NC
przyj¦to
w
arto±¢
0xFF).
W
arto±¢
dom
y±lna
jest
przypisyw
ana
do
zmiennej
wyj±cio
w
ej
w
dw
ó
c
h
przypadk
ac
h:
1)
kiedy
zmienna
ma
zero
w
¡
liczb
¦
singletonó
w,
2)
kiedy
zmiennej
nie
p
okryªa
»adna
reguªa
(X==0
p
o
instruk
cji
W
A
V).
Przykªad
implemen
tacji
funk
cji
wy
ostrza
j¡cej:
#pragma
NO_ENTRY
#pragma
NO_EXIT
#pragma
NO_FRAME
void
defuzzyfy(void){
asm{
LDD
#fuzzy_out
//
D
=
fuzzy_out
PSHD
//
D
na
stos
(zm.
'out'
)
LDD
fuzzy.out.weights
//
D
=
weights
PSHD
//
D
na
stos
(zm.
'weights'
)
LDD
#(0xff00
|
OUTPUT_VARS)//
A
=
0xff;
B
=
OUTPUT_VARS
LOOP:
INCA
CBA
//
porównaj
A
z
B
BEQ
LOOP_END
//
skocz
je±li
równe
PSHD
LDX
fuzzy.out.idx
//
X
=
fuzzy.out.idx
LDAB
A,
X
//
B
=
X[A]
TBEQ
B,
DEF_VAL
//
je±li
B
==
0
to
wart.
domy±l.
LDY
4,
SP
//
Y
=
out
TFR
Y,
X
//
X
=
Y
ABX
//
X
+=
A
STX
4,
SP
//
out
=
X
LDX
2,
SP
//
X
=
weights
PSHX
ABX
//
X
+=
A
STX
4,
SP
//
weights
=
X
PULX
WAV
//
oblicz
±rednia
wa»on¡
Y:D/X
TBEQ
X,
DEF_VAL
//
je±li
dzielenie
przez
zero
EDIV
//
Y
=
Y:D/X
TFR
Y,D
//
D
=
Y
BRA
SET_VAL
DEF_VAL:
32
Budo
w
a
rob
ota
LDX
fuzzy.out.defaults
//
X
=
fuzzy.out.defaults
LDAB
A,
X
//
B
=
fuzzy.out.defaults[A]
TFR
B,
A
//
A
=
B
IBNE
A,
SET_VAL
//
je±li
B==0xFF
wart.
domy±l.
PULD
BRA
LOOP
SET_VAL:
LDX
#out_vars
//
X
=
out_vars
PULA
STAB
A,
X
//
out_vars[A]
=
X
PULB
BRA
LOOP
//
skocz
do
LOOP_END:
LEAS
4,
SP
//
przywrócenie
wsk.
stosu
RTS
//
wyj±cie
z
funkcji
}
}
2.4
Oprogramo
w
anie
PC
Oprogramo
w
anie
na
k
omputer
PC
zostaªo
napisane
w
j¦zyku
JA
V
A.
J¦zyk
ten
wybrano
p
o
d
k
¡tem
przeno±no±ci
oprogramo
w
ania
na
ró»ne
system
y
op
eracyjne,
jak
ró
wnie»
szyb-
k
o±ci
t
w
orzenia
aplik
acji.
JA
V
A
za
wiera
implemen
tacj¦
wi¦kszo±ci
cz¦sto
u»yw
an
yc
h
al-
gorytmó
w
i
struktur
dan
yc
h.
T
w
orzenie
in
terfejsu
gra cznego
w
±ro
do
wisku
NetBeans
5.5
o
db
yw
a
si¦
bardzo
szybk
o
i
ªat
w
o.
2.4.1
HC12F
uzzyEditor
HC12F
uzzyMonitor
jest
programem
napisan
ym
w
j¦zyku
JA
V
A
do
gra cznego
progra-
mo
w
ania
stero
wnik
a
F
uzzy
opisanego
w
p
o
drozdziale
Uwzgl¦dnia
on
wszystkie
ograniczenia
zwi¡zane
ze
stero
wnikiem,
p
otra
przepro
w
adzi¢
prost¡
sym
ulacj¦
stero
w-
nik
a,
czyta
i
zapisuje
do
plik
ó
w
w
formacie
F
CL
(F
uzzy
Con
trol
Language).
Do
datk
o
w
o
p
osiada
mo»liw
o±¢
eksp
ortu
do
p
ostaci
binarnej
w
formacie
S19
i
In
tel
HEX
oraz
do
pliku
w
formacie
C.
P
osta¢
S19
b
ezp
o±rednio
nada
je
si¦
do
zaprogramo
w
ania
stero
wnik
a
przy
p
omo
cy
program
u
AutoMonitor.
Instruk
cja
obsªugi
program
u
zna
jduje
si¦
w
do
datku
P
arser
F
CL
wyk
orzystano
z
program
u
jF
uzzyLogic
autorst
w
a
P
ablo
Cingolani
J¦zyk
F
CL
jest
cz¦±ci¡
standardu
IEC
1131
który
jest
j¦zykiem
programo
w
ania
w
dla
stero
wnik
ó
w
przem
ysªo
wyc
h.
Skªadnia
j¦zyk
a
F
CL
przedsta
wiona
jest
w
do
datku
D.
2.4.2
Auto
monitor
AutoMonitor
jest
aplik
acj¡
napisan¡
w
j¦zyku
JA
V
A,
sªu»y
ona
do
k
on guro
w
ania
rob
ota,
programo
w
ania
stero
wnik
a
fuzzy
oraz
monitoro
w
ania
stanó
w
rob
ota.
Aplik
acja
k
orzysta
z
p
ortu
COM
w
celu
k
om
unik
o
w
ania
si¦
z
mo
demem.
Do
tego
celu
wyk
orzystano
bibliotek
¦
2.4
Oprogramo
w
anie
PC
33
Rysunek
2.14:
Widok
in
terfejsu
program
u
HC12F
uzzyEditor
javax.c
omm,
która
dziaªa
zaró
wno
p
o
d
lin
ux'em
jak
i
windo
ws'em.
Niestet
y
wymaga
ona
zainstalo
w
ania
o
dp
o
wiednic
h
bibliotek
w
systemie
(dost¦pne
na
stronie
ww/wsun.c
om
).
P
o
sk
on guro
w
aniu
parametró
w
transmisji
RS232
nale»y
p
oª¡czy¢
si¦
z
rob
otem
w
ci-
sk
a
j¡c
przycisk
Conne
ct.
Je»eli
ta
op
eracja
przebiegnie
p
om
y±lne
to
dost¦pne
b
¦d¡
wszyst-
kie
funk
cje
program
u.
Monitor
P
o
w
ci±ni¦ciu
przycisku
Start
aplik
acja
zaczyna
p
obiera¢
informacje
o
d
rob
ota,
jest
to
niezb
¦dne
ab
y
b
yªa
mo»liw
o±¢
zapisu
dan
yc
h
do
pliku.
W
celu
zapisu
dan
yc
h
do
pliku
nale»y
uprzednio
wybra¢
plik
i
w
cisn¡¢
przycisk
Save.
Istnieje
mo»liw
o±¢
dopisania
dan
yc
h
na
k
o«cu
pliku
lub
usuni¦cia
p
oprzedniej
za
w
arto±ci
p
oprzez
zaznaczenie
op
cji
Er
ase
le.
Program
ma
mo»liw
o±¢
zapisania
do
pliku
serii
p
omiaró
w.
W
wierszu
w
pliku
zapisane
s¡:
• t
-
aktualn
y
czas
rob
ota
[10ms]
• θ
-
orien
tacja
rob
ota
[0.01
◦
]
• x
-
skªado
w
a
X
p
oªo»enia
rob
ota
[mm]
• y
-
skªado
w
a
Y
p
oªo»enia
rob
ota
[mm]
• L
l
-
o
dlegªo±¢
z
lew
ego
dalmierza
[mm]
• L
r
-
o
dlegªo±¢
z
pra
w
ego
dalmierza
[mm]
• v
-
rzeczywista
pr¦dk
o±¢
linio
w
a
rob
ota
[mm/s]
• v
z
-
zadana
pr¦dk
o±¢
linio
w
a
rob
ota
[mm/s]
• motor
-
stero
w
anie
dla
silnik
a
[-127..127]
34
Budo
w
a
rob
ota
Rysunek
2.15:
Widok
in
terfejsu
program
u
AutoMonitor
(monitor)
• alpha
-
stero
w
anie
skr¦tem
k
óª
[-127..127]
K
olumn
y
o
ddzielone
s¡
znakiem
T
AB,
a
wiersze
LF.
Dane
dosk
onale
nada
j¡
si¦
do
p
ó¹-
niejszej
obróbki
np.
w
programie
Excel.
Con g
W
tej
zakªadce
dok
on
uje
si¦
k
on guracji
rob
ota,
mi¦dzy
inn
ymi:
k
on guracji
wsp
óª-
czynnik
ó
w
sk
aluj¡cyc
h
o
dczyt
y
z
k
o
deró
w
na
znormalizo
w
ane
jednostki
(wsp
óªczynniki
s¡
p
o
da
w
ane
w
p
ostaci
liczb
y
wymiernej:
mno»nik
a
i
dzielnik
a),
parametró
w
regulatora
PID
dla
pr¦dk
o±ci
linio
w
ej
oraz
parametró
w
usta
wia
j¡cyc
h
p
ozycj¦
zero
w
¡
serw
omec
ha-
nizmó
w.
Przycisk
Get
sªu»y
o
d
p
obrania
k
on guracji
z
rob
ota,
a
Set
zapisuje
zadan¡
k
on guracj¦
w
pami¦ci
EEPR
OM
rob
ota.
F
uzzy
Przyciski
W
rite,
R
e
ad,
V
erify
oraz
Er
ase
sªu»¡
o
dp
o
wiednio
do
zapisania,
o
dczytania,
w
e-
ry k
acji
oraz
czyszczenia
bazy
wiedzy
dla
stero
wnik
a
fuzzy
(zakres
adresó
w
w
stero
wniku
0x8000-0xBFFF).
Aplik
acja
obsªuguje
t
ylk
o
format
plik
ó
w
Motorola
S19.
W
p
olu
wyb
oru
Contr
ol
mo
de
wybiera
si¦
tryb
stero
w
ania
rob
otem:
stop,
man
ual
lub
n
umer
bloku
reguª
w
stero
wnik
a
fuzzy
,
który
ma
b
y¢
zastoso
w
an
y
do
stero
w
ania.
W
przypadku
wyb
oru
man
ual
stero
w
anie
rob
otem
o
db
yw
a
si¦
przez
m
yszk
¦.
P
o
dczas
gdy
lewy
przycisk
m
yszy
jest
w
ci±ni¦t
y
na
kw
adrato
wym
p
olu
(wido
czne
na
zdj¦ciu
przesu
w
anie
m
yszki
p
o
w
o
duje
stero
w
ane
rob
otem
(góra
dóª
o
dp
o
wiada
za
pr¦dk
o±¢
jazdy
,
a
lew
o
pra
w
o
za
skr¦t
k
óª).
2.4
Oprogramo
w
anie
PC
35
Rysunek
2.16:
Widok
in
terfejsu
program
u
AutoMonitor
(con g)
Rysunek
2.17:
Widok
in
terfejsu
program
u
AutoMonitor
(fuzzy)
36
Budo
w
a
rob
ota
Rozdziaª
3
Przykªady
implemen
tacji
zac
ho
w
a«
De nio
w
anie
zac
ho
w
a«
o
db
yw
a
si¦
w
j¦zyku
F
CL
(F
uzzy
Con
troll
Language),
który
sta-
no
wi
sió
dm
y
rozdziaª
standardu
programo
w
ania
przem
ysªo
wyc
h
stero
wnik
ó
w
IEC
1131
Zalet¡
zastoso
w
ania
tego
j¦zyk
a
jest
standaryzacja
i
dost¦pno±¢
jego
dokumen
tacji.
W
do
datku
zostaªa
przedsta
wiona
syn
takt
yk
a
j¦zyk
a
F
CL.
3.1
Zmienne
i
funk
cje
przynale»no±ci
w
stero
wniku
fuzzy
W
p
oni»szej
cz¦±ci
zostan¡
opisane
zmienne
w
ej±cio
w
e
oraz
wyj±cio
w
e
dost¦pne
w
stero
w-
niku.
F
ragmen
t
y
k
o
du
zostaªy
wygenero
w
ane
przez
program
HC12F
uzzyEditor.
W
przypadku
rozszerzania
stero
wnik
a
o
no
w
e
zmienne
w
ej±cio
w
e
nale»y
rozbudo
w
a¢
funk
cje
F
uzzyPr
epr
o
c
w
pliku
auto.c
o
instruk
cje
wpisuj¡ce
w
arto±ci
no
wyc
h
zmienn
yc
h
do
tablicy
inp_vars,
a
w
programie
HC12F
uzzyEditor
do
da¢
no
w
¡
zmienn¡
i
o
dp
o
wiednio
sk
on guro
w
a¢
jej
n
umer
(o
dp
o
wiada
j¡cy
n
umero
wi
w
stero
wniku).
W
przypadku
do
da-
w
ania
zmienn
yc
h
wyj±cio
wyc
h
nale»y
p
ost¡
pi¢
analogicznie
jak
z
w
ej±cio
wymi,
z
t
ym
»e
mo
dy kuj¡c
funk
cj¦
F
uzzyPostpr
o
c.
3.1.1
Zmienne
w
ej±cio
w
e
i
funk
cje
rozm
yw
a
j¡ce
Dost¦pne
s¡
nast¦puj¡ce
zmienne
w
ej±cio
w
e
dla
stero
wnik
a:
1.
V
-
pr¦dk
o±¢
linio
w
a
rob
ota
[mm/s]
2.
ω
-
pr¦dk
o±¢
obroto
w
a
rob
ota
[impulsó
w
k
o
deró
w/10ms]
3.
L
l
-
o
dczyt
o
dlegªo±ci
z
lew
ego
czujnik
a
[mm]
4.
L
r
-
o
dczyt
o
dlegªo±ci
z
pra
w
ego
czujnik
a
[mm]
W
stero
wniku
zastoso
w
ano
nast¦puj¡ce
funk
cje
rozm
yw
a
j¡ce
dla
zmienn
yc
h:
VAR_INPUT
V
:
REAL;
(*:
MIN
=
-128.0
MAX
=
127.0
NUMBER
=
0
*)
Omega
:
REAL;
(*:
MIN
=
-128.0
MAX
=
127.0
NUMBER
=
1
*)
LL
:
REAL;
(*:
MIN
=
0.0
MAX
=
1020.0
NUMBER
=
2
*)
LR
:
REAL;
(*:
MIN
=
0.0
MAX
=
1020.0
NUMBER
=
3
*)
38
Przykªady
implemen
tacji
zac
ho
w
a«
END_VAR
FUZZIFY
V
(*:
Predkosc
liniowa
*)
TERM
ujemna
:=
(-128.0,
0.0)
(-128.0,
1.0)
(-63.75,
1.0)
(0.0,
0.0);
TERM
zerowa
:=
(-64.0,
0.0)
(-0.25,
1.0)
(0.25,
1.0)
(64.0,
0.0);
TERM
dodatnia
:=
(0.0,
0.0)
(63.75,
1.0)
(127.0,
1.0)
(127.0,
0.0);
END_FUZZIFY
FUZZIFY
Omega
(*:
Predkosc
obrotowa
*)
TERM
prawo
:=
(-128.0,
0.0)
(-128.0,
1.0)
(-63.75,
1.0)
(0.0,
0.0);
TERM
prosto
:=
(-64.0,
0.0)
(-0.25,
1.0)
(0.25,
1.0)
(64.0,
0.0);
TERM
lewo
:=
(0.0,
0.0)
(63.75,
1.0)
(127.0,
1.0)
(127.0,
0.0);
END_FUZZIFY
FUZZIFY
LL
(*:
Odleglosc
z
lewego
dalmierza
*)
TERM
blisko
:=
(0.0,
0.0)
(0.0,
1.0)
(98.0,
1.0)
(608.0,
0.0);
TERM
srednio
:=
(96.0,
0.0)
(606.0,
1.0)
(608.0,
1.0)
(948.0,
0.0);
TERM
daleko
:=
(608.0,
0.0)
(948.0,
1.0)
(1020.0,
1.0)
(1020.0,
0.0);
END_FUZZIFY
FUZZIFY
LR
(*:
Odleglosc
z
prawego
dalmierza
*)
TERM
blisko
:=
(0.0,
0.0)
(0.0,
1.0)
(98.0,
1.0)
(608.0,
0.0);
TERM
srednio
:=
(96.0,
0.0)
(606.0,
1.0)
(608.0,
1.0)
(948.0,
0.0);
TERM
daleko
:=
(608.0,
0.0)
(948.0,
1.0)
(1020.0,
1.0)
(1020.0,
0.0);
END_FUZZIFY
Gra czna
reprezen
tacja
p
o
wy»szyc
h
funk
cji
zostaªa
przedsta
wiona
na
rysunk
ac
h
Rysunek
3.1:
F
unk
cje
przynale»no±ci
dla
zmiennej
w
ej±cio
w
ej
V
3.1.2
Zmienne
wyj±cio
w
e
i
funk
cje
wy
ostrza
j¡ce
Dost¦pne
zmienne
wyj±cio
w
e
w
stero
wniku
fuzzy:
1.
V
-
zadana
pr¦dk
o±¢
linio
w
a
[mm/s],
w
arto±¢
dom
y±lna
zmienna
jest
ró
wna
0.
2.
α
-
k
¡t
skr¦tu
k
óª
[
◦
],
w
arto±¢
dom
y±lna
jest
ró
wna
0.
3.1
Zmienne
i
funk
cje
przynale»no±ci
w
stero
wniku
fuzzy
39
Rysunek
3.2:
F
unk
cje
przynale»no±ci
dla
zmiennej
w
ej±cio
w
ej
ω
Rysunek
3.3:
F
unk
cje
przynale»no±ci
dla
zmiennej
w
ej±cio
w
ej
L
l
i
L
r
P
ozycje
singletonó
w
dla
t
yc
h
zmienn
yc
h
przedsta
wia
j¡
si¦
nast¦puj¡co:
VAR_OUTPUT
V
:
REAL;
(*:
MIN
=
-128.0
MAX
=
127.0
NUMBER
=
0
*)
Alpha
:
REAL;
(*:
MIN
=
-32.0
MAX
=
31.75
NUMBER
=
1
*)
END_VAR
DEFUZZIFY
V
(*:
Zadana
predkosc
liniowa
*)
TERM
tyl_duzo
:=
-100.0;
TERM
tyl_malo
:=
-40.0;
TERM
stop
:=
0.0;
TERM
przod_malo
:=
40.0;
TERM
przod_duzo
:=
100.0;
METHOD:
COGS;
RANGE
:=
(-128.0
..
127.0);
DEFAULT
:=
0.0;
END_DEFUZZIFY
DEFUZZIFY
Alpha
(*:
Sterowanie
skretem
kol
*)
TERM
lewo_duzo
:=
31.75;
TERM
lewo_malo
:=
12.0;
TERM
prosto
:=
0.0;
TERM
prawo_malo
:=
-12.0;
TERM
prawo_duzo
:=
-31.75;
METHOD:
COGS;
RANGE
:=
(-32.0
..
31.75);
DEFAULT
:=
0.0;
40
Przykªady
implemen
tacji
zac
ho
w
a«
END_DEFUZZIFY
3.2
Zac
ho
w
anie:
±ledzenie
obiektu
Zac
ho
w
anie
to
jest
przykªadem
p
o
d¡»ania
za
ruc
hom
ym
obiektem.
Celem
rob
ota
jest
kiero
w
anie
si¦
w
stron¦
obiektu
i
nie
wjec
hanie
w
niego,
a
w
przypadku,
gdy
obiekt
kieruje
si¦
na
rob
ota
i
jest
za
blisk
o
to
p
o
winien
on
si¦
wycofa¢.
Na
przykªadzie
opisyw
anego
w
tej
pracy
rob
ota
zac
ho
w
anie
to
mo»na
zaimplemen
to
w
a¢
przy
p
ewn
yc
h
zaªo»eniac
h,
a
miano
wicie:
w
oto
czeniu
rob
ota
nie
mog¡
zna
jdo
w
a¢
si¦
inne
obiekt
y
ni»
±ledzon
y
obiekt,
±ledzon
y
obiekt
nie
mo»e
b
y¢
bardziej
zwrotn
y
i
szybszy
o
d
rob
ota,
p
oniew
a»
ma
on
ograniczon
y
promie«
skr¦tu
i
pr¦dk
o±¢.
Przykªad
reguª
fuzzy
realizuj¡ce
p
o
wy»sze
zac
ho
w
anie
przedsta
wia
j¡:
RULEBLOCK
Sledzenie
(*:
Podazanie
za
obiektem
*)
AND
:
MIN;
OR
:
MAX;
ACT
:
MIN;
RULE
0:
IF
(LL
IS
blisko)
THEN
Alpha
IS
lewo_duzo;
RULE
1:
IF
(LR
IS
blisko)
THEN
Alpha
IS
prawo_duzo;
RULE
2:
IF
(LL
IS
srednio)
THEN
Alpha
IS
prosto;
RULE
3:
IF
(LR
IS
srednio)
THEN
Alpha
IS
prosto;
RULE
4:
IF
(LL
IS
daleko)
THEN
Alpha
IS
prawo_malo;
RULE
5:
IF
(LR
IS
daleko)
THEN
Alpha
IS
lewo_malo;
RULE
6:
IF
(LL
IS
blisko)
AND
(LR
IS
srednio)
THEN
V
IS
tyl_malo;
RULE
7:
IF
(LL
IS
blisko)
AND
(LR
IS
daleko)
THEN
V
IS
tyl_malo;
RULE
8:
IF
(LR
IS
blisko)
AND
(LL
IS
srednio)
THEN
V
IS
tyl_malo;
RULE
9:
IF
(LR
IS
blisko)
AND
(LL
IS
daleko)
THEN
V
IS
tyl_malo;
RULE
10:
IF
(LL
IS
blisko)
AND
(LR
IS
blisko)
THEN
V
IS
tyl_malo;
RULE
11:
IF
(LL
IS
srednio)
AND
(LR
IS
srednio)
THEN
V
IS
przod_duzo;
RULE
12:
IF
(LL
IS
daleko)
AND
(LR
IS
daleko)
THEN
V
IS
przod_malo;
RULE
13:
IF
(LL
IS
srednio)
AND
(LR
IS
daleko)
THEN
V
IS
przod_duzo;
RULE
14:
IF
(LR
IS
srednio)
AND
(LL
IS
daleko)
THEN
V
IS
przod_duzo;
END_RULEBLOCK
Pierwsze
sze±¢
reguª
o
dp
o
wiada
za
o
dp
o
wiedni
skr¦t
k
óª
w
zale»no±ci
o
d
o
dlegªo±ci
obiektu
i
stron
y
,
p
o
której
zostaª
obiekt
wykryt
y
.
Nast¦pne
o
dp
o
wiada
j¡
za
pr¦dk
o±¢
jazdy
,
rob
ot
stara
si¦
utrzyma¢
staª¡
o
dlegªo±¢
o
d
obiektu.
3.2.1
Przebieg
eksp
erymen
tu
1
W
tej
cz¦±ci
zna
jduj¡
si¦
c
harakteryst
yki
zarejestro
w
ane
przy
je¹dzie
rob
ota
z
wymie-
nion
ymi
w
p
oprzednim
p
o
dpunk
cie
reguªami.
Rob
ot
p
o
d¡»aª
za
przesu
w
an
ym
r¦czne
pudeªkiem
o
wymiarac
h
ok.
20cm
x
20cm
x
20cm.
Odlegªo±ci
o
dczytane
z
dalmierzy
s¡
dosy¢
p
oszarpane,
jest
to
sp
o
w
o
do
w
ane,
t
ym
»e
ruc
hom
y
obiekt
uciekª
p
oza
k
¡t
widzenia
dalmierza,
reak
cj¡
na
to
ze
stron
y
rob
ota
b
yªo
p
o
d¡»anie
w
jego
kierunku,
co
p
o
w
o
do
w
aªo
3.2
Zac
ho
w
anie:
±ledzenie
obiektu
41
p
ono
wne
p
o
ja
wienie
si¦
obiektu
w
zasi¦gu
dalmierza.
Na
wykresac
h
pr¦dk
o±ci
zadanej
i
rzeczywistej
wida¢
inercj¦
rob
ota
oraz
dziaªanie
regulatora
PID.
Na
wykresie
przedsta-
wiono
tra
jektori¦
rob
ota
jak
¡
wyk
onaª
on
p
o
dczas
eksp
erymen
tu
(start
z
punktu
(0,0)).
0,000
0,200
0,400
0,600
0,800
1,000
1,200
1,400
1,600
1,800
0,00
5,00
10,00
15,00
20,00
25,00
t [s]
l
[m
]
Ll
Lr
Rysunek
3.4:
Eksp
erymen
t
1:
Odlegªo±ci
o
dczytane
z
dalmierzy
-0,500
-0,300
-0,100
0,100
0,300
0,500
0,700
0,900
0,00
5,00
10,00
15,00
20,00
25,00
t [s]
v
[
m
/s
]
v
vz
Rysunek
3.5:
Eksp
erymen
t
1:
Pr¦dk
o±¢
zadana
i
rzeczywista
42
Przykªady
implemen
tacji
zac
ho
w
a«
-40,0
-30,0
-20,0
-10,0
0,0
10,0
20,0
30,0
40,0
0,00
5,00
10,00
15,00
20,00
25,00
t [s]
Į
[
°]
Rysunek
3.6:
Eksp
erymen
t
1:
Zadan
y
k
¡t
skr¦tu
k
óª
0,000
0,200
0,400
0,600
0,800
1,000
1,200
1,400
1,600
1,800
-0,600
-0,500
-0,400
-0,300
-0,200
-0,100
0,000
0,100
0,200
x [m]
y
[
m
]
Rysunek
3.7:
Eksp
erymen
t
1:
T
ra
jektoria
jazdy
3.3
Zac
ho
w
anie:
Omijanie
przeszk
ó
d
Celem
rob
ota
jest
omijanie
przeszk
ó
d.
W
szczególno±ci
rob
ot
p
otra
przejec
ha¢
przez
bramk
¦.
W
przypadku
gdy
nie
ma
miejsca
na
jazd¦
do
przo
du
rob
ot
zatrzym
uje
si¦,
a
gdy
nie
widzi
przeszk
o
dy
jedzie
do
przo
du.
Przykªad
reguª
dla
stero
wnik
a
fuzzy
realizuj¡cyc
h
p
o
wy»sze
zac
ho
w
anie:
RULEBLOCK
Przeszkody
(*:
Omijanie
przeszkod
-
przejazd
przez
bramke
*)
AND
:
MIN;
OR
:
MAX;
ACT
:
MIN;
RULE
0:
IF
(LL
IS
blisko)
THEN
Alpha
IS
prawo_duzo;
RULE
1:
IF
(LR
IS
blisko)
THEN
Alpha
IS
lewo_duzo;
RULE
2:
IF
(LL
IS
srednio)
THEN
Alpha
IS
prawo_malo,
V
IS
przod_malo;
RULE
3:
IF
(LR
IS
srednio)
THEN
Alpha
IS
lewo_malo,
V
IS
przod_malo;
RULE
4:
IF
(LL
IS
daleko)
THEN
Alpha
IS
lewo_duzo,
V
IS
przod_malo;
3.3
Zac
ho
w
anie:
Omijanie
przeszk
ó
d
43
RULE
5:
IF
(LR
IS
daleko)
THEN
Alpha
IS
prawo_malo,
V
IS
przod_malo;
RULE
6:
IF
(LL
IS
blisko)
AND
(LR
IS
blisko)
THEN
Alpha
IS
prawo_malo;
END_RULEBLOCK
3.3.1
Przebieg
eksp
erymen
tu
2
Zadaniem
rob
ota
b
yªo
przejec
hanie
p
o
ªuku.
Drog¦
rob
ota
wyznaczaªy
tunel,
w
którego
±cian
y
rob
ot
nie
p
o
winien
wjec
ha¢.
Szerok
o±¢
drogi
wynosiªa
ok
0.5m,
±cian
y
b
yªy
wyk
o-
nane
z
p
ousta
wian
yc
h
pudeª.
Rob
ot
p
oradziª
sobie
z
zadaniem
p
o
usta
wieniu
rozszerzenia
k
¡ta
patrzenia
dalmierzy
na
45
◦
o
d
osi
rob
ota.
Na
wykresie
wida¢
o
dp
o
wied¹
na
sk
ok
jednostk
o
wy
pr¦dk
o±ci.
Ze
wzgl¦du
na
maªe
nasta
wy
regulatora
PID
oraz
ograniczenie
zakresu
caªk
o
w
ania
rob
ot
nie
osi¡
ga
pr¦dk
o±ci
zadanej,
ale
wynik
jest
zado
w
ala
j¡cy
.
Sk
oki
o
dlegªo±ci
z
lew
ego
dalmierza
wynik
a
j¡
ze
szczelin
w
±cianie
(usta
wion
yc
h
pudeª).
0,000
0,200
0,400
0,600
0,800
1,000
1,200
1,400
1,600
1,800
0,00
1,00
2,00
3,00
4,00
5,00
6,00
7,00
t [s]
l
[m
]
Ll
Lr
Rysunek
3.8:
Eksp
erymen
t
2:
Odlegªo±ci
o
dczytane
z
dalmierzy
-0,400
-0,200
0,000
0,200
0,400
0,600
0,800
1,000
0,00
1,00
2,00
3,00
4,00
5,00
6,00
7,00
t [s]
v
[
m
/s
]
v
vz
Rysunek
3.9:
Eksp
erymen
t
2:
Pr¦dk
o±¢
zadana
i
rzeczywista
44
Przykªady
implemen
tacji
zac
ho
w
a«
-5,0
0,0
5,0
10,0
15,0
20,0
25,0
0,00
1,00
2,00
3,00
4,00
5,00
6,00
7,00
t [s]
Į
[
°]
Rysunek
3.10:
Eksp
erymen
t
2:
Zadan
y
k
¡t
skr¦tu
k
óª
-0,400
-0,200
0,000
0,200
0,400
0,600
0,800
1,000
1,200
1,400
0,000
0,200
0,400
0,600
0,800
1,000
1,200
1,400
1,600
1,800
2,000
x [m]
y
[
m
]
Rysunek
3.11:
Eksp
erymen
t
2:
T
ra
jektoria
jazdy
3.3.2
Przebieg
eksp
erymen
tu
3
Zadaniem
rob
ota
b
yªo
tra enie
i
przejec
hanie
przez
szczelin¦
w
±cianie
(usta
wion¡
z
pu-
deª).
Rob
ot
zostaª
tak
usta
wion
y
,
ab
y
widziaª
szczelin¦,
p
oniew
a»
t
ylk
o
w
takim
przy-
padku
mo»liw
e
b
yªo
wyk
onanie
zadania.
Na
wykresie
wida¢
usta
wienie
±cian
y
oraz
p
oªo»enie
rob
ota
(punkt
(0,0)).
3.3
Zac
ho
w
anie:
Omijanie
przeszk
ó
d
45
0,000
0,200
0,400
0,600
0,800
1,000
1,200
1,400
1,600
1,800
0,00
0,50
1,00
1,50
2,00
2,50
3,00
3,50
4,00
4,50
5,00
t [s]
l
[m
]
Ll
Lr
Rysunek
3.12:
Eksp
erymen
t
3:
Odlegªo±ci
o
dczytane
z
dalmierzy
-0,400
-0,200
0,000
0,200
0,400
0,600
0,800
1,000
0,00
0,50
1,00
1,50
2,00
2,50
3,00
3,50
4,00
4,50
5,00
t [s]
v
[
m
/s
]
v
vz
Rysunek
3.13:
Eksp
erymen
t
3:
Pr¦dk
o±¢
zadana
i
rzeczywista
-5,0
0,0
5,0
10,0
15,0
20,0
25,0
30,0
0,00
0,50
1,00
1,50
2,00
2,50
3,00
3,50
4,00
4,50
5,00
t [s]
Į
[
°]
Rysunek
3.14:
Eksp
erymen
t
3:
Zadan
y
k
¡t
skr¦tu
k
óª
46
Przykªady
implemen
tacji
zac
ho
w
a«
-0,600
-0,400
-0,200
0,000
0,200
0,400
0,600
0,800
1,000
1,200
1,400
0,000
0,200
0,400
0,600
0,800
1,000
1,200
1,400
1,600
1,800
2,000
x [m]
y
[
m
]
Rysunek
3.15:
Eksp
erymen
t
3:
T
ra
jektoria
jazdy
Rozdziaª
4
P
o
dsumo
w
anie
W
pracy
tej
zostaª
p
ok
azan
y
sp
osób
stero
w
ania
b
eha
wioralnego,
który
p
ozw
ala
na
za-
implemen
to
w
anie
prost
yc
h
o
druc
hó
w
rob
ota.
Dzi¦ki
u»yciu
logiki
rozm
ytej
de nio
w
anie
zac
ho
w
a«
jest
z
punktu
widzenia
czªo
wiek
a
naturalne,
ale
z
punktu
matemat
ycznego
nie
za
wsze
mo»na
do
wie±¢
p
opra
wno±ci
dziaªania
w
k
a»dym
przypadku.
Do
zde nio
w
ania
bazy
wiedzy
dla
stero
wnik
a
opartego
na
fuzzy
niezb
¦dna
jest
wiedza
eksp
erta
i
szereg
testó
w.
P
ok
azane
w
pracy
zac
ho
w
ania
s¡
bardzo
proste
i
ma
j¡
obrazo
w
a¢
przykªady
zastoso
w
a-
nia
stero
w
ania
b
eha
wioralnego
w
rob
otac
h
mobiln
yc
h.
Zaimplemen
to
w
an
y
F
uzzy
Engine
oraz
narz¦dzia
nada
j¡
si¦
do
wyk
orzystania
w
do
w
oln
ym
rob
o
cie,
w
którym
jest
p
otrzeba
wyk
orzystania
logiki
rozm
ytej.
Implemen
tacja
stero
wnik
a
fuzzy
oraz
narz¦dzi
nie
wy-
czerpuje
w
p
eªni
mo»liw
o±ci
sprz¦to
wyc
h
mikrok
on
trolera,
mo»e
on
zosta¢
wzb
ogacon
y
o
negacj¦
czy
te»
wniosk
o
w
anie
z
wyk
orzystaniem
w
ag
dla
wniosku
reguªy
(takie
rozwi¡za-
nie
zostaªo
zaimplemen
to
w
ane
w
rob
o
cie
K
oala,
który
ma
wsp
omaga¢
terapi¦
dla
dzieci
aut
yst
yczn
yc
h
Program
HC12F
uzzyMonitor
mo»na
rozszerzy¢
o
praser
p
o-
zw
ala
j¡cy
na
u»yw
anie
op
eratora
OR
w
w
arunk
ac
h
reguª.
W
hierarc
hicznej
arc
hitekturze
opisan
y
w
pracy
stero
wnik
b
eha
wioraln
y
mo»e
b
y¢
wyk
orzystan
y
w
ni»szyc
h
w
arst
w
ac
h
hierarc
hii.
Rob
ota
mo»na
wyp
osa»y¢
w
arbitra»
decyzji
p
o
dejmo
w
an
yc
h
przez
stero
wnik
fuzzy
,
tzn.
je±li
rob
ot
ma
zde nio
w
ane
kilk
a
zac
ho
w
a«
to
nast¦puje
wyb
ór
lepszego
lub
ic
h
p
oª¡czenie,
takie
rozwi¡zanie
zostaªo
wyk
orzystane
w
pracy
a
teoria
na
ten
temat
zna
jduje
si¦
w
ksi¡»ce
Problemem
p
o
dczas
de nio
w
ania
zac
ho
w
a«
ok
azaªa
si¦
niedostateczna
liczba
senso-
ró
w
obserwuj¡cyc
h
oto
czenie
i
ilo±¢
informacji
z
nic
h.
P
ok
azane
zac
ho
w
ania
daªyb
y
lepsze
rezultat
y
z
wi¦ksz¡
ilo±ci¡
dalmierzy
czy
te»
z
lepsz¡
ic
h
jak
o±ci¡
(w
szczególno±ci
cz¦sto-
tliw
o±ci¡
o
d±wie»ania
p
omiaró
w,
co
uniemo»liwiªo
wyk
onanie
prostego
sk
anera
-
dalmierz
na
serw
omec
hanizmie).
Przykªad
stero
w
ania
b
eha
wioralnego,
gdzie
zostaªa
wyk
orzystana
wi¦kszej
ilo±ci
czujnik
ó
w
Sharp
(siedem
dalmierzy)
zostaª
przedsta
wion
y
w
art
ykule
Ciek
a
wym
rozwi¡zaniem
b
yªob
y
wyp
osa»enie
rob
ota
w
system
stereo
wizyjn
y
p
ozw
ala-
j¡cy
okre±li¢
p
ozycj¦
obiektó
w,
ale
opraco
w
anie
takiego
system
u
znacznie
wykracza
p
oza
zakres
tej
pracy
.
P
o
cz¡tk
o
wym
p
om
ysªem
na
przykªady
zac
ho
w
a«
b
yªo
zaimplemen
to
w
anie
algorytmó
w
park
o
w
ania
z
wyk
orzystaniem
stero
wnik
a
fuzzy
,
eksp
erymen
t
y
nie
daªy
jednak
zado
w
ala-
j¡cyc
h
rezultató
w
ze
wzgl¦du
na
niewystarcza
j¡c¡
liczb
¦
informacji
z
sensoró
w.
Zostaªo
wyprób
o
w
ane
rozwi¡zanie
z
dalmierzem
umieszczon
ym
na
serw
omec
hanizmie
p
ozw
ala-
48
P
o
dsumo
w
anie
j¡cym
rob
oto
wi
widzie¢
w
p
ewn
ym
zakresie
k
¡to
wym
p
oprzez
stero
w
anie
serw
omec
ha-
nizmem.
Niestet
y
cz¦stotliw
o±¢
z
jak
¡
dalmierz
o
d±wie»aª
informacje
b
yªa
zb
yt
maªa,
sk
ano
w
anie
za
jmo
w
aªo
zb
yt
du»o
czasu,
co
nie
p
ozw
alaªo
rob
oto
wi
na
pªynn¡
jazd¦.
Na-
st¦pnie
wyprób
o
w
ano
sp
osób
ze
±ledzeniem
kra
w
¦dzi
przez
dalmierz
i
kiero
w
anie
rob
ota
p
o
o
dp
o
wiedniej
stronie
kra
w
¦dzi.
T
o
rozwi¡zanie
dobrze
spisyw
aªo
si¦
przy
maªyc
h
o
d-
legªo±ciac
h
(do
40cm).
Jednak
ze
wzgl¦du
na
t
ylk
o
jeden
taki
czujnik
b
yªa
mo»liw
o±¢
±ledzenia
t
ylk
o
jednej
kra
w
¦dzi,
wi¦c
rob
ot
b
yª
w
stanie
omin¡¢
jedn¡
przeszk
o
d¦,
ale
nie
widziaª
inn
yc
h.
Rob
ot
zostaª
tak
sk
onstruo
w
an
y
,
ab
y
mo»liw
o±¢
jego
rozbudo
wy
mec
hanicznej
jak
i
programo
w
ej
b
yªa
prosta
i
szybk
a.
Na
pªycie
gªó
wnej
zostaªy
wypro
w
adzone
wszystkie
nieu»yw
ane
p
ort
y
np.
zª¡cza
do
in
terfejsu
I2C,
w
ej±cia
przet
w
ornik
a
A/C.
Istnieje
mo»li-
w
o±¢
p
o
dª¡czenia
do
datk
o
w
o
dw
ó
c
h
serw
omec
hanizmó
w
i
dw
ó
c
h
k
o
deró
w.
Ze
wzgl¦du
na
wypro
w
adzone
zasilanie
3.3V
i
5V
zasilanie
no
w
ego
czujnik
a
nie
b
¦dzie
problemem
(nale»y
zwró
ci¢
u
w
ag¦
na
caªo±cio
wy
p
ob
ór
pr¡du
-
maksymalnie
1A
b
ez
silnik
a).
Oprogramo-
w
anie
stero
wnik
a
rob
ota
zostaªo
napisane
mo
duªo
w
o,
przejrzy±cie
i
z
wyczerpuj¡cymi
k
omen
tarzami,
dlatego
jego
mo
dy k
acja
nie
p
o
winna
spra
wia¢
problem
u.
Rob
ot
ze
wzgl¦du
na
dynamik
¦,
osi¡
gane
przez
niego
pr¦dk
o±ci
(na
w
et
do
20ms)
oraz
mo
c
silnik
a
p
ozw
ala
j¡c¡
na
du»e
przyspieszenia
nada
je
si¦
do
testo
w
ania
algorytmó
w
z
u
wzgl¦dnieniem
dynamiki
czy
p
o±lizgó
w.
Mo»na
na
nim
ró
wnie»
testo
w
a¢
i
rozwija¢
algo-
rytm
y
k
on
troli
trak
cji
stoso
w
ane
w
samo
c
ho
dac
h
osob
o
wyc
h
np.
ESP
,
wymaga
to
jednak
przebudo
w
ania
rob
ota,
tak
ab
y
b
yªa
mo»liw
o±¢
hamo
w
ania
k
a»dym
k
oªem
z
osobna.
Przykªadem
implemen
tacji
zac
ho
w
a«
przy
p
omo
cy
logiki
klasycznej
jest
rob
ot
mini-
sumo
Gizmo
v2
(opis
w
Ze
wzgl¦du
na
to,
i»
wi¦kszo±¢
sensoró
w
w
nim
za
w
art
yc
h
da
w
aªa
o
dczyt
y
binarne
zdecydo
w
ano
si¦
na
logik
¦
klasyczn¡.
Rob
ot
miaª
zaimplemen-
to
w
an¡
struktur¦
h
ybrydo
w
¡
zac
ho
w
a«.
P
o
dczas
starcia
z
przeciwnikiem
zac
ho
wyw
aª
si¦
o
druc
ho
w
o,
a
p
o
dczas
szuk
ania
jego
celem
b
yªo
trzymanie
si¦
±ro
dk
a
p
ola
w
alki.
T
akie
zac
ho
w
anie
p
oª¡czone
z
przem
y±lan¡
k
onstruk
cj¡
mec
haniczn¡
zapro
w
adziªo
go
do
zwy-
ci¦st
w
a
w
kilku
za
w
o
dac
h.
Do
datek
A
Pªyta
gªó
wna
rob
ota
Rysunek
A.1:
W
arst
w
a
górna
miedzi
pªyt
y
gªó
wnej
rob
ota
50
Pªyta
gªó
wna
rob
ota
Rysunek
A.2:
W
arst
w
a
dolna
miedzi
pªyt
y
gªó
wnej
rob
ota
Elemen
t
W
artos¢
Obudo
w
a
C12,
C13
22p
NPO
0603
C1,
C2,
C3,
C4,
C5,
C6,
C9,
C11
100n
XR7
0805
C6,
C10
10u/6.3V
Elektrolit
2.54-6mm
C7
100u/16V
Elektrolit
2.45-6mm
R4,
R5
4k7
0805
I1
MAX3377EEUD
TSSOP-14
I4
7805
TO-220
I5
LM1084-3V3
TO-220
I6
A
TMega8L-AI
QFP-32
I7
MAX3372EEKA
SOT23-8
MOD1
Mo
duª
z
mikrok
on
trolerem
Sp
ecjalna
MC9S12A64
MOD2
Mo
duª
CC1000PP
Sp
ecjalna
J1,
J2,
J3,
J4,
J5,
J6,
J7,
J8,
Goldpin
y
Raster
2.54mm
J9,
J10,
J11,
J12,
J13,
J14,
J15,
J16,
J17,
J18,
J19,
J20,
J21,
J22
T
ablica
A.1:
Spis
elemen
tó
w
pªyt
y
gªó
wnej
rob
ota
51
Rysunek
A.3:
Sc
hemat
ideo
wy
pªyt
y
gªó
wnej
rob
ota
52
Pªyta
gªó
wna
rob
ota
Rysunek
A.4:
Elemen
t
y
na
stronie
górnej
pªyt
y
gªó
wnej
rob
ota
Rysunek
A.5:
Elemen
t
y
na
stronie
dolnej
pªyt
y
gªó
wnej
rob
ota
Do
datek
B
Radiomo
dem
USB
Rysunek
B.1:
W
arst
w
a
górna
miedzi
radiomo
dem
u
USB
54
Radiomo
dem
USB
Rysunek
B.2:
W
arst
w
a
dolna
miedzi
radiomo
dem
u
USB
Elemen
t
W
artos¢
Obudo
w
a
C8,
C9,
C15,
C16
22p
NPO
0603
C5
10n
XR7
0805
C4
33n
XR7
0805
C1,
C2,
C6,
C7,
C11,
C13,
C14,
C17
100n
XR7
0805
C10,
C12
10u/6.3V
T
an
tal
1206
R4,
R5
27
0805
R3
470
0805
R6
1k5
0805
R2
2k2
0805
R1,
R7
10k
0805
F1
K
oralik
ferryto
wy
0805
I1
FT232BL
QFP-32
I2
93C46
SO-8
I3
(alternat
yw
a
dla
I4)
LM3480
SOT23
I4
(alternat
yw
a
dla
I3)
78L33
SO-8
I5
A
TMega8L-AI
QFP-32
MOD2
Mo
duª
CC1000PP
Sp
ecjalna
X1
6.0000MHz
HC-49S
X2
7.3728MHz
HC-49S
S1
USB
2.0
USB-B
T
ablica
B.1:
Spis
elemen
tó
w
radiomo
dem
u
USB
55
Rysunek
B.3:
Sc
hemat
ideo
wy
radiomo
dem
u
USB
56
Radiomo
dem
USB
Rysunek
B.4:
Elemen
t
y
na
stronie
górnej
radiomo
dem
u
USB
Rysunek
B.5:
Elemen
t
y
na
stronie
dolnej
radiomo
dem
u
USB
Do
datek
C
HC12F
uzzyEditor
-
Instruk
cja
obsªugi
In
terfejs
skªada
si¦
z
gªó
wnego
men
u.
P
o
lew
ej
stronie
zna
jduje
si¦
drzew
o
pro
jektu
a
p
o
pra
w
ej
zakªadki
z
edytorem
usta
wie«
stero
wnik
a,
zmienn
yc
h
w
ejcio
wyc
h
i
ic
h
termó
w,
reguª
oraz
zmienn
yc
h
wyjscio
wyc
h
i
ic
h
termó
w.
Rysunek
C.1:
In
terfej
program
u
HC12F
uzzyEditor
58
HC12F
uzzyEditor
-
Instruk
cja
obsªugi
Men
u
gªó
wne
Men
u
Opis
File->New
le.
.
.
T
w
orzy
no
wy
pro
jekt
o
dom
y±lnej
nazwie
pliku
NewFile.fcl
z
dom
y±ln
ym
blokiem
funk
cyjn
ym
F
uzzy0
File->Op
en.
.
.
Ot
wiera
plik
F
CL
1
.
File->A
dd
from
le.
.
.
Do
da
je
do
pro
jektu
za
w
arto±¢
innego
pliku
F
CL
1
.
File->Sa
v
e
Zapisuje
plik.
File->Sa
v
e
As.
.
.
Zapisuje
plik
p
o
d
inna
nazw
¡
File->Close
Zam
yk
a
pro
jekt
File->Exit
K
o«czy
dziaªanie
program
u
T
o
ols->Exp
ort
to.
.
.
->S19
le
Zapisuje
plik
w
formacie
binarn
ym
3
MotorolaS19.
T
o
ols->Exp
ort
to.
.
.
->C
le
Zapisuje
plik
j¦zyku
C,
goto
wy
do
sk
ompilo
w
ania
ze
stero
wnikiem
2
.
T
o
ols->Exp
ort
to.
.
.
->Hex
le
Zapisuje
plik
w
formacie
binarn
ym
3
In
tel
HEX.
Help->Ab
ort
P
ok
azuje
okno
z
informacj¡
o
w
ersji
oraz
z
autorem
program
u.
T
ablica
C.1:
Men
u
program
u
HC12F
uzzyEditor
Nota
1
:
W
pliku
F
CL
zna
jduj¡
si¦
do
datk
o
w
e
informacje
dla
program
u.
W
niektóryc
h
przypadk
ac
h
nie
b
¦dzie
mo»liw
e
ot
w
arcie
pliku
zez
t
yc
h
informacji.
Nota
2
:
Nale»y
zwró
ci¢
u
w
ag¦
na
nazwy
blok
ó
w
funk
cyjn
yc
h,
ab
y
b
yªy
zgo
dne
z
na
w
ami
zmienn
yc
h
w
j¦zyku
C.
Nota
3
:
Struktura
F
uzzy
zaczyna
si¦
na
adresie
0x8000.
Nale»y
zwró
ci¢
u
w
ag¦
ab
y
ste-
ro
wnik
b
yª
sk
ompilo
w
an
y
ab
y
czytaª
z
adresu
0x8000.
Drzew
o
pro
jektu
Rysunek
C.2:
Drzew
o
pro
jektu
program
u
HC12F
uzzyEditor
W
drzewie
pro
jektu
jest
p
o
dgl¡d
wszystkic
h
zmienn
yc
h,
blok
ó
w
reguª
oraz
funk
cji
przynale»no±ci.
W
t
ym
miejscu
dziaªa
p
o
dr¦czne
men
u,
za
p
omo
c¡
którego
do
da
je
si¦
lub
usu
w
a
elemen
t
y
drzew
a
pro
jektu
(bloki
funk
cyjne,
zmienne,
term
y
,
bloki
reguª
i
reguªy).
59
Przez
klikni¦cie
na
dan
y
elemen
t
zosta
je
on
p
ok
azan
y
w
edytorze
(je±li
jest
w
danej
c
h
wili
edytorem
akt
ywn
ym),
p
o
dwukrotn
ym
klikni¦ciu
zostanie
p
ok
azan
y
edytor
dla
elemen
tu.
Edytor
bloku
funk
cyjnego
W
zakªadce
F
unction
blo
c
k
editor
zna
jduj¡
si¦
usta
wienia,
które
trzeba
wpro
w
adzi¢
zgo
dnie
z
usta
wieniami
stero
wnik
a
(za
wyj¡tkiem
p
ola
Name
i
Description
).
Zmiana
którejk
olwiek
z
liczb
nast¦puje
dopiero
p
o
naci±ni¦ciu
kla
wisza
En
ter.
Rysunek
C.3:
P
anel
edytora
bloku
funk
cyjnego
P
ole
Opis
Zakres
Name
Nazw
a
bloku
funk
cyjnego
String
Input
v
ariables
Liczba
dost¦pn
yc
h
zmienn
yc
h
1-255
zmienn
yc
h
w
stero
wniku
Rule
blo
c
ks
Maksymalna
liczba
ró»n
yc
h
1-255
zesta
w
ó
w
reguª
(Rule
blo
c
k
ó
w).
Output
v
ariables
Liczba
zmienn
yc
h
wyj±cio
wyc
h
1-255
ze
stero
wnik
a
Input
terms
Maksymalna
liczba
w
ej±cio
wyc
h
1-253
funk
cji
przynale»no±ci
Suma
termó
w
w
ej+wyj
<
254
Output
terms
Maksymalna
liczba
wyj±cio
wyc
h
1-253
funk
cji
przynale»no±ci
Suma
termó
w
w
ej+wyj
<
254
Description
Opis
bloku
funk
cyjnego
String
T
ablica
C.2:
Opis
p
ól
k
on guracji
bloku
funk
cyjnego
Editor
w
ej±cia
(zmienn
yc
h
w
ej±cio
wyc
h
i
funk
cji
przynale»no±ci)
Edycj¦
funk
cji
przynale»no±ci
mo»na
dok
on
yw
a¢
ró
wnie»
przeci¡
ga
j¡c
kra
w
¦dzie
trap
ezu
na
wykresie.
60
HC12F
uzzyEditor
-
Instruk
cja
obsªugi
P
ole
Opis
Zakres
Name
Nazw
a
zmiennej
String
Min
Minimalna
w
arto±¢
zmiennej
Float<Max
Max
Maksymalna
w
arto±¢
zmiennej
Float>Min
Nr
Numer
zmiennej
w
stero
wniku
0..255
Description
Opis
zmiennej
String
Inp
W
y±wietla
rzeczywist¡
w
arto±¢
zmiennej
String
p
o
przesk
alo
w
aniu.
T
ablica
C.3:
Opis
p
ól
k
on guracji
zmiennej
w
ej±cio
w
ej
P
ole
Opis
Zakres
Name
Nazw
a
term
u
String
P
oin
t
1
Punkt
lew
ej
dolnej
p
o
dsta
wy
trap
ezu
0..255
P
oin
t
2
Punkt
pra
w
ej
dolnej
p
o
dsta
wy
trap
ezu
0..255
Slop
e
1
Nac
h
ylenie
lew
ego
b
oku
trap
ezu
0..255
Slop
e
2
Nac
h
ylenie
pra
w
ego
b
oku
trap
ezu
0..255
X0
W
y±wietla
lewy
punkt
dolnej
p
o
dsta
wy
oat
trap
ezu
(p
o
przesk
alo
w
aniu)
X1
W
y±wietla
lewy
punkt
górnej
p
o
dsta
wy
oat
trap
ezu
(p
o
przesk
alo
w
aniu)
X2
W
y±wietla
pra
wy
punkt
dolnej
p
o
dsta
wy
oat
trap
ezu
(p
o
przesk
alo
w
aniu)
X3
W
y±wietla
pra
wy
punkt
górnej
p
o
dsta
wy
oat
trap
ezu
(p
o
przesk
alo
w
aniu)
F
uzzyInp
W
y±wietla
stopie«
akt
yw
acji
funk
cji
[0,1]
przynale»no±ci.
T
ablica
C.4:
Opis
p
ól
k
on guracji
w
ej±cio
w
ej
funk
cji
przynale»no±ci
Rysunek
C.4:
P
anel
edytora
w
ej±cia
Edytor
reguª
P
o
do
daniu
no
w
ej
reguªy
do
bloku
reguª,
nale»y
zaznaczy¢
reguª¦
do
edycji
(p
oprzez
wybranie
jej
w
drzewie
pro
jektu
lub
w
li±cie
reguª),
nast¦pnie
w
drzewie
pro
jektu
klikn¡¢
61
Rysunek
C.5:
P
anel
edytora
regóª
P
ole
Opis
Zakres
Name
Nazw
a
bloku
reguª
String
Description
Opis
zmiennej
String
Ev
aluate
W
ylicza
w
arto±ci
zmienn
yc
h
[przycisk]
wyj±cio
wyc
h
dla
usta
wionego
w
edytorze.
T
ablica
C.5:
Opis
p
ól
k
on guracji
bloku
reguª
lewym
przyciskiem
m
yszy
na
wybran¡
funk
cj¦
przynale»no±ci
i
nast¦pnie
klikn¡¢
na
p
ole
IF
lub
THEN
(w
zale»no±ci
czy
funk
cja
przynale»no±ci
jest
w
ej±cio
w
a
czy
wyj±cio
w
a).
W
ten
sp
osób
t
w
orzon
y
jest
w
arunek
i
wniosek
reguªy
.
Ab
y
usun¡¢
funk
cj¦
przynale»no±ci
z
reguªy
nale»y
wybra¢
j¡
w
p
olu
IF
lub
THEN
a
nast¦pnie
klikn¡¢
przycisk
DELETE.
Edytor
wyj±cia
(zmienn
yc
h
wyj±cio
wyc
h
i
funk
cji
przynale»no±ci
wyj±cio
wyc
h)
P
ole
Opis
Zakres
Name
Nazw
a
zmiennej
String
Min
Minimalna
w
arto±¢
zmiennej
Float<Max
Max
Maksymalna
w
arto±¢
zmiennej
Float>Min
Nr
Numer
zmiennej
w
stero
wniku
0..255
Description
Opis
zmiennej
String
Outp
W
yliczona
na
p
o
dsta
wie
p
ozycji
String
silgnetonó
w
oraz
stopnia
akt
yw
acji.
w
arto±¢
dla
zmiennej
wyj±cio
w
ej
T
ablica
C.6:
Opis
p
ól
k
on guracji
zmiennej
wyj±cio
w
ej
P
ozycj¦
singletonó
w
oraz
ic
h
w
arto±ci
mo»na
ró
wnie»
edyto
w
a¢
przez
przeci¡
ganie
punktó
w
na
wykresie.
62
HC12F
uzzyEditor
-
Instruk
cja
obsªugi
P
ole
Opis
Zakres
Name
Nazw
a
term
u
String
P
os
P
ozycja
singleton
u
0..255
Real
p
os
P
ozycja
singleton
u
(p
o
przesk
alo
w
aniu)
oat
F
uzzyOut
W
y±wietla
stopie«
akt
yw
acji
funk
cji
[0,1]
przynale»no±ci.
T
ablica
C.7:
Opis
p
ól
k
on guracji
wyj±cio
w
ej
funk
cji
przynale»no±ci
Rysunek
C.6:
P
anel
edytora
wyj±cia
64
Skªadnia
j¦zyk
a
F
CL
Do
datek
D
Skªadnia
j¦zyk
a
F
CL
function_blo
c
k_declaration::=
'FUNCTION_BLOCK'
function_blo
c
k_name
{fb_io_v
ar_declarations}
{other_v
ar_declarations}
function_blo
c
k_b
o
dy
'END_FUNCTION_BLOCK'
fb_io_v
ar_declarations
::=
input_declarations
|
output_declarations
other_v
ar_declarations
::=
v
ar_declarations
function_blo
c
k_b
o
dy
::=
{fuzzify_blo
c
k}
{defuzzify_blo
c
k}
{rule_blo
c
k}
{option_blo
c
k}
fuzzify_blo
c
k
::=
'FUZZIFY'
v
ariable_name
{linguistic_term}
'END_FUZZIFY'
defuzzify_blo
c
k
::=
'DEFUZZIFY'
f_v
ariable_name
{linguistic_term}
defuzzi cation_metho
d
default_v
alue
[range]
'END_DEFUZZIFY'
rule_blo
c
k
::=
'R
ULEBLOCK'
rule_blo
c
k_name
op
erator_de nition
[activ
ation_metho
d]
accum
ulation_metho
d
{rule}
'END_R
ULEBLOCK'
option_blo
c
k
::=
'OPTION'
an
y
man
ufacturere
sp
eci c
parameter
'END_OPTION'
65
linguistic_term
::=
'TERM'
term_name
':='
mem
b
ership_function
';'
mem
b
ership_function
::=
singleton
|
p
oin
ts
singleton
::=
n
umeric_literal
|
v
ariable_name
p
oin
ts
::=
{'('
n
umeric_literal
|
v
ariable_name
','
n
umeric_literal
')'}
defuzzi cation_metho
d
::=
'METHOD'
':'
'COG'
|
'COGS'
|
'CO
A'
|
'LM'
|
'RM'
';'
default_v
alue
::=
'DEF
A
UL
T'
':='
n
umeric_literal
|
'NC'
';'
range
::=
'RANGE'
':='
'('n
umeric_literal
'..'
n
umeric_literal')'
';'
op
erator_de nition
::=
('OR'
':'
'MAX'
|
'ASUM'
|
'BSUM')
|
('AND'
':'
'MIN'
|
'PR
OD'
|
'BDIF')
';'
activ
ation_metho
d
::=
'A
CT'
':'
'PR
OD'
|
'MIN'
';'
accum
ulation_metho
d
::=
'A
CCU'
':'
'MAX'
|
'BSUM'
|
'NSUM'
';'
rule
::=
'R
ULE'
in
teger_literal
':'
'IF'
condition
'THEN'
conclusion
[WITH
w
eigh
ting_factor]
';'
condition
::=
(sub
condition
|
v
ariable_name)
{AND'
|
'OR'(sub
condition
|
v
ariable_name)}
sub
condition
::=
('NOT'
'('
v
ariable_name
'IS'
['NOT']
)
term_name
')')
|
(
v
ariable_name
'IS'
['NOT']
term_name
)
conclusion
::=
{
(v
ariable_name
|
(v
ariable_name
'IS'
term_name))
','}
(v
ariable_name
|
v
ariable_name
'IS'
term_name)
w
eigh
ting_factor
::=
v
ariable
|
n
umeric_literal
function_blo
c
k_name
::=
iden
ti er
rule_blo
c
k_name
::=
iden
ti er
term_name
::=
iden
ti er
f_v
ariable_name
::=
iden
ti er
v
ariable_name
::=
iden
ti er
n
umeric_literal
::=
in
teger_literal
|
real_literal
66
Skªadnia
j¦zyk
a
F
CL
input_declarations
::=
see
IEC
1131-3
Annex
B
output_declarations
::=
see
IEC
1131-3
Annex
B
v
ar_declarations
::=
see
IEC
1131-3
Annex
B
iden
ti er
::=
see
IEC
1131-3
Annex
B
Spis
rysunk
ó
w
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
5
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
7
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
8
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
10
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
11
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
11
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
12
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
14
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
16
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
18
.
.
.
.
.
.
.
22
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
23
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
25
.
.
.
.
.
.
.
.
.
.
.
.
.
28
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
33
.
.
.
.
.
.
.
.
.
.
.
.
.
34
.
.
.
.
.
.
.
.
.
.
.
.
.
.
35
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
35
.
.
.
.
.
.
.
.
.
.
.
.
.
.
38
.
.
.
.
.
.
.
.
.
.
.
.
.
.
39
.
.
.
.
.
.
.
.
.
.
.
39
.
.
.
.
.
.
.
.
.
.
.
.
.
.
41
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
41
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
42
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
42
.
.
.
.
.
.
.
.
.
.
.
.
.
.
43
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
43
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
44
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
44
.
.
.
.
.
.
.
.
.
.
.
.
.
.
45
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
45
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
45
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
46
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
49
68
SPIS
R
YSUNKÓ
W
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
50
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
51
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
52
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
52
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
53
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
54
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
55
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
56
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
56
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
57
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
58
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
59
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
60
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
61
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
62
Spis
tablic
.
.
.
.
.
.
.
.
.
.
.
.
.
.
9
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
50
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
54
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
58
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
59
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
60
.
.
.
.
.
.
.
.
.
.
.
60
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
61
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
61
.
.
.
.
.
.
.
.
.
.
.
62
70
SPIS
T
ABLIC
Bibliogra a
[1]
R.
C.
Arkin
Behavior-b
ase
d
r
ob
otics
,
The
MIT
Press
Cam
bridge,
Massac
h
usetts
London,
England
1998.
[2]
Z.
Hendzel,
A.
Burghardt,
Implementacja
sie
ci
Kohonena
w
ster
owniku
b
ehawior
al-
nym
mobilne
go
r
ob
ota
koªowe
go
w:
K.
T
c
ho«
(red.),
Post¦py
R
ob
otyki
-
ster
owanie,
p
er
c
ep
cja
i
komunikacja
,
W
yda
wnict
w
o
k
om
unik
acji
i
ª¡czno±ci,
W
arsza
w
a
2006.
[3]
R.
Hug,
G.
K.
I.
Mann,
R.
G.
Gosine,
Behavior-Mo
dulation
T
e
chnique
in
Mobile
R
o-
b
otics
Using
F
uzzy
Discr
ete
Event
System
,
IEEE
TRANSA
CTIONSON
R
OBOTICS,
V
OL.
22,
NO.
5.
OCTOBER
2006.
[4]
IEC
1131
Pr
o
gr
ammable
Contr
ol
lers.
Part
7
-
F
uzzy
Contr
ol
Pr
o
gr
amming
,
Commi-
tee
Draft
CD
1.0
(Rel.
19
jan
97).
[5]
K.
Aren
t,
M.
W
n
uk,
,Konstrukcja
i
zastosowanie
inter
aktywne
go
r
ob
ota
ter
ap
eutycz-
ne
go
Ko
ala
,
Rap
ort
SPR
24/07,
Inst.
Cyb.
T
ec
hn.
P
olitec
hnik
a
W
ro
cªa
wsk
a
2007.
[6]
K.
Aren
t,
M.
W
n
uk,
M.
Kabaªa,
Pr
o
gr
amowanie
i
konstrukcja
kuliste
go
r
ob
ota
sp
o-
ªe
czne
go
wspier
aj¡c
e
go
ter
api¦
dzie
ci
autystycznych
,
Rap
ort
SPR
3/05,
Inst.
Cyb.
T
ec
hn.
P
olitec
hnik
a
W
ro
cªa
wsk
a
2005.
[7]
M.
W
n
uk,
,K
O
ALA:
inter
aktywny
r
ob
ot
kulisty
do
wsp
omagania
ter
apii
dzie
ci
auty-
stycznych
,
Rap
ort
PRE
21/06,
Inst.
Cyb.
T
ec
hn.
P
olitec
hnik
a
W
ro
cªa
wsk
a
2006.
[8]
M.
W
n
uk
Mo
duª
z
mikr
okontr
oler
em
MC9S12A64
,
Rap
ort
SPR
11/2005,
Inst.
Cyb.
T
ec
hn.
P
olitec
hnik
a
W
ro
cªa
wsk
a
2005.
[9]
A.
Materek
Pr
osty
mo
dem
r
adiowy
,
Rap
ort
SPR
1/2007
Inst.
Cyb.
T
ec
hn.
P
olitec
h-
nik
a
W
ro
cªa
wsk
a
2007.
[10]
A.
Materek
R
ob
ot
minisumo
-
Gizmo
v2
,
Rap
ort
PRE
2007
Inst.
Cyb.
T
ec
hn.
P
olitec
hnik
a
W
ro
cªa
wsk
a
2007.
[11]
www.chip
c
on.c
om,
CC1000
Data
Sheet,
Chip
con
pro
ducts
from
T
exas
Instrumen
ts,
2005.
[12]
www.chip
c
on.c
om,
CC1000PP
User
Man
ual
Rev.
1.22,
Chip
con
pro
ducts
from
T
exas
Instrumen
ts,
2003.
[13]
www.chip
c
on.c
om,
Aplication
note
AN009,
CC1000/C1050
Micro
con
troller
in
terfa-
cing,
Chip
con
pro
ducts
from
T
exas
Instrumen
ts,
2004.
72
BIBLIOGRAFIA
[14]
www.chip
c
on.c
om,
CC1000
Errata
Note
001,
rev.
1.0,
Chip
con
pro
ducts
from
T
exas
Instrumen
ts.
[15]
www.atmel.c
om,
A
TMega8
Data
Sheet,
A
TMEL,
2004.
[16]
Austriamicrosystems.
AS5040
10
BIT
360
◦
PR
OGRAMMABLE
MA
GNETIC
R
O-
T
AR
Y
ENCODER
,
2006.
Datasheet.
[17]
F
reescale.
MC9S12DJ64
Devic
e
User
Guide
V01.19
,
2004.
User
man
ual.
[18]
Motorola.
S12CPUV2/D
R
efer
enc
e
Manual
,
2003.
[19]
jfuzzylo
gic.sour
c
efor
ge.net.
Document Outline
- Wstęp
- Budowa robota
- Przykłady implementacji zachowań
- Podsumowanie
- Płyta główna robota
- Radiomodem USB
- HC12FuzzyEditor - Instrukcja obsługi
- Składnia języka FCL
- Spis rysunków
- Spis tabel
- Bibliografia
Wyszukiwarka
Podobne podstrony:
Implementacja algorytmów sterowania osi robota
Implementacja algorytmów sterowania osi robota
Projekt robota mobilnego szukającego wyjścia z labiryntu, MECHATRONIKA
Instalacja nowszych sterowników do układów mobilnych w laptopie tzw modowanie sterowników
Projekt robota mobilnego szukającego wyjścia z labiryntu, MECHATRONIKA
Prusinski Wojciech modelowanie dwukolowego robota mobilnego
Podstawy sterowania siłowego w robotach
36. Procesy automatyczne i kontrolowane i ich rola w sterowaniu zachowaniem.
Implementacja i badania algorytmów sterowania robotem dwukołowym
MATLAB STEROWANIE ROBOTAMI, MATLABIK2
Roboty mobilne, Wiadomości wstępne o robotach
MATLAB STEROWANIE ROBOTAMI MATLABIK STEROWANIE
MATLAB STEROWANIE ROBOTAMI MATLABIK4
MATLAB STEROWANIE ROBOTAMI MATLABIK6
MATLAB STEROWANIE ROBOTAMI, MATLABIK3
MATLAB STEROWANIE ROBOTAMI, MATLABIK6
więcej podobnych podstron