Dual Rotary Axis Calibration Script
The calibration of dual rotary axes, where axis2 is mounted to axis1, is performed the same way as a single axis. The first axis (axis1) must remain stationary while axis2 is sampled and calibrated. This basic script will prompt you to freedrive to the target point at different positions, sample the point, then run the calibration function (calibrate_rotary_axis) and store the results to global variables CAL_AX1_ROTARY and CAL_AX2_ROTARY.
# Dual Axis Configuration
# NOTE: You must first create the following global variables in the installation tab under "General"
# CAL_AX1_ROTARY p[0,0,0,0,0,0]
# CAL_AX2_ROTARY p[0 ,0,0,0,0,0]
# This will be used to store the calibrated poses calculated here
PI = acos(-1)
AXIS_TYPE1 = 0 # 0:rotary, 1:linear
VELOCITY_LIMIT1 = 0.7*2 # rad/s
ACCELERATION_LIMIT1 = 0.1*100 # rad/s^2
ENCODER_RESOLUTION1 = <ENCODER1>
FEED_CONSTANT1 = 2 * PI # make sure +Z points out of motor
GEAR_RATIO1 = <GEAR1>
ZERO_OFFSET1 = 0
AXIS_TYPE2 = 0 # 0:rotary, 1:linear
VELOCITY_LIMIT2 = 0.7*2 # rad/s
ACCELERATION_LIMIT2 = 0.1*100 # rad/s^2
ENCODER_RESOLUTION2 = <ENCODER2>
FEED_CONSTANT2 = 2 * PI # make sure +Z points out of motor
GEAR_RATIO2 = <GEAR2>
ZERO_OFFSET2 = 0
def run_dualaxis_config():
# stop EtherCAT master first, otherwise, reset_world_model may throw an error
ethercat_clear_error()
ethercat_stop(True)
reset_world_model()
axis_group_add("positioner", p[0,0,0,0,0,0], "base")
# after calibration, change these to use CAL_AX1_ROTARY and CAL_AX2_ROTARY
axis1_position = p[0,0,0,0,0,0]
axis2_position = p[0,0,0,0,0,0]
#axis1_position = CAL_AX1_ROTARY
#axis2_position = CAL_AX2_ROTARY
# configured as independent at first
axis_group_add_axis("positioner", "axis1", "", axis1_position, AXIS_TYPE1, VELOCITY_LIMIT1, ACCELERATION_LIMIT1)
axis_group_add_axis("positioner", "axis2", "axis1", axis2_position, AXIS_TYPE2, VELOCITY_LIMIT2, ACCELERATION_LIMIT2)
ethercat_config_axis("axis1", 1, ENCODER_RESOLUTION1, GEAR_RATIO1, FEED_CONSTANT1, ZERO_OFFSET1)
ethercat_config_axis("axis2", 2, ENCODER_RESOLUTION2, GEAR_RATIO2, FEED_CONSTANT2, ZERO_OFFSET2)
ethercat_set_parameter("dc_enable", True)
ethercat_start(10)
if (ethercat_is_axis_in_fault_state("axis1")):
textmsg("FAULT in: ", "axis1")
if(ethercat_reset_axis_fault("axis1")):
textmsg("Axis 1 FAULT cleared")
else:
return False
end
end
ethercat_enable_axis("axis1")
if (ethercat_is_axis_in_fault_state("axis2")):
textmsg("FAULT in: ", "axis2")
if(ethercat_reset_axis_fault("axis2")):
textmsg("Axis2 FAULT cleared")
else:
return False
end
end
ethercat_enable_axis("axis2")
return True
end
# Dual Rotary Axis Calibration
def move_axes(group_name, axis1_position, axis2_position):
msg1 = group_name
msg1 = str_cat(msg1, " will now move to [")
msg1 = str_cat(msg1, to_str(axis1_position))
msg1 = str_cat(msg1, ", ")
msg1 = str_cat(msg1, to_str(axis2_position))
msg1 = str_cat(msg1, "] degrees")
popup(msg1, "Axes are about to move!", warning=True, error=False, blocking=True)
ethercat_enable_axis("axis1")
ethercat_enable_axis("axis2")
axis_group_movej(group_name, [d2r(axis1_position), d2r(axis2_position)], 1, 1)
ethercat_disable_axis("axis2")
ethercat_disable_axis("axis1")
end
def teachCalPointAxis(position1, position2, ref_frame):
move_axes("positioner", position1, position2)
freedrive_mode()
msg = "Freedrive to the calibration point, then click Continue"
popup(msg, "Axis Calibration", blocking=True)
p = get_pose("tcp", ref_frame)
end_freedrive_mode()
return p
end
#-----------------------------------------------------------
# Provide a function to configure your base ethercat setup
#-----------------------------------------------------------
if not run_dualaxis_config():
popup("dual configuration failed to run", title="Axis Calibration", warning=False, error=True, blocking=True)
halt
end
# Teach the pose of axis1 relatitve to the robot base.
# The robot base should not move
popup("Ready to Calibrate Axis1?", title="Axis Calibration", warning=False, error=False, blocking=True)
p1 = teachCalPointAxis( 0.0, 0.0, "base")
p2 = teachCalPointAxis( 90.0, 0.0, "base")
p3 = teachCalPointAxis(180.0, 0.0, "base")
p4 = teachCalPointAxis(270.0, 0.0, "base")
move_axes("positioner", 0, 0)
CAL_AX1_ROTARY = calibrate_rotary_axis([p1, p2, p3, p4],[])[0]
popup("Axis1 pose saved in the installation variable CAL_AX1_ROTARY", "Calibration")
axis_group_update_axis("axis1", CAL_AX1_ROTARY)
# Teach the pose of axis2 relatitve to axis1
# Axis1 should not move
popup("Ready to Calibrate Axis2?", title="Axis Calibration", warning=False, error=False, blocking=True)
p1 = teachCalPointAxis(0.0, 0.0, "axis1")
p2 = teachCalPointAxis(0.0, 90.0, "axis1")
p3 = teachCalPointAxis(0.0,180.0, "axis1")
p4 = teachCalPointAxis(0.0,270.0, "axis1")
move_axes("positioner", 0, 0)
CAL_AX2_ROTARY = calibrate_rotary_axis([p1, p2, p3, p4],[])[0]
popup("Axis2 pose saved in the installation variable CAL_AX2_ROTARY", "Calibration")
axis_group_update_axis("axis2", CAL_AX2_ROTARY)
popup("Completed Dual Axis Calibration", "Calibration")