78 lines
3.1 KiB
XML
78 lines
3.1 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="detect">
|
|
<Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
|
|
<Parameter name="x_nTolPercent" type="num" xsi:type="element" />
|
|
</Parameters>
|
|
<Locals>
|
|
<Local name="l_nIndex" type="num" xsi:type="array" size="1" />
|
|
<Local name="l_bCondition" type="bool" xsi:type="array" size="1" />
|
|
<Local name="l_sDisplay" type="string" xsi:type="array" size="1" />
|
|
<Local name="x_nThreshold" type="num" xsi:type="array" size="1" />
|
|
<Local name="l_nPerc" type="num" xsi:type="array" size="1" />
|
|
</Locals>
|
|
<Code><![CDATA[begin
|
|
//-----------------------------------------------------------------------
|
|
//Header begin
|
|
//Description of program
|
|
// Reads the current joint force values to compare them to the trained ones (a tolerance can be added).
|
|
// If the force on a joint becomes greater than the trained value for this joint +
|
|
// the tolerance, the robot is stopped and the motion stack is emptied.
|
|
//
|
|
//Input paramaters
|
|
// num x_nTolPercent :
|
|
// role : tolerance percentage to apply to the trained values.
|
|
//
|
|
//History
|
|
// Creation : 23-04-08 - f.Rob
|
|
// Modification: 11-03-09 - ffab
|
|
//Header end
|
|
//-----------------------------------------------------------------------
|
|
// Wait to be in the zone
|
|
wait(bInZone)
|
|
|
|
cls()
|
|
gotoxy(0,7)
|
|
put("MaxForc Thresld Measur % %max")
|
|
|
|
// Detect contact while robot into the zone
|
|
while bInZone
|
|
//Record the joint force (comment out next line for use with expansion add-on)
|
|
getJointForce(nJntForce)
|
|
//Uncomment next line for use with expansion add-on
|
|
//$getJntForce(nJntForce)
|
|
// Compare with the trained values
|
|
for l_nIndex=0 to 5
|
|
x_nThreshold = abs(nJntMaxForce[l_nIndex])+abs(x_nTolPercent*nJntMaxForce[l_nIndex]/100)
|
|
if nJntMaxForce[l_nIndex]>0
|
|
l_nPerc = abs(nJntForce[l_nIndex]/nJntMaxForce[l_nIndex] *100)
|
|
if l_nPerc>nJntMaxMeasured[l_nIndex]
|
|
nJntMaxMeasured[l_nIndex]=l_nPerc
|
|
endIf
|
|
l_bCondition=abs(nJntForce[l_nIndex]) > x_nThreshold
|
|
|
|
gotoxy(0,8+l_nIndex)
|
|
put(toString(".2",nJntMaxForce[l_nIndex]))
|
|
gotoxy(8,8+l_nIndex)
|
|
put(toString(".2",x_nThreshold))
|
|
gotoxy(16,8+l_nIndex)
|
|
put(toString(".2",abs(nJntForce[l_nIndex])))
|
|
gotoxy(24,8+l_nIndex)
|
|
put(toString(".2",l_nPerc))
|
|
gotoxy(32,8+l_nIndex)
|
|
put(toString(".2",nJntMaxMeasured[l_nIndex]))
|
|
|
|
if l_bCondition
|
|
stopMove()
|
|
resetMotion()
|
|
bContact=true
|
|
l_sDisplay="Contact: force on joint "+toString("",l_nIndex+1)+" = "+toString("",abs(nJntForce[l_nIndex]))
|
|
logMsg(l_sDisplay)
|
|
endIf
|
|
endIf
|
|
endFor
|
|
delay(0)
|
|
endWhile
|
|
end]]></Code>
|
|
</Program>
|
|
</Programs> |