209 lines
8.3 KiB
Python
209 lines
8.3 KiB
Python
import time
|
|
import ch.psi.pshell.device.PositionerConfig as PositionerConfig
|
|
|
|
|
|
def on_motion_events(Motor):
|
|
def on_combined_motion(self, changed_motors):
|
|
s = "Combining " + str(len(changed_motors)) + " motions to new target:\n"
|
|
for k, v in changed_motors.items():
|
|
s += "{:8} {:6} --> {}\n".format(k,v[0], v[1])
|
|
get_context().sendEvent("motion", s)
|
|
|
|
def on_coordinate_system_change(self, old, new):
|
|
s = "User changed coordinate system " + old + " --> " +new + ":\n"
|
|
get_context().sendEvent("motion", s)
|
|
|
|
Motor.on_combined_motion = on_combined_motion
|
|
Motor.on_coordinate_system_change = on_coordinate_system_change
|
|
return Motor
|
|
|
|
|
|
|
|
|
|
|
|
class LinearAxisMotor(PositionerBase):
|
|
def __init__(self, robot, name = "z_lin"):
|
|
PositionerBase.__init__(self, name, PositionerConfig())
|
|
self.robot = robot
|
|
|
|
@property
|
|
def target_coordinates(self):
|
|
return {k: m.setpoint for k,m in robot.linear_axis_motors.items()}
|
|
|
|
#ATTENTION: Always initialize linear axis motors before scanning (or call robot.set_lin_axis_motor_enabled(True))
|
|
def doInitialize(self):
|
|
self.setpoint = self.doReadReadback()
|
|
self.setCache(self.doRead(), None)
|
|
|
|
def doRead(self):
|
|
return float("nan") if self.robot.linear_axis_destination is None else float(self.robot.linear_axis_destination[self.name])
|
|
|
|
def doWrite(self, value):
|
|
if self.robot.linear_axis_destination is not None:
|
|
self.setpoint = value
|
|
self.robot.linear_axis_destination.update({self.name: value})
|
|
self.setCache(float(value), None)
|
|
robot.evaluate("n_moveAbsPos = " + str(value))
|
|
robot.evaluate("setEcAo(ecao_posSet,n_moveAbsPos)")
|
|
|
|
def doReadReadback(self):
|
|
return float("nan") if len(self.robot.linear_axis_pos)==0 else float(self.robot.linear_axis_pos[self.name])
|
|
|
|
def stop(self):
|
|
robot.evaluate("setEcDo(ecdo_linMotStop,true)")
|
|
time.sleep(0.1)
|
|
robot.evaluate("setEcDo(ecdo_linMotStop,false)")
|
|
|
|
@on_motion_events
|
|
class RobotCartesianMotor(PositionerBase):
|
|
def __init__(self, robot, name):
|
|
PositionerBase.__init__(self, name, PositionerConfig())
|
|
self.robot = robot
|
|
|
|
|
|
@property
|
|
def target_pos(self):
|
|
return {k: m.setpoint for k,m in robot.cartesian_motors.items()}
|
|
@target_pos.setter
|
|
def target_pos(self, value):
|
|
for k,m in robot.cartesian_motors.items():
|
|
m.setpoint = value[k] if k in value.keys() else robot.cartesian_pos[k]
|
|
|
|
#ATTENTION: Always initialize cartesian motors before scanning (or call robot.set_motors_enabled(True))
|
|
def doInitialize(self):
|
|
self.setpoint = self.doReadReadback()
|
|
self.setCache(self.doRead(), None)
|
|
|
|
def doRead(self):
|
|
return self.doReadReadback()
|
|
|
|
def doWrite(self, value):
|
|
if self.robot.cartesian_destination is not None:
|
|
self.setpoint = value
|
|
self.setCache(float(value), None)
|
|
changed_motors = {k: [self.robot.cartesian_pos[k], self.target_pos[k]] for k in self.target_pos.keys() if round(self.robot.cartesian_pos[k], 2) != round(self.target_pos[k], 2)}
|
|
if (len(changed_motors) > 1) & (self.robot.last_remote_motion == "cartesian"):
|
|
self.robot.evaluate("resetMotion()")
|
|
target = self.target_pos
|
|
print("moving to combined", target)
|
|
self.on_combined_motion(changed_motors)
|
|
else:
|
|
if self.robot.last_remote_motion != "cartesian":
|
|
if not self.robot.empty:
|
|
self.robot.evaluate("resetMotion()")
|
|
self.on_coordinate_system_change(self.robot.last_remote_motion, "cartesian")
|
|
target = {self.name:value}
|
|
self.target_pos = target
|
|
print("moving to", target)
|
|
robot.move_cartesian(**target)
|
|
self.robot.last_remote_motion = "cartesian"
|
|
|
|
def doReadReadback(self):
|
|
return float("nan") if len(self.robot.cartesian_pos)==0 else float(self.robot.cartesian_pos[self.name])
|
|
|
|
def stop(self):
|
|
robot.stop()
|
|
robot.reset_motion()
|
|
robot.resume()
|
|
|
|
def move_done(self):
|
|
"""once move done, do readback update and send event and readback"""
|
|
|
|
@on_motion_events
|
|
class RobotSphericalMotor (PositionerBase):
|
|
def __init__(self, robot, name):
|
|
PositionerBase.__init__(self, name, PositionerConfig())
|
|
self.robot = robot
|
|
|
|
@property
|
|
def target_pos(self):
|
|
return {k: m.setpoint for k,m in robot.spherical_motors.items()}
|
|
@target_pos.setter
|
|
def target_pos(self, value):
|
|
for k,m in robot.spherical_motors.items():
|
|
m.setpoint = value[k] if k in value.keys() else robot.spherical_pos[k]
|
|
|
|
def doInitialize(self):
|
|
self.setpoint = self.doReadReadback()
|
|
self.setCache(self.doRead(), None)
|
|
|
|
def doRead(self):
|
|
return self.doReadReadback()
|
|
|
|
def doWrite(self, value):
|
|
if self.robot.spherical_destination is not None:
|
|
self.setpoint = value
|
|
self.setCache(float(value), None)
|
|
changed_motors = {k: [self.robot.spherical_pos[k], self.target_pos[k]] for k in self.target_pos.keys() if round(self.robot.spherical_pos[k], 2) != round(self.target_pos[k], 2)}
|
|
if (len(changed_motors) > 1) & (self.robot.last_remote_motion == "spherical"):
|
|
self.robot.evaluate("resetMotion()")
|
|
target = self.target_pos
|
|
print("moving to combined", target)
|
|
self.on_combined_motion(changed_motors)
|
|
else:
|
|
if self.robot.last_remote_motion != "spherical":
|
|
if not self.robot.empty:
|
|
self.robot.evaluate("resetMotion()")
|
|
self.on_coordinate_system_change(self.robot.last_remote_motion, "spherical")
|
|
target = {self.name:value}
|
|
self.target_pos = target
|
|
print("moving to", target)
|
|
robot.move_spherical(**target)
|
|
self.robot.last_remote_motion = "spherical"
|
|
|
|
def doReadReadback(self):
|
|
return float("nan") if len(self.robot.spherical_pos)==0 else robot.spherical_pos[self.name]
|
|
|
|
def stop(self):
|
|
robot.stop()
|
|
robot.reset_motion()
|
|
robot.resume()
|
|
|
|
@on_motion_events
|
|
class RobotJointMotor (PositionerBase):
|
|
def __init__(self, robot, name):
|
|
PositionerBase.__init__(self, name, PositionerConfig())
|
|
self.robot = robot
|
|
|
|
@property
|
|
def target_pos(self):
|
|
return {k: m.setpoint for k,m in robot.joint_motors.items()}
|
|
@target_pos.setter
|
|
def target_pos(self, value):
|
|
print(value)
|
|
for k,m in robot.joint_motors.items():
|
|
m.setpoint = value[k] if k in value.keys() else robot.joint_pos[k]
|
|
|
|
def doInitialize(self):
|
|
self.setpoint = self.doReadReadback()
|
|
self.setCache(self.doRead(), None)
|
|
|
|
def doRead(self):
|
|
return self.doReadReadback()
|
|
|
|
def doWrite(self, value):
|
|
self.setpoint = value
|
|
changed_motors = {k: [self.robot.joint_pos[k], self.target_pos[k]] for k in self.target_pos.keys() if round(self.robot.joint_pos[k], 2) != round(self.target_pos[k], 2)}
|
|
if (len(changed_motors) > 1) & (self.robot.last_remote_motion == "joint"):
|
|
self.robot.evaluate("resetMotion()")
|
|
target = self.target_pos
|
|
print("moving to combined", target)
|
|
self.on_combined_motion(changed_motors)
|
|
else:
|
|
if self.robot.last_remote_motion != "joint":
|
|
if not self.robot.empty:
|
|
self.robot.evaluate("resetMotion()")
|
|
self.on_coordinate_system_change(self.robot.last_remote_motion, "joint")
|
|
target = {self.name:value}
|
|
self.target_pos = target
|
|
print("moving to", target)
|
|
self.robot.move_joint(**target)
|
|
self.robot.last_remote_motion = "joint"
|
|
|
|
def doReadReadback(self):
|
|
return self.robot.get_joint_pos()[self.name] if len(self.robot.joint_pos)==0 else float(self.robot.joint_pos[self.name])
|
|
|
|
def stop(self):
|
|
robot.stop()
|
|
robot.reset_motion()
|
|
robot.resume() |