90 lines
2.9 KiB
XML
90 lines
2.9 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="calcDewarCold" access="private" >
|
|
<Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1" >
|
|
<Parameter name="segment" type="num" use="value" xsi:type="element" dimensions="1" />
|
|
<Parameter name="puck" type="num" use="value" xsi:type="element" dimensions="1" />
|
|
<Parameter name="sample" type="num" use="value" xsi:type="element" dimensions="1" />
|
|
</Parameters>
|
|
<Locals>
|
|
<Local name="nAnglePuck3" type="num" xsi:type="array" size="1" />
|
|
<Local name="nAnglePuck4" type="num" xsi:type="array" size="1" />
|
|
<Local name="nAnglePuck5" type="num" xsi:type="array" size="1" />
|
|
<Local name="nUPAngleIn" type="num" xsi:type="array" size="1" />
|
|
<Local name="nUPAngleOut" type="num" xsi:type="array" size="1" />
|
|
<Local name="nUPOffsetIn" type="num" xsi:type="array" size="1" />
|
|
<Local name="nUPOffsetOut" type="num" xsi:type="array" size="1" />
|
|
<Local name="trPuck" type="trsf" xsi:type="array" size="1" />
|
|
<Local name="trSegment" type="trsf" xsi:type="array" size="1" />
|
|
</Locals>
|
|
<Code><![CDATA[begin
|
|
|
|
|
|
setFrame(pBaseplateOri,pBaseplateX,pBaseplateY,fBaseplate)
|
|
|
|
//Add correction to fBaseplate
|
|
fCorrBaseplate.trsf=trCorrBaseplate
|
|
|
|
//Calcualte fSegment
|
|
nRadiusSeg=75
|
|
nHeightSeg=0
|
|
fSegment.trsf={nRadiusSeg*cos(-(segment-1)*(60)),nRadiusSeg*sin(-(segment-1)*(60)),nHeightSeg,0,0,(-(segment-1)*(60))}
|
|
|
|
//Add correction to fSegment
|
|
fCorrSegment.trsf=trCorrSegment[segment-1]
|
|
|
|
//calculate intermeditae point in dewar, to avoid crash with LID
|
|
trCorr.rx=trCorr.ry=trCorr.rz=0
|
|
pDewarTemp=compose(pDewarTemp,fCorrSegment,trCorr)
|
|
|
|
//Calculate fPuck
|
|
nHeightPuck=0
|
|
nPuck_s=75
|
|
switch puck
|
|
case 1
|
|
fPuck.trsf={0,0,nHeightPuck,0,0,0}
|
|
break
|
|
case 2
|
|
fPuck.trsf={nPuck_s,0,nHeightPuck,0,0,0}
|
|
break
|
|
case 3
|
|
fPuck.trsf={nPuck_s/2,-nPuck_s*sqrt(3)/2,nHeightPuck,0,0,-30}
|
|
break
|
|
case 4
|
|
fPuck.trsf={nPuck_s/2*3,-nPuck_s*sqrt(3)/2,nHeightPuck,0,0,-19.106}
|
|
break
|
|
case 5
|
|
fPuck.trsf={nPuck_s,-nPuck_s*sqrt(3),nHeightPuck,0,0,-40.893}
|
|
break
|
|
endSwitch
|
|
|
|
//Add correction to fPuck
|
|
fCorrPuck.trsf=trCorrPuck[segment-1,puck-1]
|
|
|
|
|
|
|
|
//Calculate pPin
|
|
nHeight_Sample=5
|
|
nUnipuck_Rin=12.12
|
|
nUnipuck_Rout=26.31
|
|
|
|
switch sample
|
|
//Sample calculation inner circle 1-5
|
|
case 1,2,3,4,5
|
|
pSample.trsf={nUnipuck_Rin*cos(-(sample-1)/5*360),nUnipuck_Rin*sin(-(sample-1)/5*360),nHeight_Sample,0,0,-(sample-1)/5*360-90}
|
|
break
|
|
//Sample calculation outer circle 11-16
|
|
case 6,7,8,9,10,11,12,13,14,15,16
|
|
pSample.trsf={nUnipuck_Rout*cos(-(sample-6)/11*360),nUnipuck_Rout*sin(-(sample-6)/11*360),nHeight_Sample,0,0,-(sample-6)/11*360-90}
|
|
break
|
|
endSwitch
|
|
|
|
nTemp=sample
|
|
pResult=pSample
|
|
|
|
|
|
|
|
|
|
end]]></Code>
|
|
</Program>
|
|
</Programs> |