From 5e798200ffcfc6cf45d04e66204895b282da0ca7 Mon Sep 17 00:00:00 2001 From: gac-S_Changer Date: Fri, 1 Jun 2018 17:27:13 +0200 Subject: [PATCH] --- script/devices/RobotSC.py | 10 ++- script/devices/RobotTCP.py | 109 +++++++++++++++++++------- script/motion/mount.py | 2 +- script/motion/unmount.py | 2 +- script/test/TestCmdSynchronization.py | 35 ++++++++- 5 files changed, 119 insertions(+), 39 deletions(-) diff --git a/script/devices/RobotSC.py b/script/devices/RobotSC.py index 4c17c6e..2dee391 100644 --- a/script/devices/RobotSC.py +++ b/script/devices/RobotSC.py @@ -25,6 +25,7 @@ class RobotSC(RobotTCP): RobotTCP.__init__(self, name, server, timeout, retries) self.set_tasks(["getDewar", "putDewar", "putGonio", "getGonio", "robotRecover", "moveDewar", "movePark"]) self.setPolling(DEFAULT_ROBOT_POLLING) + self.known_points = ["pHome", "pPark", "pDewarHome", "pGonioHome", "pDewarWait"] def get_dewar(self, segment, puck, sample): segment = self.toSegmentNumber(segment) @@ -138,10 +139,11 @@ class RobotSC(RobotTCP): if not self.is_cleared(): raise Exception("Robot not in cleared position") - def get_current_point(self): - for p in ["pHome", "pPark", "pDewarHome", "pGonioHome", "pDewarWait"]: - if self.is_in_point(p): - return p + def get_current_point(self): + ret = robot.is_in_points(*self.known_points, tolerance = 20) + for i in range(len(ret)): + if ret[i] == True: + return self.known_points[i] return None diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index 9bfa736..790b653 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -40,7 +40,11 @@ class RobotTCP(TcpDevice, Stoppable): #self.tool_trsf = [0.0] * 6 self.frame = FRAME_DEFAULT self.polling_interval = 0.01 - self.reset = True + self.reset = True + + self.task_start_retries = 3 + self.exception_on_task_start_failure = False #Tasks may start and be finished when checked + def doInitialize(self): super(RobotTCP, self).doInitialize() @@ -453,6 +457,16 @@ class RobotTCP(TcpDevice, Stoppable): else: status = "Error" return (code,status) + def get_tasks_status(self, *pars): + ret = self.execute("task_sts", *pars) + ret = ret[0:len(pars)] + for i in range(len(ret)): + try: + ret[i] = int(ret[i]) + except: + ret[i] = None + return ret + def _update_state(self): #self.setState(State.Busy if self.status=="move" else State.Ready) if self.state==State.Offline: @@ -469,8 +483,6 @@ class RobotTCP(TcpDevice, Stoppable): def doUpdate(self): try: start = time.time() - #sts = self._sendReceive("EVT").strip().split(self.array_separator) - #sts = self.execute("get_status", self.current_task, self.tool, self.frame) sts = self.execute("get_status", self.current_task) self._update_working_mode(int(sts[0]), int(sts[1])) self.powered = sts[2] == "1" @@ -479,24 +491,20 @@ class RobotTCP(TcpDevice, Stoppable): self.settled = sts[5] == "1" if int(sts[6]) < 0: if self.current_task is not None: - log("Task finished: " + str(self.current_task), False) + log("Task "+ str(self.current_task) + " finished with code: " + str(sts[7]), False) self.current_task = None for i in range(6): - self.joint_pos[i] = float(sts[7 + i]) + self.joint_pos[i] = float(sts[8 + i]) for i in range(6): - #self.flange_pos[i] = float(sts[13 + i]) - #self.tool_pos[i] = (self.flange_pos[i] - self.tool_trsf[i] ) if i<3 else (self.flange_pos[i] + self.tool_trsf[i] ) - self.cartesian_pos[i] = float(sts[13 + i]) + self.cartesian_pos[i] = float(sts[14 + i]) - ev_index = 19 #7 + ev_index = 20 #7 ev = sts[ev_index] if len(sts)>ev_index else "" if len(ev.strip()) >0: self.getLogger().info(ev) self.on_event(ev) - - #if (self.current_task is not None) and (self.get_task_status(self.current_task)[0]<0): self._update_state() self.reset = False self.setCache({"powered": self.powered, @@ -537,13 +545,33 @@ class RobotTCP(TcpDevice, Stoppable): self.get_cartesian_pos() return self.distance_p("tcp_p", name) - def is_in_point(self, p, tolerance = 50): #Tolerance in mm + def is_in_point(self, p, tolerance = 20): #Tolerance in mm d = self.get_distance_to_pnt(p) if d<0: raise Exception ("Error calculating distance to " + p + " : " + str(d)) return d=0: - raise Exception("Ongoing high-level task: " + task) - self.task_create(program, *args, **kwargs) - start = time.time() - #while self.get_task_status(program)[0] < 0: - # if time.time() - start > 5.0: - # raise Exception("Cannot start task " + task) - # time.sleep(0.1) - self.current_task = program - (code, status) = self.get_task_status(task) - log("Task started: " + str(self.current_task) + " - status: " + str(status) + " (" + str(code) + ")", False) + #for task in tasks: + # if self.get_task_status(task)[0]>=0: + # raise Exception("Ongoing high-level task: " + task) + ts = robot.get_tasks_status(*tasks) + for i in range(len(ts)): + if ts[i] > 0: + raise Exception("Ongoing high-level task: " + tasks[i]) + + self.clear_task_ret() + for i in range(self.task_start_retries): + self.task_create(program, *args, **kwargs) + (code, status) = self.get_task_status(program) + if code>=0: break + if i < retries-1: + ret = self.get_task_ret() + if ret == 0: #Did't start + log("Retrying starting : " + str(program) + " - status: " + str(status) + " (" + str(code) + ")", False) + print "Retrying starting : " + str(program) + " - status: " + str(status) + " (" + str(code) + ")" + else : + break + else: + log("Failed starting : " + str(program), False) + print "Failed starting : " + str(program) + if self.exception_on_task_start_failure: + raise Exception("Cannot start task: " + task) + + log("Task started: " + str(program) + " - status: " + str(status) + " (" + str(code) + ")", False) + self.current_task = program self.update() - self._update_state() + #self._update_state() return code def stop_task(self): @@ -631,6 +675,13 @@ class RobotTCP(TcpDevice, Stoppable): #if self.get_task_status(task)[0]>=0: self.task_kill(task) + + def get_task_ret(self): + return self.eval_int("tcp_ret") + + def clear_task_ret(self): + return self.evaluate("tcp_ret=0") + def get_task(self): return self.current_task @@ -648,7 +699,7 @@ class RobotTCP(TcpDevice, Stoppable): self.polling = polling try: while self.get_task() != None: - time.sleep(self.polling_interval) + time.sleep(self.polling_interval) finally: if polling is not None: self.polling = cur_polling diff --git a/script/motion/mount.py b/script/motion/mount.py index b595560..92fb98b 100644 --- a/script/motion/mount.py +++ b/script/motion/mount.py @@ -23,7 +23,7 @@ def mount(segment, puck, sample, force=False): robot.move_park() assert_img_in_cover_pos(segment) - location = get_current_point() + location = robot.get_current_point() print "Location: " + location robot.move_dewar_wait() diff --git a/script/motion/unmount.py b/script/motion/unmount.py index ab9640e..aada773 100644 --- a/script/motion/unmount.py +++ b/script/motion/unmount.py @@ -22,7 +22,7 @@ def unmount(segment, puck, sample, force=False): robot.move_park() assert_img_in_cover_pos(segment) - location = get_current_point() + location = robot.get_current_point() try: diff --git a/script/test/TestCmdSynchronization.py b/script/test/TestCmdSynchronization.py index 41a51e2..82dda1c 100644 --- a/script/test/TestCmdSynchronization.py +++ b/script/test/TestCmdSynchronization.py @@ -1,5 +1,32 @@ -enable_motion() -while True: +import java.util.logging.Level - robot.move_dewar_wait() - robot.move_park() +def get_pos_str(): + return "point: " + str(robot.get_current_point()) + " - here: " + str(robot.here()) + " - herej: " + str(robot.herej()) + +enable_motion() + +get_context().setLogLevel(java.util.logging.Level.FINER) +try: + while True: + #robot.move_dewar_wait() + #robot.move_park() + + log("Moving dewar", False) + flag = robot.start_task('moveDewar') + print "moveDewar: ", flag + log("Waiting", False) + robot.wait_task_finished(TASK_WAIT_ROBOT_POLLING) + log("Moving dewar finished - " + get_pos_str(), False) + robot.assert_dewar_wait() + + + log("Moving park", False) + flag = robot.start_task('movePark') + print "movePark: ", flag + log("Waiting", False) + robot.wait_task_finished(TASK_WAIT_ROBOT_POLLING) + log("Moving park finished - " + get_pos_str(), False) + robot.assert_park() + +finally: + get_context().setLogLevel(java.util.logging.Level.INFO) \ No newline at end of file