66 lines
1.2 KiB
Python
66 lines
1.2 KiB
Python
import plotutils
|
|
from mathutils import fit_gaussian, Gaussian
|
|
|
|
cal_tool = TOOL_CALIBRATION
|
|
|
|
robot.set_tool(cal_tool)
|
|
robot.enable()
|
|
move_to_laser()
|
|
|
|
|
|
robot.set_motors_enabled(True)
|
|
robot.set_joint_motors_enabled(True)
|
|
initial_pos = robot.get_cartesian_pos()
|
|
|
|
#robot.align()
|
|
|
|
run("calibration/ScanX")
|
|
|
|
pos1 = robot.get_cartesian_pos()
|
|
x1, l1 = closest_x, closest_y
|
|
|
|
print "Closest 1: ", [x1, l1]
|
|
print "Position 1: ", pos1
|
|
|
|
|
|
pj6 = robot_j6.position
|
|
if pj6>0:
|
|
robot_j6.move(pj6 - 180.0)
|
|
else:
|
|
robot_j6.move(pj6 + 180.0)
|
|
|
|
|
|
run("calibration/ScanX")
|
|
|
|
|
|
x2, l2 = closest_x, closest_y
|
|
pos2 = robot.get_cartesian_pos()
|
|
|
|
print "Closest 2: ", [x2, l2]
|
|
print "Position 2: ", pos2
|
|
|
|
|
|
#Updates the tool
|
|
t=robot.get_tool_trsf(TOOL_DEFAULT)
|
|
|
|
xoff = (x1-x2)/2
|
|
yoff = (l2 - l1)/2
|
|
xrot = math.degrees(math.atan(yoff/t[2]))
|
|
yrot = math.degrees(math.atan(xoff/t[2]))
|
|
|
|
print "Former tool: " + str(t)
|
|
t[0]=xoff
|
|
t[1]=-yoff
|
|
print "Offset: ", [t[0], t[1]]
|
|
|
|
print "CALIBRATED tool: " + str(t)
|
|
#robot.set_tool_trsf(t, TOOL_DEFAULT)
|
|
|
|
robot.set_tool(TOOL_DEFAULT)
|
|
d = robot.get_distance_to_pnt("pLaser")
|
|
if d<POSITION_TOLERANCE:
|
|
print "Moving calibrated tool to laser"
|
|
#move_to_laser()
|
|
else:
|
|
print "Cannot move calibrated tool to laser: too big offset"
|