diff --git a/config/config.properties b/config/config.properties index ab936ff..3a6da75 100644 --- a/config/config.properties +++ b/config/config.properties @@ -1,4 +1,4 @@ -#Tue Aug 27 08:44:59 CEST 2024 +#Thu Jan 16 15:06:14 CET 2025 xscanMoveTimeout=600 autoSaveScanData=true simulation=false @@ -39,6 +39,7 @@ filePermissionsLogs=Public logPath={logs}/{date}_{time} dataLayout=default disableDataFileLogs=false +dataScanSaveTimestamps=false sessionHandling=Off terminalEnabled=true filePermissionsScripts=Public diff --git a/script/devices/RobotBernina.py b/script/devices/RobotBernina.py index 750bbdb..c2ce90a 100644 --- a/script/devices/RobotBernina.py +++ b/script/devices/RobotBernina.py @@ -9,6 +9,7 @@ DESC_DEFAULT = DESCS[0] FLANGE = "flange" +#DEFAULT_ROBOT_POLLING = 200 DEFAULT_ROBOT_POLLING = 200 TASK_WAIT_ROBOT_POLLING = 50 DEFAULT_SPEED=100 @@ -222,19 +223,26 @@ class RobotBernina(RobotTCP): # New try first delta ry = -math.asin(-math.cos(delta)*math.sin(gamma)) - rx = -math.acos(math.cos(delta)*math.cos(gamma)/math.cos(ry)) + rx = (-math.acos(math.cos(delta)*math.cos(gamma)/math.cos(ry))) rz = math.asin(-math.sin(ry)*math.sin(rx)*math.sqrt(1/(math.cos(rx)**2+(math.sin(ry)*math.sin(rx))**2))) rx, ry, rz = self.rad2deg([rx, ry, rz]) #end new try - print rz - #Necessary to keep detector orientation for gamma larger 90 + + #Necessary to keep detector orientation for gamma larger 90 or smaller -90 if self.rad2deg(gamma) > 90: rz = 180-rz + if self.rad2deg(gamma) < -90: + rz = -180-rz if self.rad2deg(delta) > 90: rz = -rz + #For delta --> 90, the calculation of rz runs into numerical issues: + if (self.rad2deg(delta) < 90.05)&(self.rad2deg(delta)>89.95): + rz = self.rad2deg(gamma) if abs(rz//180) > 0: rz = rz - (rz+180)//360*360 - + #Necessary correction for delta < 0 + rx = math.copysign(1, delta)*rx + rz = math.copysign(1, delta)*rz if return_dict: ret = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry, "rz":rz} @@ -564,13 +572,32 @@ class RobotBernina(RobotTCP): def motor_to_epics_pvs(self, motors=[]): self.epics_pvs = {str("SARES20-ROB:" + motor.name + ".VAL").upper(): CAS(str("SARES20-ROB:" + motor.name + ".VAL").upper(), motor, "double") for motor in motors} self.epics_pvs.update({str("SARES20-ROB:" + motor.name + ".RBV").upper(): CAS(str("SARES20-ROB:" + motor.name + ".RBV").upper(), motor.readback, "double") for motor in motors}) - + + + def on_reconnected(self): + #self.setup() + sleep(2) + get_context().restart() + + #robot.latency = 0.005 + def setup(self): + self.set_default_desc(DESC_DEFAULT) + self.default_speed = DEFAULT_SPEED + self.set_frame(self.frame_persistent()) + self.set_frame(self.frame(), name="tcp_f_spherical", change_default=False) + self.set_tool(self.tool_persistent()) + self.setPolling(DEFAULT_ROBOT_POLLING) + self.get_current_point() #TODO: REMOVE WHEN CURRENT POINT REPORTED BY POLLING MESSAGE + self.set_motors_enabled(True) + self.set_joint_motors_enabled(True) + + if simulation: add_device(RobotBernina("robot","localhost:1234"),force = True) else: add_device(RobotBernina("robot", "129.129.243.106:1234"), force = True) -time.sleep(0.1) +time.sleep(1) class DummySTP(RegisterBase): @@ -637,16 +664,9 @@ def make_archivable(Motor): epics_pvs.update({base + ".FLNK": CAS((base+".FLNK").upper(), DummySTP((base+".RBV").upper()), "string")}) Motor.epics_pvs = epics_pvs -#robot.latency = 0.005 -robot.set_default_desc(DESC_DEFAULT) -robot.default_speed = DEFAULT_SPEED -robot.set_frame(robot.frame_persistent()) -robot.set_frame(robot.frame(), name="tcp_f_spherical", change_default=False) -robot.set_tool(robot.tool_persistent()) -robot.setPolling(DEFAULT_ROBOT_POLLING) -robot.get_current_point() #TODO: REMOVE WHEN CURRENT POINT REPORTED BY POLLING MESSAGE -robot.set_motors_enabled(True) -robot.set_joint_motors_enabled(True) + +robot.setup() + motors = list(robot.cartesian_motors.values())+list(robot.spherical_motors.values())+list(robot.joint_motors.values())+list(robot.linear_axis_motors.values()) for m in motors: make_archivable(m) diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index 388ea30..6221f01 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -496,7 +496,7 @@ class RobotTCP(TcpDevice, Stoppable): if tool is None: tool = self.tool() #If joint_or_point is a list assumes ir is a point if not is_string(joint_or_point): - robot.set_pnt(joint_or_point , "tcp_p") + self.set_pnt(joint_or_point , "tcp_p") joint_or_point = "tcp_p" #TODO: in new robot movel and movej is freezing controller @@ -515,7 +515,7 @@ class RobotTCP(TcpDevice, Stoppable): if desc is None: desc = self.default_desc if tool is None: tool = self.tool() if not is_string(point): - robot.set_pnt(point , "tcp_p") + self.set_pnt(point , "tcp_p") point = "tcp_p" #TODO: in new robot movel and movej is freezing controller ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")") @@ -579,7 +579,7 @@ class RobotTCP(TcpDevice, Stoppable): def is_tool_open(self, tool=None): if tool is None: tool = self.tool() - self.tool_open = robot.eval_bool(tool + ".gripper") + self.tool_open = self.eval_bool(tool + ".gripper") return self.tool_open @@ -688,18 +688,15 @@ class RobotTCP(TcpDevice, Stoppable): except: ret[i] = None return ret + + def on_reconnected(self): + pass def _update_state(self): #self.setState(State.Busy if self.status=="move" else State.Ready) if self.state==State.Offline: print "Communication resumed" - sleep(5) - robot.set_default_desc(DESC_DEFAULT) - robot.default_speed = DEFAULT_SPEED - robot.set_frame(self.frame_persistent()) - robot.set_frame(self.frame_persistent(), name="tcp_f_spherical", change_default=False) - robot.set_tool(self.tool_persistent()) - robot.setPolling(DEFAULT_ROBOT_POLLING) + self.on_reconnected() if self.reset or (self.state==State.Offline): self.check_task() if self.current_task is not None: @@ -758,7 +755,7 @@ class RobotTCP(TcpDevice, Stoppable): self._update_state() self.reset = False - self.setCache({"powered": self.powered, + self.updateCache({"powered": self.powered, "speed": self.speed, "empty": self.empty, "settled": self.settled, @@ -766,7 +763,7 @@ class RobotTCP(TcpDevice, Stoppable): "mode": self.working_mode, "status": self.status, "open": self.tool_open, - }, None) + }) self.on_poll_status.update({ "connected": self.connected, @@ -787,7 +784,12 @@ class RobotTCP(TcpDevice, Stoppable): - + def updateCache(self, dictionary): + cur = self.take() + if cur is None: + cur={} + cur.update(dictionary) + self.setCache(cur, None) def doUpdate_fast(self): @@ -807,16 +809,17 @@ class RobotTCP(TcpDevice, Stoppable): self.linear_axis_pos = {"z_lin": float(sts[12])} self.update_spherical_pos() - + + """ ev_index = 8 #7 ev = sts[ev_index] if len(sts)>ev_index else "" if len(ev.strip()) >0: self.getLogger().info(ev) self.on_event(ev) - - #self.setCache({ - # "pos": self.get_current_point_cached() if self.state==State.Ready else None #TODO: make it calculated in robot by polling funtion - # }, None) + """ + self.updateCache({ + "pos": self.get_current_point_cached() if self.state==State.Ready else None #TODO: make it calculated in robot by polling funtion + }) pos = self.cartesian_pos.copy() pos.update(self.spherical_pos) pos.update(self.joint_pos) @@ -858,110 +861,6 @@ class RobotTCP(TcpDevice, Stoppable): self.setState(State.Offline) self.on_polling() - - def doUpdate_backup(self): - try: - start = time.time() - cur_task = self.current_task #Can change in background so cache it - if self.isSimulated(): - sts = [1, 1,"1", 100, "1", "1", 0, 0, \ - 0, 0, 0, 0, 0, 0, \ - 0, 0, 0, 0, 0, 0, \ - ] - else: - sts = self.execute("get_status", cur_task) - self._update_working_mode(int(sts[0]), int(sts[1])) - self.powered = sts[2] == "1" - self.speed = int(sts[3]) - self.empty = sts[4] == "1" - self.settled = sts[5] == "1" - - #TODO: add tool open - if cur_task is not None: - if self.isSimulated(): - if time.time() - self.current_task_timestamp > 3.0: - log("Task "+ str(cur_task) + " finished with code: -1", False) - ret = -1 if self.current_task_timestamp>0 else None - self.on_task_finished(self.current_task, ret) - self.current_task, self.current_task_ret = None, ret - - else: - if int(sts[6]) < 0: - try: - ret = int(sts[7]) - except: - ret = None - log("Task "+ str(cur_task) + " finished with code: " + str(ret), False) - self.on_task_finished(cur_task, ret) - self.current_task, self.current_task_ret = None, ret - - - - for i in range(6): - self.joint_pos[ROBOT_JOINT_MOTORS[i]] = float(sts[8 + i]) - for i in range(6): - self.cartesian_pos[ROBOT_MOTORS[i]] = float(sts[14 + i]) - self.linear_axis_pos = {"z_lin": float(sts[20])} - self.update_spherical_pos() - - ev_index = 21 #7 - ev = sts[ev_index] if len(sts)>ev_index else "" - if len(ev.strip()) >0: - self.getLogger().info(ev) - self.on_event(ev) - - self._update_state() - self.reset = False - self.setCache({"powered": self.powered, - "speed": self.speed, - "empty": self.empty, - "settled": self.settled, - "task": cur_task, - "mode": self.working_mode, - "status": self.status, - "open": self.tool_open, - "pos": self.get_current_point_cached() if self.state==State.Ready else None #TODO: make it calculated in robot by polling funtion - }, None) - pos = self.cartesian_pos.copy() - pos.update(self.spherical_pos) - pos.update(self.joint_pos) - pos.update(self.linear_axis_pos) - self.on_poll_status = { - "connected": self.connected, - "powered": self.powered, - "speed": self.speed, - "settled": self.settled, - "task": self.current_task, - "mode": self.working_mode, - "override_remote_safety": self.override_remote_safety, - "status": self.status, - "frame": self.frame(), - "tool": self.tool(), - "pos": pos, - "cartesian_motors_enabled": self.cartesian_motors_enabled, - "spherical_motors_enabled": self.spherical_motors_enabled, - } - - get_context().sendEvent("polling", self.on_poll_status) - if self.cartesian_motors_enabled: - for m in self.cartesian_motors.values(): - m.readback.update() - if self.spherical_motors_enabled: - for m in self.spherical_motors.values(): - m.readback.update() - if self.linear_axis_motors_enabled: - for m in self.linear_axis_motors.values(): - m.readback.update() - if self.joint_motors_enabled: - for m in self.joint_motors.values(): - m.readback.update() - - - except: - if self.state != State.Offline: - print >> sys.stderr, "Update error: " + str(sys.exc_info()[1]) - self.setState(State.Offline) - def on_poll_info(self): on_poll_config_methods ={ @@ -1104,7 +1003,7 @@ class RobotTCP(TcpDevice, Stoppable): raise Exception ("Not in position " + p) def is_emulation(self): - return "localhost" in robot.client.getServerAddress() + return "localhost" in self.client.getServerAddress() #Cartesian peudo-motors def set_motors_enabled(self, value, pseudos=["cartesian", "spherical", "linear"]):