66 lines
2.0 KiB
XML
66 lines
2.0 KiB
XML
<?xml version="1.0" encoding="utf-8" ?>
|
|
<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2" >
|
|
<Program name="robotRecover" access="private" >
|
|
<Code><![CDATA[begin
|
|
|
|
tcp_ret=-100
|
|
|
|
phere=here(tSuna,world)
|
|
jhere=herej()
|
|
|
|
//Robot is in Dewar
|
|
if (270<phere.trsf.x) and (phere.trsf.x<710) and (60<phere.trsf.y) and (phere.trsf.y<500) and (-500<phere.trsf.z) and (phere.trsf.z<-390)
|
|
movel(appro(phere,{0,0,30,0,0,0}),tSuna,mSlow)
|
|
movel(pDewarWait,tSuna,mSlow)
|
|
movel(pDewarHome,tSuna,mSlow)
|
|
waitEndMove()
|
|
tcp_ret=1
|
|
|
|
//Robot is on near dewar Lid surface
|
|
elseIf (270<phere.trsf.x) and (phere.trsf.x<710) and (60<phere.trsf.y) and (phere.trsf.y<500) and (-390<phere.trsf.z) and (phere.trsf.z<-270)
|
|
movel(appro(phere,{0,0,100,0,0,0}),tSuna,mSlow)
|
|
movel(pDewarHome,tSuna,mSlow)
|
|
waitEndMove()
|
|
tcp_ret=2
|
|
|
|
//Robot is over dewar surface
|
|
elseIf (270<phere.trsf.x) and (phere.trsf.x<710) and (60<phere.trsf.y) and (phere.trsf.y<500) and (-270<phere.trsf.z) and (phere.trsf.z<-70)
|
|
movel(pDewarHome,tSuna,mSlow)
|
|
waitEndMove()
|
|
tcp_ret=3
|
|
|
|
//Robot is near scanner
|
|
elseIf (175<phere.trsf.x) and (phere.trsf.x<300) and (-140<phere.trsf.y) and (phere.trsf.y<80)
|
|
movel(pScanHome,tSuna,mSlow)
|
|
waitEndMove()
|
|
tcp_ret=4
|
|
|
|
//Robot is in Helium Chamber
|
|
elseIf (-360<phere.trsf.x) and (phere.trsf.x<-340) and (100<phere.trsf.y) and (phere.trsf.y<120)
|
|
movel(pGonioHome,tSuna,mSlow)
|
|
waitEndMove()
|
|
tcp_ret=5
|
|
|
|
//Robot is near Heater
|
|
elseIf (-90<phere.trsf.x) and (phere.trsf.x<-30) and (240<phere.trsf.y) and (phere.trsf.y<300)
|
|
movel(pHeaterHome,tSuna,mSlow)
|
|
waitEndMove()
|
|
tcp_ret=6
|
|
|
|
//Robot is near Park
|
|
elseIf (20<phere.trsf.x) and (phere.trsf.x<90) and (390<phere.trsf.y) and (phere.trsf.y<450)
|
|
movel(pPark,tSuna,mSlow)
|
|
waitEndMove()
|
|
tcp_ret=7
|
|
|
|
else
|
|
gotoxy(0,14)
|
|
putln("Robot needs to be recovered manually")
|
|
tcp_ret=-1
|
|
|
|
endIf
|
|
|
|
tcp_ret=1
|
|
end]]></Code>
|
|
</Program>
|
|
</Programs> |