pshell_config/bernina_robot
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added general motion command

Browse Source
This commit is contained in:
gac-bernina
2024-02-02 16:11:49 +01:00
parent d164e929ed
commit 253db3b21d
2 changed files with 43 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
script/devices/RobotBernina.py
View File

@@ -19,11 +19,11 @@ DESC_DEFAULT = DESCS[0]
FLANGE = "flange"
TOOLS = ["t_JF01T03", "t_JF07T32", ]
TOOLS = ["t_Detector",]
#TOOLS = ["t_Detector",]
TOOL_DEFAULT = TOOL_DET = TOOLS[0]
FRAMES = ["f_actualFrame", "f_direct", "f_2mRad", "f_4mRad", "f_6mRad", "f_8mRad"]
FRAMES = ["f_actualFrame", "f_Experiment1", "f_Experiment2", "f_Experiment3"]
#FRAMES = ["f_actualFrame", "f_Experiment1", "f_Experiment2", "f_Experiment3"]
FRAME_DEFAULT = FRAMES[1]
DEFAULT_ROBOT_POLLING = 200
@@ -220,7 +220,23 @@ class RobotBernina(RobotTCP):
if (any(idx1)) & (any(idx2)):
return True
return False
def general_motion(self, **kwargs):
## check motion type
self.reset_motion()
if (sum([m in kwargs.keys() for m in ROBOT_JOINT_MOTORS])>0) & (sum([m in kwargs.keys() for m in ROBOT_SPHERICAL_MOTORS])==0) & (sum([m in kwargs.keys() for m in ROBOT_CARTESIAN_MOTORS])==0):
move = self.move_joint
elif (sum([m in kwargs.keys() for m in ROBOT_CARTESIAN_MOTORS])>0) & (sum([m in kwargs.keys() for m in ROBOT_SPHERICAL_MOTORS])==0) & (sum([m in kwargs.keys() for m in ROBOT_JOINT_MOTORS])==0):
move = self.move_cartesian
elif (sum([m in kwargs.keys() for m in ROBOT_SPHERICAL_MOTORS])>0) & (sum([m in kwargs.keys() for m in ROBOT_CARTESIAN_MOTORS])==0) & (sum([m in kwargs.keys() for m in ROBOT_JOINT_MOTORS])==0):
move = self.move_spherical
else:
get_context().sendEvent("motion", "\nNo unique coordinate system found for these motions use only unique keywords out of:\n {}\n {}\n {}.".format(ROBOT_JOINT_MOTORS, ROBOT_CARTESIAN_MOTORS, ROBOT_SPHERICAL_MOTORS))
return
self.last_remote_motion = "recording"
move(sync = True, **kwargs)
return True
def record_motion(self, **kwargs):
## check motion type
self.reset_motion(msg = "RECORDING TRAJECTORY TO LIST OF ALLOWED REMOTE MOTIONS - USE HANDHELD TO FINISH MOTION")
@@ -347,6 +363,7 @@ class RobotBernina(RobotTCP):
reachable = self.eval_bool("pointToJoint(" + tool + ",tcp_j," + point + ",j)")
if verbose:
if not reachable:
get_context().sendEvent("motion", "Point is not in range")
print("Point is not in range")
return reachable
@@ -355,10 +372,10 @@ class RobotBernina(RobotTCP):
ret = None
elif self.last_remote_motion == "cartesian":
target = self.cartesian_motors["x"].target_pos
ret = move_cartesian(simulate = True, **target)
ret = self.move_cartesian(simulate = True, **target)
elif self.last_remote_motion == "spherical":
target = self.spherical_motors["r"].target_pos
ret = move_spherical(simulate = True, **target)
ret = self.move_spherical(simulate = True, **target)
elif self.last_remote_motion == "joint":
target = self.joint_motors["j1"].target_pos
ret = self.move_joint(simulate = True, **target)
@@ -408,8 +425,12 @@ class RobotBernina(RobotTCP):
else:
self.set_motion_queue_empty(False)
ret = self.movel("tcp_p_spherical[1]", tool=tool, desc=None, sync=sync)
else:
return False
if simulate:
return sim
else:
return ret
def move_spherical(self, r=None, gamma=None, delta=None, tool=None, frame=None, desc=None, sync=False, simulate=False, coordinates="joint"):
"""
@@ -655,8 +676,21 @@ def make_archivable(Motor):
".RBV": [Motor.readback, "double"],
".EGU": [MotorConfigPV(Motor, "unit"), "string"],
".ADEL": [DummySTP(0.002), "double"],
".SCAN": [DummySTP("passive"), "string"],
".PINI": [DummySTP("No"), "string"],
".PHAS": [DummySTP(0), "short"],
".EVNT": [DummySTP(""), "string"],
".PRIO": [DummySTP(0), "short"],#should be ENUM with 0,1,3 being LOW, MEDIUM, HIGH
".DISV": [DummySTP(1), "short"],
".DISA": [DummySTP(0), "short"],
".SDIS": [DummySTP(""), "string"],
".PROC": [DummySTP(0), "short"],#should be uchar
".DISS": [DummySTP(0), "short"],#should be ENUM with 0,1,2,3 being NO?ALARM, MINOR, MAJOR, INVALID
".LCNT": [DummySTP(0), "short"],#should be uchar
".PACT": [DummySTP(0), "short"],#should be uchar
}
epics_pvs = {base + field+"_": CAS((base+field).upper(), device, dtype) for field, [device, dtype] in pv_def.items()}
epics_pvs = {base + field: CAS((base+field).upper(), device, dtype) for field, [device, dtype] in pv_def.items()}
epics_pvs.update({base + ".FLNK": CAS((base+".FLNK").upper(), DummySTP((base+".RBV").upper()), "string")})
Motor.epics_pvs = epics_pvs
#robot.latency = 0.005

6
script/devices/RobotMotors.py
View File

@@ -75,7 +75,7 @@ class RobotCartesianMotor(PositionerBase):
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.name])
return self.doReadReadback()
def doWrite(self, value):
if self.robot.cartesian_destination is not None:
@@ -128,7 +128,7 @@ class RobotSphericalMotor (PositionerBase):
self.setCache(self.doRead(), None)
def doRead(self):
return float("nan") if len(self.robot.spherical_pos)==0 else robot.spherical_pos[self.name]
return self.doReadReadback()
def doWrite(self, value):
if self.robot.spherical_destination is not None:
@@ -179,7 +179,7 @@ class RobotJointMotor (PositionerBase):
self.setCache(self.doRead(), None)
def doRead(self):
return self.setpoint
return self.doReadReadback()
def doWrite(self, value):
self.setpoint = value