TIU LUN ROBOT

GVHD: CHU VN BO

I. Gii thiu phn mm EASY-ROB 2.0

EASY-ROB l cng c m phng robot s dng ho trong khng gian 3
chiu (3D) v cc hnh nh c th hot ng c. Mt h thng 3D-CAD n gin
cho php to ra cc khi hnh hc c bn nh khi tr, khi cu, khi ch nht, khi
tam gic, khi hnh thang, ... v kt cu ca robot. Trong EASY-ROB chng ta c
th dng chut quay hoc tnh tin robot n mt to tu . EASY-ROB cng c
cc chc nng phng to, thu nh i tng v nh nhiu phn mm thit k khc...
Chng trnh cho php thit k cc robot n 12 bc t do. Chuyn ng ca Robot c
th c iu khin theo cc bin khp hoc cc to -ct. Chng ta cng c th
m t ng hc ca robot theo kiu DH hoc trong h to ton cc (Universa
Coordinates). Easy-Rob c sn cc trnh iu khin ng hc thun v ngc ca
cc cu hnh robot thng dng, khi thit k ta ch cn khai bo kiu ng hc thch
hp. Trong trng hp robot c kt cu c bit hoc c cc khu b ng gn vi cc
chuyn ng ca cc khp th cn phi gii bi ton ng hc ngc hoc xc nh
hm ton hc m t s ph thuc ca khu b ng i vi khp quay, vit chng
trnh xc nh s ph thuc bng ngn ng C v sau dng tp tin MAKE.EXE
trong C dch thnh tp tin th vin lin kt ng er_kin.dll (Easy- Rob kinematic
Dynamic link library), khi chy chng trnh, EASY-ROB s lin kt vi tp tin ny
v thc hin kiu ng hc c khai bo trong chng trnh iu khin.
Easy-ROB c mt s cc lnh iu khin ring, Chng trnh c vit theo
kiu x l tun t, tp tin dng Text, c th son tho chng trnh trong bt k trnh
son tho no. Cc cng c gn trn khu chp hnh cui c th thay i c. Chng
ta c th vit mt chng trnh chuyn ng cho mt robot theo mt qu o mong
mun, c th kim tra kh nng vn ti ca cnh tay, xc nh vng lm vic ca
robot . . . Robot m phng c th cm nm hoc th cc i tng lm vic. Cc
chuyn ng ca robot c th ghi vo mt tp tin v c th thc hin li.
Phn mm cho php ta xem c cc h to gn trn cc khu ca robot,
xem c qu o chuyn ng ca im cui cng c gn trn khu chp hnh cui.
Phn mm cn c nhiu tin ch khc nh : cho php ta lp trnh iu khin robot bng
phng php dy hc, thit k cc i tng lm vic ca robot, c cc ca s v to
v gi tr gc quay ca cc khp ti tng thi im khi robot hot ng...
Vic s dng phn mm EASY-ROB m phng robot gip chng ta hai kh
nng nghin cu :
a. M phng li mt robot c v cc i tng lm vic ca n. nh gi
kh nng lm vic v mc linh hot ca robot, xc nh cc thng s iu khin,
qu o chuyn ng dng trong iu khin thc.
b. Nghin cu thit k ng hc, cc kch thc v kt cu ca robot trn my
tnh c th chn c phng n ng hc tt nht, m bo cho robot hon thnh
cc nhim v yu cu.
II. HNG DN S DNG PHN MM EASY ROB 2.0
1. Lu gii thut ca chng trnh.

Khi ng chng trnh

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

Thit k h ta

Thit k hnh dng robot

Xy dng chng trnh iu khin

robot

Chy m phng
Hnh
2.1:
Lu gii thut ca EASY ROB
2. Giao din lm vic ca phn mm.
Khi ng chng trnh
EASY ROB 2.0 l chng trnh t chy bn khng cn phi ci t. chy
chng trnh bn kch vo tp tin EASYROBW.EXE
Giao din lm vic ca phn mm gm 3 phn chnh:
Ca s lm vic
Cc thanh cng c
Thanh menu chnh

Hnh 2.2 Mn hnh lm vic ca EASY ROB 2.0

a. Thanh menu chnh ca chng trnh

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

Menu file: Dng load, lu, xa cc dng file ca robot

File c dng *.Rob : (Robotfile) m t ring kt cu ca mt robot.

File c dng *.Bod : (Bodyfile) m t cc i tng lm vic ca robot.
File c dng *.Tol : (Toolfile) m t cng c gn trn khu chp hnh cui ca
robot.
File c dng *.Vie : (Viewfile) xc nh gc nhn trong khng gian.
File c dng *.igp : (Igrip Partfile) lu tr mt b phn kt cu.
File c dng *.Prg : (Programm) Chng trnh iu khin.
Menu Robotics : Dng nhp cc thng s DH, xc nh v tr ca dng c, xc nh
v tr robot v cc thng s khc.

Menu simulate: Dng m phng hot ng ca robot.

Menu 3D-CAD :
EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

Cung cp cc cng c v kt cu robot trong khng gian 3 chiu (3D) cng

nh thit k cc cng c, cc i tng lm vic. v c kt cu ca robot,
da vo cc khi hnh hc n gin ta c th lp ghp chng li to nn cc hnh
dng khc nhau ca robot.

Menu view: Dng xem mn hnh robot di cc gc khc nhau.

Menu aux :
Dng iu chnh robot trong qu trnh thit k v trong qu trnh lp trnh cho
robot .

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

Menu help : Tin ch gip ca chng trnh

b. Thanh cng c nm ngang pha trn

Chc nng ca cc nt trn thanh cng c, tnh t tri qua phi.

1. Bt tt ch chiu sng cc i tng v.
2. Chuyn tt c cc i tng sang dng li.
3. Chuyn i tng dng tr / khi phc tp.
5. Th hin/khng th hin sn.
6. Th hin sn dng li.
7. Reset v tr robot trn mn hnh.
8. Chuyn i ca s khi m Cellfile hoc igip partfile (kt hp vi nt 7).
9. Chy chng trnh.
10. Tm dng chng trnh.
11. Tip tc chy cng trnh.
12. Kt thc chng trnh.
13. Chy chng trnh theo tng bc.
14. Lp li chng trnh sau khi kt thc.
15. 16. Gim v tng tc iu khin.
17. nh gi sai s v xem cc gi tr ng hc
c. Thanh cng c nm ngang pha di

Chc nng ca cc nt trn thanh cng c, tnh t tri qua phi:

1. Thy hoc khng thy kt cu robot.
2. Thy hoc khng thy dng c.
3. Thy hoc khng thy cc i tng lm vic.
4. Th hin/khng th hin h to gn vi dng c .
5. Th hin/khng th hin h to gn trn cc khu ca robot.
6. Th hin v tr iu khin.
7. M phng ng lc hc.
8. Th hin qu o chuyn ng.
9. S dng cc gii hn ca khp.
10. Son tho chng trnh v dy hc.
12. Th hin hoc khng th hin H to gn trn i tng hin thi.
13. Chuyn n i tng tip theo (khi thit k).
14. Xc nh v tr tuyt i ca i tng hin ti.
15. Xc nh v tr tng i ca i tng hin ti.
16. Reset v tr ca i tng hin ti.
17. Ghi li v tr ca i tng sau khi iu chnh.
18. a robot v v tr dng (Home position).
19. iu khin robot theo khp quay.
EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

d. Thanh cng c thng ng

Chc nng ca cc nt trn thanh cng c, tnh t trn sung di.
1. Dng chut view, zoom v Pan.
2.3. iu khin hng ca khu chp hnh cui bng chut.
4. iu khin cc khp 1,2,3 (Dng cc phm chut).
5. Di chuyn thn robot. (h to c s)
6. Di chuyn cc i tng (body) bng chut.
7. Di chuyn tt c cc i tng bng chut.
9. Chuyn i chuyn ng l quay hoc tnh tin
11.12. Tng gim tc iu khin bng chut
3. Thao tc chut
Easy-Rob cho php dng chut vi nhiu chc nng nh :
Khi nt lnh s 1 ca thanh cng c thng ng c chn :
Zoom (Phng to, thu nh) : n nt chut phi, r chut ln xung theo phng
thng ng ca mn hnh.
Pan (thay i v tr ca i tng so vi khung mn hnh) : n ng thi hai
nt chut phi v tri, r chut trn mn hnh.
Rotate (quay robot nhn cc gc khc nhau) : n chut tri, r chut.
Khi nt lnh s 4 ca thanh cng c thng ng c chn ln th nht s iu khin
ba khp u tin (1, 2, 3) :
Quay khp 1: n nt chut phi, r chut (nu l khp tnh tin s lm khu
chuyn ng tnh tin).
Quay khp 2: n ng thi 2 nt chut phi v tri, r chut.
Quay khp 3: n nt chut tri, r chut.
Khi click ln nt s 4 ln th hai s iu khin ba khp tip theo (4, 5, 6) iu
khin cc khp lm ging nh trn.
Quay khp 4: n nt chut phi, r chut (nu l khp tnh tin s lm khu
chuyn ng tnh tin).
Quay khp 5: n ng thi 2 nt chut phi v tri, r chut.
Quay khp 6: n nt chut tri, r chut.
Tng t khi nt s 4 c chn ln th 3 s iu khin c ba khp tip theo.
4. Xy dng h ta
Mun xc nh h to ca robot trc ht phi thc hin bng tay cc cng
vic sau:
V s ng robot v tr dng, gn h to ca cc khu ln hnh v
trn giy, xc nh cc thng s DH.
Cc buc tip theo :
1- Bt nt lnh s 5 trn menu ngang, di.
2- Vo menu chnh : FILE -> LOAD -> ROBOTFILE chn DHTempl ->
OPEN.
3- Vo menu chnh : ROBOTICS -> ROBOTMOTION + KINEMATICS
EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

->KINEMATICS DATA.
4- Chn Active Join -> Ok -> Activ Joint (1) RZ (hoc chn TZ nu l
khp
tnh tin) -> Ok -> Nhp cc thng s DH ca khu th nht.
5- Chn Quit -> Ok.
Vo li bc 4 -> Number Active Joint(1) -> Ok -> n p chut vo vt xanh
hoc a con tr vo phn nhp d liu (text box) n 2 (By gi s khu ng l 2),
nhp cc thng s DH cho khu s 2 ...
Lm tng t cho n khi s khp yu cu.
Ta c th kim tra cc s liu nhp bng cch kch chut vo menu :
ROBOTICS -> ROBOTMOTION + KINEMATICS -> KINEMATICS DATA->
KINEMATIC INFOMATION xem li s khu, khp v cc thng s DH. Nu vo
d liu sai ta c th hiu chnh li.
th hin h to ca robot trn mn hnh (H to mu vng), nh kch
chut vo nt s 5 ca thanh cng c nm ngang pha di.
5. Thit k hnh dng robot
Sau khi hon thnh vic gn h to ca robot, bc tip theo l v hnh dng
ca n. Hnh dng ca robot c th c m phng ging nh robot thc nh cng c
3D CAD ca EasyRob. Menu 3D-CAD cho php to ra cc khi hnh hc c bn nh
khi tr, khi cu, khi ch nht, khi tam gic ... S phi hp hp l v kch thc
v v tr ca cc khi hnh hc ny cho php th hin c cc kt cu khc nhau ca
robot.
Cc menu ko xung ca Menu 3D- CAD , mt s cc chc nng chnh nh
sau:

Select group : Chn nhm i tng thit k : 1/Robot group, 2/Tool

group hay 3/ Body group.
Select body from group : Chn cc b phn ca robot v (theo tn t trc)
ca nhm chn hin hnh.
Create/Import new 3D body : To mi hoc nhp mt b phn c sn.
Cn nhp cc thng s cn thit to ra i tng mong mun.
Modify sel. Body_set Jnt_idx : Hiu chnh cc thuc tnh ca b phn hin
hnh.
Clone : Copy b phn ang v thnh nhiu hnh.
Render : Biu hin i tng dng li, dng hp, . . .
Color : Thay i mu sc.
Name:Thay i tn b phn ang v.
Clear : Xo i tng (b phn) hin hnh.
Position's : Thay i v tr ca i tng (b phn) hin hnh.
3D CAD Coorsys Visibility : Cho hin hoc n h ta ca i tng
EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

Next Body in group : Chn i tng v tip theo.

Dng menu 3D CAD ta ln lt v tt c cc khu ca robot, c th dung cc mu
sc khc nhau th hin hnh dng ca robot. Lu trong qu trnh v, nu v sai
phi dng mc CLEAR xa i hoc dng mc MODIFY CEL hiu chnh. Mi
i tng v phi gn vi mt khu nht nh, c khai bo trong mc SET JOINT
INDEX.
C th dng thanh cng c thng ng pha phi thay i v tr ca cc i
tng v cho thch hp.
6. Lp trnh iu khin robot
lp trnh iu khin robot m phng ta dng phng php lp trnh kiu
dy hc. Sau khi thit k hnh dng robot, cng c gn trn khu chp hnh cui,
cc i tng lm vic khc . . . ta c th lp trnh iu khin robot m phng.
Vic lp trnh thc hin theo trnh t sau y :
Nhp chut vo nt lnh s 10 (Show program window) kch hot ca s lp trnh.

Chn New t tn cho File chng trnh.

Chn Append nu mun b sung mt chng trnh c trn a.
Xc nh v tr cc im m dng c phi i qua (dng chut iu khin cc
khp, dng menu ng). C sau mi ln xc nh c mt v tr th n nt PTP (iu
khin im) hoc LIN (iu khin ng) hoc VIA (im trung gian dn hng khi
iu khin ng cong), CIRC (iu khin theo ng cong). Lm lin tc cho tt c
cc im c mt chng trnh hon thin.
Sau khi kt thc vic dy robot hc, n nt Close trn Program Window kt
thc. hiu chnh v b sung cc lnh iu khin khc vo chng trnh, n chut
vo nt EDIT, Dng cc lnh ca EasyRob nh di y hon thin chng
trnh.
Mt s lnh thng dng trong lp trnh:
PROGRAMFILE : Bt u chng trnh
ENDPROGRAMFILE or END : Kt thc chng trnh.
CALL fct_name : Gi mt hm c tn fct_name(), c nh ngha trong chng
trnh.
CALL FILE filename : Gi mt File chng trnh c tn filename, File phi c cung
cu trc nh chng trnh chnh.
FCT fct_name() : Bt u nh ngha mt hm c tn fct_name(). ENDFCT : Kt thc
nh ngha mt function.
! Cc ghi ch trong chng trnh.
TOOL X Y Z A B C [m,deg] : nh ta im cui ca dng so so vi khu chp
hnh cui.
EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

PTP X Y Z A B C [m,deg] : Di chuyn robot n im mi (ta tuyt i). iu

khin im.
PTP_REL dX dY dZ dA dB dC [m,deg] : Di chuyn robot n im mi (ta
tng i). iu khin im.
LIN X Y Z A B C [m,deg] : Di chuyn robot n im mi (ta tuyt i). iu
khin ng.
LIN_REL dX dY dZ dA dB dC [m,deg] : Di chuyn robot n im mi (ta tng
i). iu khin ng.
CIRC X Y Z A B C [X2 Y2 Z2] [m,deg] : Di chuyn robot n im mi (ta tuyt
i). iu khin ng cong.
[X2 Y2 Z2] - im trung gian (3 im xc nh mt cung trn).
CIRC_REL dX dY dZ dA dB dC [dX2 dY2 dZ2] [m,deg] : Di chuyn robot n im
mi (ta tng i). iu khin ng cong.
WAIT x [sec] : Robot dng hot ng trong x giy.
ERC TRACK ON,OFF : Th hin hoc khng th hin qu o chuyn ng.
ERC LOAD TOOL filename : Gi mt Tool file (*.tol)
ERC LOAD VIEW filename : Gi mt View file (*.vie) ERC LOAD ROBOT
filename Loads a Robot file (*.rob) ERC LOAD BODY filename Loads a Body file
(*.bod) ERC LOAD TAGS filename Loads a Tag file (*.tag)
ERC GRAB BODY bodyname : Dng c cm ly mt vt th (body) c tn
Bodyname.
ERC GRAB BODY_GRP : Dng c cm ly mt nhm vt th (Body_Grp).
ERC RELEASE BODY bodyname : Dng c th (bung) mt vt th (body) c tn
Bodyname.
ERC RELEASE BODY_GRP Dng c th (bung) mt nhm vt th (Body_Grp).
ERC ROBOT_BASE XYZ ABC [m,deg] : Di chuyn gc ta c bn ca robot
n v tr mi. v.v...
Ghi ch :
- n v chiu di l Mt [m], Gc l [deg] hoc [%]
- n v ca tc l [m/s]
- V tr v hng ca h ta gn trn khu chp hnh cui c xc nh gm : X,
Y v Z : ch ta v tr, A, B v C ch gc hng.
Hng ca khu chp hnh cui xc nh theo cc gc ABC l: Rot (A,B,C) =
Rot(X,A) * Rot(Y,B) * Rot(Z,C)
III. THIT K V M PHNG MT S ROBOT THNG DNG
1. M phng robot ELBOW
ELBOW l robot c 6 khu vi cu hnh RRRRRR. Tt c 6 khu u chuyn ng
quay.

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

Hnh 3.1 Robot Elbow

Khu
1

ai

di

90 0

d1

a2

a3

-90 0

a4

90 0

d4

Hnh 3.2 Bng thng s DH ca robot Elbow

Trong v d m phng robot ny chng ta chn cc gi tr nh sau:
a2 = 0.4m
a3 = 0.3m
a4 = 0.25m
d1 = 0.45m
d 4 = 0.15m

Cc gi tr i chn ban u bng 0

B1. Xy dng h to .
Bt nt lnh s 5 trn menu ngang, di.
Vo menu chnh : FILE -> LOAD -> ROBOTFILE chn DHTempl ->
OPEN.
Vo menu chnh : ROBOTICS -> ROBOTMOTION + KINEMATICS ->
KINEMATICS DATAChn Active Join -> Ok -> Activ Joint (1) RZ (khp th nht
l khp quay) -> Ok -> Nhp cc thng s DH ca khu th nht.
Vo li bc 4 -> Number Active Joint(1) -> Ok -> n p chut vo vt xanh hoc
a con tr vo phn nhp d liu (text box) n 2 (By gi s khp ng l 2), nhp
cc thng s DH cho khu s 2.

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

Lm t ng t nhp cc thng s DH cho 4 khu cn li. Ch tt c cc khp

u l khp quay (Activ Joint (n) RZ)

Kt qu cc h ta gn trn cc khu nh hnh sau

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

Ta c th kim tra cc s liu nhp bng cch kch chut vo menu : ROBOTICS
-> ROBOTMOTION + KINEMATICS -> KINEMATICS DATA-> KINEMATIC
INFOMATION xem li s khu, khp v cc thng s DH. Nu vo d liu sai ta
c th hiu chnh li.

B2. Thit k hnh dng robot

Thit k robot
Vo menu 3D-CAD -> select group -> robot group-> ok ( thit k robot)

Thit k khu th nht:

Vo menu 3D-CAD -> create/import new 3D body-> cylinder ( chn khi
hnh tr)

Chn v tr t ca khu

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

iu chnh kch thc ca khu

Chn khp m khu ni vo

t tn cho khu

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

Nu cha va chng ta c th sa cha kch thc khu, thay i v tr ca

khu, thay i mu hoc c th xa i.
Lm tng t thit k hnh dng cc khu cn li.
Kt qu hnh dng robot sau khi thit k.

Thit k dng c lm vic.

Vo menu 3D-CAD -> select group -> tool group-> ok ( thit k dng
c lm vic)

Tng t nh trn ta c th thit k c hnh dng dng c lm vic theo

mun ca mnh.

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

Thit k i tng lm vic:

Vo menu 3D-CAD -> select group -> body group-> ok ( thit k i
tng lm vic lm vic)

Kt qu sau khi thit k xong robot.

B3. Lp trnh iu khin robot.

Sau khi thit k xong robot cng vic cui cng ca chng ta l lp trnh iu
khin robot.
Chng trnh iu khin robot Elbow.
PROGRAMFILE
! prgfln E:\LE KHANH\EASY_ROB\proj\program_elbow.prg
! TRINH DIEN RO BOT
ERC LOAD VIEW K1
WAIT 2
ERC LOAD VIEW K2
WAIT 2
ERC LOAD VIEW K3
WAIT 2

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

ERC LOAD VIEW K4

WAIT 2
ERC LOAD VIEW K5
WAIT 2
ERC LOAD VIEW K6
WAIT 2
! GOI CAC CHUONG TRINH CON
CALL A1
CALL A2
CALL A3
CALL A4
CALL A5
CALL A6
! CHUONG TRINH A1
FCT A1 ()
PTP_AX
-0.0000 44.0000 -53.5000 -72.5000
PTP_AX
-29.5000 39.0000 -43.0000 -83.5000
ERC LOAD VIEW W1
PTP_AX
-29.5000 39.0000 -43.0000 -83.5000
PTP_AX
-29.5000 29.5000 -43.0000 -78.5000
ERC LOAD TOOL KHANHPRO_TOOL_CLOSE
ERC GRAB BODY A1
PTP_AX
-31.0000 39.5000 -43.0000 -88.0000
PTP_AX
-77.9999 55.5000 -87.5000 -59.0001
ERC LOAD VIEW W4
PTP_AX
-69.0000 60.0000 -97.4999 -52.5001
ERC LOAD VIEW W2
PTP_AX
-68.5000 60.0000 -98.9999 -51.5001
PTP_AX
-68.5000 49.5000 -98.9999 -44.0001
ERC LOAD TOOL KHANHPRO_TOOL_OPEN
ERC RELEASE BODY A1
ENDFCT
! CHUONG TRINH A2
FCT A2 ()
ERC LOAD VIEW W
PTP_AX
-68.5000 64.0000 -98.9999 -54.0001
PTP_AX
-68.5000 64.0000 -98.9999 -54.0001
PTP_AX
-20.5000 42.0001 -55.4999 -75.5001
PTP_AX
-20.5000 42.0001 -54.4999 -78.0001
PTP_AX
-20.5000 35.0001 -54.4999 -73.0001
ERC LOAD TOOL KHANHPRO_TOOL_CLOSE
ERC GRAB BODY A2
PTP_AX
-20.5000 46.0001 -54.4999 -81.5000
PTP_AX
77.5000 59.0001 -95.9997 -56.0001
PTP_AX
77.0000 50.5001 -99.9997 -43.5001
ERC LOAD TOOL KHANHPRO_TOOL_OPEN
ERC RELEASE BODY A2
ENDFCT
! CHUONG TRINH A3
FCT A3 ()
PTP_AX
77.0000
PTP_AX
77.0000
PTP_AX
-11.3750
PTP_AX
-10.7500

EASY ROB 2.0

62.3750
62.3750
62.3750
52.8751

-99.9997
-99.9997
-99.9997
-70.1249

0.0000 0.0000
91.0000 -61.9999
91.0000 -120.9998
91.0000 -120.9998
91.0000 -120.9998
1.0000 -120.9998
90.5001 -120.9998
90.5001 -69.9998
90.5001 -69.9998

91.0001 -69.9998
181.5000 -69.9998
90.9999 -69.9998
90.9999 -103.4997
91.4999 -103.4997
91.4999 -103.4997
90.9999 -103.4997
91.9999 -100.9997

-51.2501 91.9999 -100.9997

-49.7501 3.9999 -100.9997
-49.7501 3.9999 -100.9997
-69.6250 89.6249 -143.8749

NHM 9

TIU LUN ROBOT

PTP_AX
-10.7500 52.8751 -70.1249 -69.6250
ERC LOAD VIEW W1
PTP_AX
-10.7500 41.5001 -70.1249 -61.0001
ERC LOAD TOOL KHANHPRO_TOOL_CLOSE
ERC GRAB BODY A3
PTP_AX
-10.7500 56.1251 -70.1249 -73.0001
PTP_AX
-10.7500 56.1251 -70.1249 -72.2500
PTP_AX
-83.5000 56.1251 -70.1249 -72.3750
PTP_AX
-83.5000 54.0002 -91.3747 -53.0002
ERC LOAD VIEW W2
PTP_AX
-83.5000 54.0002 -91.3747 -53.0002
PTP_AX
-83.3750 46.5002 -91.9997 -46.6252
ERC LOAD TOOL KHANHPRO_TOOL_OPEN
ERC RELEASE BODY A3
ENDFCT
! CHUONG TRINH A4
FCT A4 ()
ERC LOAD VIEW W
PTP_AX
-83.3750 65.5001 -92.4997 -62.1252
PTP_AX
2.6250 55.5002 -71.4998 -73.1252
PTP_AX
1.6250 53.0002 -63.4999 -77.6251
PTP_AX
2.5000 45.0002 -75.9998 -58.2502
ERC LOAD TOOL KHANHPRO_TOOL_CLOSE
ERC GRAB BODY A4
PTP_AX
3.7500 64.8751 -96.1247 -61.2501
ERC LOAD VIEW W3
PTP_AX
68.3750 52.3752 -86.6248 -55.7502
PTP_AX
68.0000 44.1252 -86.3748 -48.5002
ERC LOAD TOOL KHANHPRO_TOOL_OPEN
ERC RELEASE BODY A4
ENDFCT
! CHUONG TRINH A5
FCT A5 ()
ERC LOAD VIEW W
PTP_AX
70.0000 61.6251 -91.6248 -56.7502
PTP_AX
13.0000 72.5001 -90.1248 -68.7501
ERC LOAD VIEW W1
PTP_AX
14.3750 56.3752 -77.7498 -62.5002
PTP_AX
14.8750 45.3752 -77.7498 -53.8752
ERC LOAD TOOL KHANHPRO_TOOL_CLOSE
ERC GRAB BODY A5
PTP_AX
14.5000 65.3751 -79.4998 -76.3752
PTP_AX
80.2499 65.3751 -79.4998 -76.3752
PTP_AX
80.2499 47.6252 -73.7499 -63.2503
PTP_AX
80.2499 38.7502 -73.7499 -57.5003
ERC LOAD TOOL KHANHPRO_TOOL_OPEN
ERC RELEASE BODY A5
ENDFCT

GVHD: CHU VN BO
89.6249 -100.5001
89.6249 -100.5001
89.6249 -100.5001
181.1249 -101.6251
181.1249 -101.6251
89.1249 -146.3753
89.1249 -85.1254
89.1249 -81.2504

175.6249 -96.2504
175.6249 -96.2504
90.1251 -86.7504
89.1251 -86.7504
186.8750 -80.6254
91.1250 -80.6254
92.0000 -80.6254

0.7500 -80.6254
-2.2500 -80.6254
91.2500 -80.3754
90.8750 -80.3754
179.6251
179.6251
90.1250
90.3750

-80.1254
-80.1254
-82.8754
-82.8754

! CHUONG TRINH A6
FCT A6 ()
ERC LOAD VIEW W
PTP_AX
80.2499 53.5002 -73.7499 -65.8753 183.7501 -89.1254
PTP_AX
28.6250 41.3752 -52.6249 -76.7503 88.2501 -104.8753
ERC LOAD VIEW W1

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

PTP_AX
28.1250 41.3752 -52.6249 -73.5003
PTP_AX
28.1250 34.1252 -52.8749 -70.5003
ERC LOAD TOOL KHANHPRO_TOOL_CLOSE
ERC GRAB BODY A6
PTP_AX
19.0000 54.5002 -65.4999 -73.2503
PTP_AX
-72.8749 50.2502 -74.2498 -63.6254
ERC LOAD VIEW W2
PTP_AX
-72.8749 50.2502 -74.2498 -63.6254
PTP_AX
-72.8749 38.7502 -74.2498 -56.6254
ERC LOAD TOOL KHANHPRO_TOOL_OPEN
ERC RELEASE BODY A6
PTP_AX
0 0 0000
ENDFCT
PTP_AX
0 0 0000
ENDPROGRAMFILE

88.2501 -67.6253
90.7500 -62.3753
180.1250 -93.3753
90.2499 -116.2504
90.2499 -77.2504
90.2499 -77.2504

2. M phng robot SCARA

SCARA l robot gm 4 khu c cu hnh kiu RRTR

Hnh 3.3 Robot SCARA v cc h ta (v tr ban u)

Khu
1
2

ai

di

a1

180

a2

d3

d4

2
0

Hnh 3.4 Bng thng s DH ca robot SCARA

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

Hnh 3.5 M hnh robot scara

Chng trnh iu khin robot SCARA
PROGRAMFILE
! prgfln E:\LE KHANH\EASY_ROB\proj\HUI_PROGRAM.prg
! CHNG TRNH TO LOGO HUI
! CHUONG TRINH TAO H1
PTP_AX
-4.5000 0.0000 0.0000
PTP_AX
-6.0000 -67.9999 0.0000
PTP_AX
-6.0000 -67.9999 0.0000
PTP_AX
-6.0000 -67.9999 0.1440
!KEP BAN TAY LAI VA NAM H1
ERC LOAD TOOL HUI_CLOSE
ERC GRAB BODY H1
PTP_AX
-6.0000 -67.9999 0.0687
PTP_AX
0.0000 -0.0000 0.0000
ERC ROBOT_BASE 0 -0.19 0 0 0 0
PTP_AX
0.0000 -0.0000 0.1440
! MO BAN TAY VA NHA H1
ERC LOAD TOOL HUI_OPEN
ERC RELEASE BODY H1
PTP_AX
0.0000 0.0000 0.0000
ERC ROBOT_BASE 0 0 0 0 0 0

0.0000
0.0000
-73.9999
-73.9999

-73.9999
-0.0000
-0.0000

0.0000

! CHUONG TRINH TAO H2

PTP_AX
5.5000 0.0000 0.0000 0.0000
PTP_AX
1.0000 -75.1248 0.0000 0.0000
PTP_AX
1.0000 -75.1248 0.0000 -74.8749
PTP_AX
1.0000 -75.1248 0.1440 -74.8749
!KEP BAN TAY LAI VA NAM H2
ERC LOAD TOOL HUI_CLOSE

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

ERC GRAB BODY H2
PTP_AX
1.0000 -75.1248 0.0714
PTP_AX
-0.0000 0.0000 0.0000
ERC ROBOT_BASE 0 -0.09 0 0 0 0
PTP_AX
-0.0000 0.0000 0.1434
! MO BAN TAY VA NHA H2
ERC LOAD TOOL HUI_OPEN
ERC RELEASE BODY H2
PTP_AX
0.0000 0.0000 0.0000
ERC ROBOT_BASE 0 0 0 0 0 0

GVHD: CHU VN BO
-74.8749
0.0000
0.0000

0.0000

! CHUONG TRINH TAO H3

PTP_AX
20.0000 -0.0000 0.0000 0.0000
PTP_AX
20.0000 50.2500 0.0000 0.0000
PTP_AX
20.0000 50.2500 0.0000 -109.8749
PTP_AX
20.0000 50.2500 0.1440 -109.8749
!KEP BAN TAY LAI VA NAM H3
ERC LOAD TOOL HUI_CLOSE
ERC GRAB BODY H3
PTP_AX
20.0000 50.2500 0.0720 -109.8749
PTP_AX
0.0000 0.0000 -0.0000 -0.0000
ERC ROBOT_BASE 0.07 -0.12 0 0 0 0
PTP_AX
0.0000 0.0000 0.0000 -90
PTP_AX
0.0000 0.0000 0.1462 -90
! MO BAN TAY VA NHA H3
ERC LOAD TOOL HUI_OPEN
ERC RELEASE BODY H3
PTP_AX
0.0000 0.0000 -0.0000 0.0000
ERC ROBOT_BASE 0 0 0 0 0 0
! CHUONG TRINH TAO U1
PTP_AX
14.2500 -86.2498 -0.0000
PTP_AX
14.3125 -86.2498 -0.0000
PTP_AX
14.3125 -86.2498 0.1442
!KEP BAN TAY LAI VA NAM U1
ERC LOAD TOOL HUI_CLOSE
ERC GRAB BODY U1
PTP_AX
14.3125 -86.2498 0.0437
PTP_AX
0.0000 -0.0000 -0.0000
ERC ROBOT_BASE 0 -0.04 0 0 0 0
PTP_AX
0.0000 -0.0000 0.1431
! MO BAN TAY VA NHA U1
ERC LOAD TOOL HUI_OPEN
ERC RELEASE BODY U1
PTP_AX
0.0000 0.0000 -0.0000
ERC ROBOT_BASE 0 0 0 0 0 0
! CHUONG TRINH TAO U2
PTP_AX
20.4800 -90.1196 0.0000
PTP_AX
20.4800 -90.1196 0.0000
PTP_AX
20.4800 -90.1196 0.1438
!KEP BAN TAY LAI VA NAM U2
ERC LOAD TOOL HUI_CLOSE
ERC GRAB BODY U2
PTP_AX
20.4800 -90.1196 0.0691
PTP_AX
0.0000 0.0000 0.0000
ERC ROBOT_BASE 0 0.06 0 0 0 0
PTP_AX
-0.0000 0.0000 0.1438

EASY ROB 2.0

0.0000
-72.5626
-72.5626

-73.1226
0.0000
0.0000

0.0000

0.0000
-69.5998
-69.5998

-69.5998
-0.0000
0.0000

NHM 9

TIU LUN ROBOT

! MO BAN TAY VA NHA U2
ERC LOAD TOOL HUI_OPEN
ERC RELEASE BODY U2
PTP_AX
0.0000 0.0000
ERC ROBOT_BASE 0 0 0 0 0 0

GVHD: CHU VN BO

0.0000

0.0000

!CHUONG TRINH TAO U3

PTP_AX
12.0800 60.6400 -0.0000 0.0000
PTP_AX
12.0800 60.6400 -0.0000 -106.9600
PTP_AX
12.0800 60.6400 0.1438 -106.9600
!KEP BAN TAY LAI VA NAM U3
ERC LOAD TOOL HUI_CLOSE
ERC GRAB BODY U3
PTP_AX
12.0800 60.6400 0.0398 -106.9600
PTP_AX
0.0000 -0.0000 -0.0000 -0.0000
ERC ROBOT_BASE 0.165 0.018 0 0 0 0
PTP_AX
0.0000 -0.0000 -0.0000 -90
PTP_AX
0.0000 -0.0000 0.1438 -90
! MO BAN TAY VA NHA U3
ERC LOAD TOOL HUI_OPEN
ERC RELEASE BODY U3
PTP_AX
0.0000 0.0000 0.0000 -0.0000
ERC ROBOT_BASE 0 0 0 0 0 0
! CHUONG TRINH TAO U4
PTP_AX
5.0000 68.8799 0.0000
PTP_AX
5.0000 68.8799 0.0000
PTP_AX
5.0000 68.8799 0.1445
!KEP BAN TAY LAI VA NAM U4
ERC LOAD TOOL HUI_CLOSE
ERC GRAB BODY U4
PTP_AX
5.0000 68.8799 0.0565
PTP_AX
-0.0000 0.0000 -0.0000
ERC ROBOT_BASE 0.1570 -0.029 0 0 0 0
PTP_AX
-0.0000 0.0000 -0.0000
PTP_AX
-0.0000 0.0000 0.1445
! MO BAN TAY VA NHA U4
ERC LOAD TOOL HUI_OPEN
ERC RELEASE BODY U4
PTP_AX
0.0000 0.0000 -0.0000
ERC ROBOT_BASE 0 0 0 0 0 0

0.0000
73.9600
73.9600

73.9600
0.0000
-36
-36

-0.0000

! CHUONG TRINH TAO U5

PTP_AX
-1.8400 76.0796 0.0000 0.0000
PTP_AX
-1.8400 76.0796 0.0000 75.3600
PTP_AX
-1.8400 76.0796 0.1438 75.3600
!KEP BAN TAY LAI VA NAM U5
ERC LOAD TOOL HUI_CLOSE
ERC GRAB BODY U5
PTP_AX
-1.8400 76.0796 0.0279 75.3600
PTP_AX
0.0000 -0.0000 0.0000 -0.0000
ERC ROBOT_BASE 0.1560 0.0478 0 0 0 0
PTP_AX
0.0000 -0.0000 0.0000 35
PTP_AX
0.0000 -0.0000 0.1438 35
! MO BAN TAY VA NHA U5
ERC LOAD TOOL HUI_OPEN
ERC RELEASE BODY U5
PTP_AX
0.0000 0.0000 -0.0000 0.0000

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

ERC ROBOT_BASE 0 0 0 0 0 0
! CHUONG TRINH TAO CHU I
PTP_AX
7.6000 -81.2001 0.0014
PTP_AX
7.6000 -81.2001 0.0014
PTP_AX
7.6000 -81.2001 0.1442
!KEP BAN TAY LAI VA NAM I
ERC LOAD TOOL HUI_CLOSE
ERC GRAB BODY I
PTP_AX
7.6000 -81.2001 0.0276
PTP_AX
-0.0000 -0.0000 -0.0000
ERC ROBOT_BASE 0 0.11 0 0 0 0
PTP_AX
-0.0000 -0.0000 0.1421
! MO BAN TAY VA NHA I
ERC LOAD TOOL HUI_OPEN
ERC RELEASE BODY I
PTP_AX
0.0000 0.0000 0.0000
ERC ROBOT_BASE 0 0 0 0 0 0

0.0000
-73.3599
-73.3599

-73.3599
-0.0000
-0.0000

0.0000

ENDPROGRAMFILE

3. M phng robot STANFORD

Stanford l robot 6 khu c cu hnh RRT.RRR( khu th ba chuyn ng tnh
tin, nm khu cn li chuyn ng quay)

Hnh3.6 H ta ca robot stanford

Khu
i
ai

EASY ROB 2.0

-90 0

90 0

d2

-90 0

d3
0

90 0

2
3

2
0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

Hinh 3.7 Bng thng s DH ca robot stanford

Hnh 3.8 M hnh robot stanford

Chng trnh iu khin robot stanford
PROGRAMFILE
! prgfln E:\LE KHANH\EASY_ROB\proj\STANFORD_PROGRAM.prg
ERC LOAD VIEW S_1
WAIT 2
ERC LOAD VIEW S_2
WAIT 2
ERC LOAD VIEW S_3
WAIT 2
ERC LOAD VIEW S_4
WAIT 2
!CHUONG TRINH GAP HANG 1
ERC LOAD VIEW S
PTP_AX
-95.6249 0.0000 0.0000 0.0000
PTP_AX
-95.1249 113.5000 0.0349 -1.5000
ERC LOAD VIEW S2
PTP_AX
-95.2499 113.5000 0.0349 -1.6250
PTP_AX
-95.8749 123.8754 0.1200 -1.6250
ERC LOAD TOOL STANFORD_TOOL_CLOSE
ERC GRAB BODY HANG1
PTP_AX
-96.2499 108.8754 -0.0087 -0.0000
PTP_AX
-21.2499 86.3755 0.4800 1.3750
ERC LOAD VIEW S1
PTP_AX
-21.1249 96.7505 0.4800 1.3750
ERC LOAD TOOL STANFORD_TOOL_OPEN
ERC RELEASE BODY HANG1

0.0000 0.0000
66.8750 47.0000
66.8750 -6.1250
58.0001 -6.1250
70.7500 -6.1250
92.6249 -21.7500
81.3750 -21.7500

!CHUONG TRINH GAP HANG 2

ERC LOAD VIEW S

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

PTP_AX
-21.1249 85.5006 0.5084 1.8750
PTP_AX
-138.0001 106.8755 0.1331 1.1250
ERC LOAD VIEW S2
PTP_AX
-138.0001 106.8755 0.1331 1.1250
ERC LOAD VIEW S
PTP_AX
-138.8752 121.6258 0.1833 2.8750
ERC LOAD TOOL STANFORD_TOOL_CLOSE
ERC GRAB BODY HANG2
PTP_AX
-139.3752 101.5008 0.0633 -1.0000
PTP_AX
-30.6252 88.0009 0.0415 1.2500
ERC LOAD VIEW S1
PTP_AX
-30.6252 88.0009 0.0415 1.2500
PTP_AX
-30.6252 100.1258 0.0415 1.2500
ERC LOAD TOOL STANFORD_TOOL_OPEN
ERC RELEASE BODY HANG2
!CHUONG TRINH GAP HANG 3
ERC LOAD VIEW S
PTP_AX
-30.6252 89.0009 0.0415 1.7500
PTP_AX
-30.6252 80.8759 -0.1788 1.7500
PTP_AX
48.9997 80.8759 -0.1788 -0.8750
ERC LOAD VIEW S3
PTP_AX
55.6247 104.2509 0.0590 -0.8750
PTP_AX
55.6247 104.2509 0.0590 -0.8750
PTP_AX
55.7497 121.2512 0.1811 -1.0000
ERC LOAD TOOL STANFORD_TOOL_CLOSE
ERC GRAB BODY HANG3
PTP_AX
55.7497 82.2513 -0.0479 -0.5000
ERC LOAD VIEW S
PTP_AX
-5.5003 84.0013 0.5193 -0.5000
ERC LOAD VIEW S1
PTP_AX
-5.2815 96.6263 0.5193 -0.5000
ERC LOAD TOOL STANFORD_TOOL_OPEN
ERC RELEASE BODY HANG3
! CHUONG TRINH GAP HANG 4
ERC LOAD VIEW S
PTP_AX
-5.2190 80.6575 0.5193 -0.7187
PTP_AX
-5.2190 80.6575 -0.0807 -0.7187
PTP_AX
94.9373 110.4075 0.1571 0.3750
ERC LOAD VIEW S3
PTP_AX
94.9373 119.5324 0.2477 0.1562
ERC LOAD TOOL STANFORD_TOOL_CLOSE
ERC GRAB BODY HANG4
ERC LOAD VIEW S
PTP_AX
95.0310 81.5324 -0.2546 0.2187
PTP_AX
-11.8440 81.5324 -0.2546 0.1875
PTP_AX
-8.9378 81.5324 0.0732 1.9062
ERC LOAD VIEW S1
PTP_AX
-8.5315 99.5324 0.0732 0.4688
ERC LOAD TOOL STANFORD_TOOL_OPEN
ERC RELEASE BODY HANG4
PTP_AX
0.0000 -0.0000 0.0000 -0.0000
WAIT 4

95.7499 -21.7500
75.1251 -114.8750
75.1251 -46.2500
55.2501 -50.0000
80.7500 -50.1250
91.0000 -74.1249
91.0000 -211.2500
76.7500 -211.2500

91.0000 -211.2500
95.6250 -209.6249
95.6250 -209.6249
77.1250 -244.4998
77.1250 -215.3750
59.5001 -217.3750
96.3749 -217.3750
96.3749 -184.7497
83.3750 -184.7497

97.3750 -184.7497
97.3750 -184.7497
72.3125 -175.1872
65.8438 -175.1872

97.8750 -175.1872
98.9687 -175.1872
98.9687 -187.7810
83.1875 -187.7810
-0.0000 -0.0000

! CHO XE CH HNG I
ERC LOAD VIEW S_XE
ERC LOAD BODY S1

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

GVHD: CHU VN BO

WAIT 2
ERC LOAD BODY S2
WAIT 2
ERC LOAD BODY S3
WAIT 2
ERC LOAD BODY S4
WAIT 2
ERC LOAD BODY S5
ERC LOAD BODY S6
WAIT 2
ENDPROGRAMFILE

EASY ROB 2.0

NHM 9

TIU LUN ROBOT

EASY ROB 2.0

GVHD: CHU VN BO

NHM 9