I think you should try 800mm above the pBagPos1 and then start searching on the way down.
movel(pHome,tPartTool,mTravelFast)
movej(appro(pPickPartFromTable,trAbovePickPart[0]),tPartTool,mTravelFast[2])
movel(pPickPartFromTable,tPartTool,mPickFast[1])
waitEndMove()
//
// Turn on vacuum
oPickPartVacON=true
//
setMoveId(10100)
if (bLastPosPickedPart==true)
movel(pLastPickedPart,tPartTool,mPickPart)
endIf
waitEndMove()
do
// Move down 1mm until find plate or get minimum height
if l_bPickFailed==true
if (l_nNumberOfFails<2)
oPickPartVacON=true
endIf
movel(LP_pLastPickPOS,tPartTool,mPickPart)
waitEndMove()
else
while (iPickPartVacON==false and iPartInGripper==false)
link(LP_pLastPickPOS,fTtable)
LP_pLastPickPOS=here(tPartTool,fTtable)
LP_pLastPickPOS.trsf.z=LP_pLastPickPOS.trsf.z-0.5
// Check if Z limit *TO DO change z minimum height, table height!
if LP_pLastPickPOS.trsf.z<-415
// *TO DO, what to do when there is no parts
// replace line below with recovery code
wait(true)
else
movel(LP_pLastPickPOS,tPartTool,mPickPart)
endIf
waitEndMove()
delay(0)
endWhile
endIf
//Save the position of last picked plate and set the flag
pLastPickedPart=here(tPartTool,world)
bLastPosPickedPart=true
// Press down to compress vacuum cup
LP_pLastPickPOS=here(tPartTool,fTtable)
LP_pLastPickPOS.trsf.z=LP_pLastPickPOS.trsf.z-0.49
movel(LP_pLastPickPOS,tPartTool,mPickPart)
waitEndMove()
// Wait for vaccum ON signal
l_bPartVacOk=watch(iPickPartVacON==true,3)
l_bPickFailed=false
//
movel(appro(pPickPartFromTable,trAbovePickPart[1]),tPartTool,mTravelSlow[0])
waitEndMove()
if (!iPickPartVacON)
// Vacuum fail
popUpMsg("Plate vac fail",2)
// *TO DO, what should the robot do if it fails?
l_bPickFailed=true
oPickPartVacON=false
l_nNumberOfFails=l_nNumberOfFails+1
//Do two attempts to pick the part from the jig. Send an error when failed.
if (l_nNumberOfFails>=2)
do_REQ_Robot_Error=true
wait(di_ACK_Robot_Error==true)
do_REQ_Robot_Error=false
stopMove()
taskSuspend("MainTask")
endIf
endIf
//
until l_bPickFailed==false
Display More
I use this snippet for many jobs when I use vacuum generator feedback signal to detect the part.
As for the line 80: trBag is a transformation ARRAY containing 6 values. In your calculations you ask robot to deduct from single value of aiDistSensor an array of six values. You should point to the 3rd element of trBag[3]
Does Stäubli has some thing like: ShiftFrame.Z like Kuka? Does it have some like getPosition so i can minus that z value with my distance sensor value? What would be the right way to do this?
VAL3 is more powerful and modern than KRL even thou it is older language than KRL. It uses command HERE to save the current position. You can use l_pCurrentPOS=here(tTool,fFrame) to save the current position and than change it by pointing to the element you want to change l_pCuurentPOS.z=50.0 or you can approach the point by using transformation:
movej(appro(l_pCurrentPOS,trApproachArray[0]),tPart,mTravelFast[2])
movel(appro(l_pCurrentPOS,trApproachArray[1]),tPart,mTravelFast[0])
movej(appro(l_pCurrentPOS,trApproachArray[2]),tPart,mTravelFast[1])
movej(appro(l_pCurrentPOS,trApproachArray[3]),tPart,mTravelFast[5])
movel(pPickPartFromTable,tPartTool,mTravelFast[3])