65 lines
1.9 KiB
XML
65 lines
1.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="start">
|
|
<Locals>
|
|
<Local name="l_bTrain" type="bool" xsi:type="array" size="1" />
|
|
<Local name="l_bContact" type="bool" xsi:type="array" size="1" />
|
|
</Locals>
|
|
<Code><![CDATA[begin
|
|
//Trining mode for the first cicle
|
|
l_bTrain=true
|
|
resetMotion(jSafe)
|
|
|
|
taskCreate "com_modbus",10,comModbus()
|
|
taskCreate "com_tcp",10,comTcp()
|
|
//taskCreate "sim_ev",10,simulateEvents(interval)
|
|
|
|
while true
|
|
movel(pForce1,flange,mNomSpeed)
|
|
waitEndMove()
|
|
|
|
//Force controling section
|
|
if (l_bTrain)
|
|
// Reset joint force limits
|
|
call resetData()
|
|
// Start training of force limits
|
|
call startTraining()
|
|
else
|
|
// Start forces monitoring
|
|
call startDetection(20)
|
|
endIf
|
|
// Activate force controlled zone
|
|
call setInZone()
|
|
|
|
|
|
movel(pForce2,flange,mNomSpeed)
|
|
waitEndMove()
|
|
|
|
|
|
if l_bTrain
|
|
call setOutZone()
|
|
else
|
|
//Detection mode, check if collision was detected
|
|
//BEFORE terminating force monitoring
|
|
call getContact(l_bContact)
|
|
//Terminate force monitoring
|
|
call setOutZone()
|
|
if l_bContact
|
|
//A collision was detected, inform user and move up
|
|
popUpMsg("Collision detected . Press Enter to proceed.")
|
|
//gotoxy(0,0)
|
|
//put("Collision detected . Press Enter to proceed.")
|
|
wait(getKey()==270)
|
|
resetMotion(jSafe)
|
|
waitEndMove()
|
|
else
|
|
//No collision detected, move up from last point
|
|
endIf
|
|
endIf
|
|
//Set to detection mode after the end of the first cycle
|
|
l_bTrain=false
|
|
|
|
endWhile
|
|
end]]></Code>
|
|
</Program>
|
|
</Programs> |