62 lines
2.3 KiB
Python
62 lines
2.3 KiB
Python
|
|
import ch.psi.pshell.device.PositionerConfig as PositionerConfig
|
|
|
|
|
|
|
|
ROBOT_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"]
|
|
|
|
|
|
class RobotCartesianMotor (PositionerBase):
|
|
def __init__(self, robot, index):
|
|
PositionerBase.__init__(self, robot.name + "_" + ROBOT_MOTORS[index], PositionerConfig())
|
|
self.index = index
|
|
self.robot = robot
|
|
|
|
#ATTENTION: Always initialize cartesian motors before scanning (or call robot.set_motors_enabled(True))
|
|
def doInitialize(self):
|
|
self.setCache(self.doRead(), None)
|
|
|
|
def doRead(self):
|
|
return float("nan") if self.robot.cartesian_destination is None else float(self.robot.cartesian_destination[self.index])
|
|
|
|
def doWrite(self, value):
|
|
if self.robot.cartesian_destination is not None:
|
|
#print "Move " + ROBOT_MOTORS[self.index] + " to " + str(value)
|
|
self.robot.cartesian_destination[self.index] = float(value)
|
|
self.robot.set_pnt(robot.cartesian_destination , "tcp_p")
|
|
self.robot.movel("tcp_p", self.robot.tool , DESC_SCAN)
|
|
|
|
def doReadReadback(self):
|
|
return float("nan") if self.robot.cartesian_pos is None else float(self.robot.cartesian_pos[self.index])
|
|
|
|
|
|
|
|
|
|
ROBOT_JOINT_MOTORS = ["j1" , "j2", "j3", "j4", "j5", "j6"]
|
|
|
|
|
|
class RobotJointMotor (PositionerBase):
|
|
def __init__(self, robot, index):
|
|
PositionerBase.__init__(self, robot.name + "_" + ROBOT_JOINT_MOTORS[index], PositionerConfig())
|
|
self.index = index
|
|
self.robot = robot
|
|
|
|
def doInitialize(self):
|
|
self.setpoint = self.doReadReadback()
|
|
self.setCache(self.doRead(), None)
|
|
|
|
def doRead(self):
|
|
return self.setpoint
|
|
|
|
def doWrite(self, value):
|
|
#print "Move " + ROBOT_JOINT_MOTORS[self.index] + " to " + str(value)
|
|
self.setpoint = value
|
|
joint = self.robot.herej()
|
|
joint[self.index] = value
|
|
self.robot.set_jnt(joint, name="tcp_j")
|
|
self.robot.movej("tcp_j", self.robot.tool , DESC_SCAN)
|
|
|
|
def doReadReadback(self):
|
|
return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.index])
|
|
|
|
|