61 lines
1.1 KiB
Python
61 lines
1.1 KiB
Python
import plotutils
|
|
from mathutils import fit_gaussian, Gaussian
|
|
|
|
|
|
robot.assert_tool(TOOL_CALIBRATION)
|
|
robot.set_motors_enabled(True)
|
|
robot.set_joint_motors_enabled(True)
|
|
|
|
|
|
initial_pos = robot.get_cartesian_pos()
|
|
|
|
robot.enable()
|
|
move_to_laser()
|
|
|
|
robot.align()
|
|
|
|
|
|
run("calibration/ScanYZ")
|
|
|
|
robot.set_motors_enabled(True)
|
|
|
|
|
|
first_y = robot_y.take()
|
|
first_z = robot_z.take()
|
|
first_y = ue.take()
|
|
first_j6 = robot_j6.take()
|
|
if first_y is None:
|
|
raise Exception("Invalid YZ scan values in first scan")
|
|
|
|
|
|
robot.set_joint_motors_enabled(True)
|
|
if first_j6>0:
|
|
robot_j6.moveRel(-180.0, -1)
|
|
else:
|
|
robot_j6.moveRel(180.0, -1)
|
|
|
|
|
|
robot.set_motors_enabled(True)
|
|
run("calibration/ScanYZ")
|
|
|
|
robot.set_motors_enabled(True)
|
|
|
|
second_y = robot_y.take()
|
|
second_z = robot_z.take()
|
|
second_y = ue.take()
|
|
second_j6 = robot_j6.take()
|
|
if second_y is None:
|
|
raise Exception("Invalid XZ scan values in first scan")
|
|
|
|
|
|
#Updates the tool
|
|
xoff = (first_x - second_x)/2
|
|
yoff = (first_y - second_y)/2
|
|
t=robot.get_tool_trsf(TOOL_DEFAULT)
|
|
t[0]=xoff
|
|
t[1]=-yoff
|
|
robot.set_tool_trsf(t, TOOL_DEFAULT)
|
|
|
|
|
|
|