script/motion/tools.py

@@ -0,0 +1,60 @@
+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)
+        wait_end_of_move()
+        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)
+        wait_end_of_move()
+        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)    
+        wait_end_of_move()
+        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()
+    
+
+