From ac5bb50a5ece5ca936addfec93315122e552c73c Mon Sep 17 00:00:00 2001 From: gac-bernina Date: Thu, 21 Dec 2023 16:28:49 +0100 Subject: [PATCH] Split updates in slowly and rapidly polled variables --- script/devices/RobotBernina.py | 2 +- script/devices/RobotTCP.py | 190 ++++++++++++++++++++++++++++++--- 2 files changed, 177 insertions(+), 15 deletions(-) diff --git a/script/devices/RobotBernina.py b/script/devices/RobotBernina.py index 0f338c5..25d29d9 100644 --- a/script/devices/RobotBernina.py +++ b/script/devices/RobotBernina.py @@ -334,7 +334,7 @@ class RobotBernina(RobotTCP): if simulation: add_device(RobotBernina("robot","localhost:1234"),force = True) else: - add_device(RobotBernina("robot", "129.129.243.105:1234"), force = True) + add_device(RobotBernina("robot", "129.129.243.106:1234"), force = True) time.sleep(0.1) diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index 8c768be..374d6bd 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -49,8 +49,9 @@ class RobotTCP(TcpDevice, Stoppable): self.tool = None self.default_desc = None self.tool_open = None - #self.tool_trsf = [0.0] * 6 + self.tool_trsf = [0.0] * 6 self.frame = FRAME_DEFAULT + self.frame_trsf = [0.0] * 6 self.polling_interval = 0.01 self.reset = True self.default_tolerance = 5 @@ -60,27 +61,33 @@ class RobotTCP(TcpDevice, Stoppable): self.task_start_retries = 3 self.exception_on_task_start_failure = True #Tasks may start and be finished when checked self.simulated_point = "" + self.update_start_time = 0 + self.polling_env = 1 def doInitialize(self): super(RobotTCP, self).doInitialize() self.reset = True def set_tool(self, tool): - self.tool = tool + self.tool=tool + self.set_str(tool, name="s_actualTool") + self.evaluate("t_actualTool=" + tool) #self.tool_trsf = self.get_tool_trsf() - self.evaluate("tcp_curtool=" + tool) if self.cartesian_motors_enabled: self.update() self.set_motors_enabled(True) self.is_tool_open() - def get_tool(self): + def get_tool(self): return self.tool - def set_frame(self, frame, name="tcp_curframe", change_default=True): - self.evaluate(name + "=" + frame) + def set_frame(self, frame, name=None, change_default=True): + if name is not None: + self.evaluate(name + "=" + frame) if change_default: - self.frame = frame + self.set_str(frame, name="s_actualFrame") + self.evaluate("f_actualFrame" + "=" + frame) + self.frame=frame if self.cartesian_motors_enabled: self.update() self.set_motors_enabled(True) @@ -198,9 +205,12 @@ class RobotTCP(TcpDevice, Stoppable): def get_var(self, name): return self.execute('get_var', name) - #Makes app crash - #def get_str(self, name='s'): - # return self.execute('get_str', name) + #Makes app crash - Still true? + def get_str(self, name='tcp_s'): + return self.execute('get_str', name) + + def set_str(self, l, name = "tcp_s"): + self.evaluate(name + '="' + str(l) + '"') def get_arr(self, name, size): return self.execute('get_arr', name, size) @@ -290,9 +300,9 @@ class RobotTCP(TcpDevice, Stoppable): self.evaluate("tcp_b=" + cmd) return self.get_bool() - #def eval_str(self, cmd): - # self.evaluate("s=" + cmd) - # return self.get_str() + def eval_str(self, cmd): + self.evaluate("tcp_s=" + cmd) + return self.get_str() def eval_jnt(self, cmd): self.evaluate("tcp_j=" + cmd) @@ -640,8 +650,157 @@ class RobotTCP(TcpDevice, Stoppable): if (not self.settled) or (self.current_task is not None): self.setState(State.Busy) elif not self.empty: self.setState(State.Paused) else: self.setState(State.Ready) + + def doUpdate_env(self): + 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_env", 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 + + self.frame = sts[8] + self.tool = sts[9] + for i in range(6): + self.frame_trsf[i] = float(sts[10 + i]) + for i in range(6): + self.tool_trsf[i] = float(sts[16 + i]) + self.update_spherical_pos() + + ev_index = 22 #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, + }, None) + + self.on_poll_status.update({ + "connected": self.connected, + "powered": self.powered, + "speed": self.speed, + "settled": self.settled, + "task": self.current_task, + "mode": self.working_mode, + "status": self.status, + "frame": self.frame, + "frame_coordinates": self.frame_trsf, + "tool": self.tool, + "tool_coordinates": self.tool_trsf, + "cartesian_motors_enabled": self.cartesian_motors_enabled, + "spherical_motors_enabled": self.spherical_motors_enabled, + }) + + + + + + + def doUpdate_fast(self): + 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_fast", cur_task) + + for i in range(6): + self.joint_pos[ROBOT_JOINT_MOTORS[i]] = float(sts[0 + i]) + for i in range(6): + self.cartesian_pos[ROBOT_MOTORS[i]] = float(sts[6 + i]) + + self.linear_axis_pos = {"z_lin": float(sts[7])} + 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) + 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.update({ + "pos": pos, + }) + + if self.cartesian_motors_enabled: + for m in self.cartesian_motors: + m.readback.update() + if self.spherical_motors_enabled: + for m in self.spherical_motors: + m.readback.update() + if self.linear_axis_motors_enabled: + for m in self.linear_axis_motors: + m.readback.update() + if self.joint_motors_enabled: + for m in self.joint_motors: + m.readback.update() + + def doUpdate(self): + try: + self.doUpdate_fast() + except: + if self.state != State.Offline: + print >> sys.stderr, "Update error: " + str(sys.exc_info()[1]) + self.setState(State.Offline) + + if time.time() - self.update_start_time >= self.polling_env: + self.update_start_time = time.time() + try: + self.doUpdate_env() + except: + if self.state != State.Offline: + print >> sys.stderr, "Update error: " + str(sys.exc_info()[1]) + self.setState(State.Offline) + + get_context().sendEvent("polling", self.on_poll_status) - def doUpdate(self): + def doUpdate_backup(self): try: start = time.time() cur_task = self.current_task #Can change in background so cache it @@ -742,13 +901,16 @@ class RobotTCP(TcpDevice, Stoppable): 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 ={ "powered": {"cmd":"robot.set_powered", "def_kwargs": ""}, "speed": {"cmd":"robot.set_monitor_speed", "def_kwargs": ""}, "frame": {"cmd": "robot.set_frame", "def_kwargs": "change_default=True",}, + "frame_coordinates": {"cmd": "robot.set_trsf", "def_kwargs": "name='f_actualFrame.trsf'",}, "tool": {"cmd":"robot.set_tool", "def_kwargs": ""}, + "tool_coordinates": {"cmd": "robot.set_trsf", "def_kwargs": "name='t_actualTool.trsf'",}, "cartesian_motors_enabled": {"cmd":"robot.set_motors_enabled", "def_kwargs": "pseudos=['cartesian']"}, "spherical_motors_enabled": {"cmd":"robot.set_motors_enabled", "def_kwargs": "pseudos=['spherical']"}, }