58 lines
1.7 KiB
Python
58 lines
1.7 KiB
Python
POSITION_TOLERANCE = 50
|
|
|
|
def wait_end_of_move():
|
|
robot.update()
|
|
while (not robot.settled) or (not robot.empty) or (not robot.isReady()) :
|
|
time.sleep(0.01)
|
|
|
|
|
|
def move_home():
|
|
#robot.reset_motion("jHome")
|
|
robot.movej("pHome", robot.tool , DESC_SCAN)
|
|
wait_end_of_move()
|
|
|
|
def move_to_laser():
|
|
tool = robot.tool
|
|
d = robot.get_distance_to_pnt("pLaser")
|
|
if d<0:
|
|
raise Exception ("Error calculating distance to laser: " + str(d))
|
|
if d<POSITION_TOLERANCE:
|
|
print "FROM LASER"
|
|
robot.reset_motion()
|
|
robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
|
|
return
|
|
d = robot.get_distance_to_pnt("pLaserAppro")
|
|
if d<0:
|
|
raise Exception ("Error calculating distance to laser appro: " + str(d))
|
|
if d<POSITION_TOLERANCE:
|
|
print "FROM APPRO"
|
|
robot.reset_motion()
|
|
robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
|
|
return
|
|
d = robot.get_distance_to_pnt("pHome")
|
|
if d<0:
|
|
raise Exception ("Error calculating distance to home: " + str(d))
|
|
if d<POSITION_TOLERANCE:
|
|
print "FROM HOME"
|
|
robot.reset_motion()
|
|
robot.movel("pLaserAppro", robot.tool, DESC_DEFAULT)
|
|
robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
|
|
return
|
|
raise Exception ("Must be in home position to start move to laser")
|
|
|
|
|
|
|
|
|
|
def update_tool(tool=None, x_offset=0.0, y_offset=0.0, z_offset=0.0):
|
|
#Updating tool:
|
|
t=robot.get_tool_trsf(tool)
|
|
t[0]=t[0] - x_offset
|
|
t[1]=t[1] - y_offset
|
|
t[2]=t[2] - z_offset
|
|
robot.set_tool_trsf(t, tool)
|
|
print "Updated " + (str(robot.tool) if (tool is None) else tool) + ": " + str(t)
|
|
robot.save_program()
|
|
|
|
|
|
|