diff --git a/plugins/MXSC-1.9.0.jar b/plugins/MXSC-1.9.0.jar index e67d248..57618ab 100644 Binary files a/plugins/MXSC-1.9.0.jar and b/plugins/MXSC-1.9.0.jar differ diff --git a/script/devices/RobotSC.py b/script/devices/RobotSC.py index 27142fb..4c17c6e 100644 --- a/script/devices/RobotSC.py +++ b/script/devices/RobotSC.py @@ -9,6 +9,7 @@ DESC_DEFAULT = DESC_FAST DEFAULT_ROBOT_POLLING = 500 +TASK_WAIT_ROBOT_POLLING = 50 run("devices/RobotTCP") @@ -22,44 +23,45 @@ joint_forces = False class RobotSC(RobotTCP): def __init__(self, name, server, timeout = 1000, retries = 1): RobotTCP.__init__(self, name, server, timeout, retries) + self.set_tasks(["getDewar", "putDewar", "putGonio", "getGonio", "robotRecover", "moveDewar", "movePark"]) self.setPolling(DEFAULT_ROBOT_POLLING) def get_dewar(self, segment, puck, sample): segment = self.toSegmentNumber(segment) self.assert_dewar() self.start_task('getDewar',segment, puck, sample) - self.wait_task_finished() + self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) self.assert_dewar() def put_dewar(self, segment, puck, sample): segment = self.toSegmentNumber(segment) self.assert_dewar() self.start_task('putDewar',segment, puck, sample) - self.wait_task_finished() + self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) self.assert_dewar() def put_gonio(self): self.start_task('putGonio',segment, puck, sample) - self.wait_task_finished() + self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) #TODO def get_gonio(self): self.start_task('getGonio') - self.wait_task_finished() + self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) #TODO def robot_recover(self): self.start_task('robotRecover') - self.wait_task_finished() + self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) - def move_dewar(self): + def move_dewar_wait(self): self.start_task('moveDewar') - self.wait_task_finished() - self.assert_dewar() + self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) + self.assert_dewar_wait() def move_park(self): self.start_task('movePark') - self.wait_task_finished() + self.wait_task_finished(TASK_WAIT_ROBOT_POLLING) self.assert_park() def toSegmentNumber(self, segment): @@ -86,11 +88,12 @@ class RobotSC(RobotTCP): def start_task(self, program, *args, **kwargs): #TODO: Check safe position - RobotTCP.start_task(self, program, *args, **kwargs) + return RobotTCP.start_task(self, program, *args, **kwargs) def stop_task(self): RobotTCP.stop_task(self) - #TODO: Restore safe position + #TODO: Restore safe position + def set_remote_mode(self): robot.set_profile("remote") @@ -104,10 +107,10 @@ class RobotSC(RobotTCP): def is_park(self): return self.is_in_point("pPark") - def is_dewarHome(self): + def is_dewar_home(self): return self.is_in_point("pDewarHome") - def is_dewarWait(self): + def is_dewar_wait(self): return self.is_in_point("pDewarWait") def is_gonio(self): @@ -122,10 +125,10 @@ class RobotSC(RobotTCP): def assert_park(self): self.assert_in_point("pPark") - def assert_dewarHome(self): + def assert_dewar_home(self): self.assert_in_point("pDewarHome") - def assert_dewarWait(self): + def assert_dewar_wait(self): self.assert_in_point("pDewarWait") def assert_gonio(self): @@ -149,7 +152,7 @@ else: add_device(RobotSC("robot", "129.129.110.100:1000"), force = True) #robot.set_monitor_speed(20) - +robot.set_default_desc(DESC_DEFAULT) class jf1(ReadonlyRegisterBase): diff --git a/script/devices/RobotTCP.py b/script/devices/RobotTCP.py index 3d9785e..9bfa736 100644 --- a/script/devices/RobotTCP.py +++ b/script/devices/RobotTCP.py @@ -36,31 +36,44 @@ class RobotTCP(TcpDevice, Stoppable): self.joint_motors_enabled = False self.joint_motors = [] self.tool = None + self.default_desc = None #self.tool_trsf = [0.0] * 6 self.frame = FRAME_DEFAULT - + self.polling_interval = 0.01 + self.reset = True def doInitialize(self): super(RobotTCP, self).doInitialize() self.reset = True - - def set_tool(self,tool): + def set_tool(self, tool): self.tool = 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.set_motors_enabled(True) def get_tool(self): return self.tool + def assert_tool(self, tool): if self.tool != tool: raise Exception("Invalid tool: " + self.tool) - + + def set_default_desc(self,default_desc): + self.default_desc=default_desc + + def get_default_desc(self): + return self.default_desc + + def set_tasks(self,tasks): + self.high_level_tasks=tasks + + def get_tasks(self): + return self.high_level_tasks + def _sendReceive(self, msg_id, msg = "", timeout = None): tx = self.header if (self.header != None) else "" tx = tx + msg_id + " " + msg @@ -318,19 +331,25 @@ class RobotTCP(TcpDevice, Stoppable): self.joint_forces = None raise - def movej(self, joint_or_point, tool, desc, sync=False): + def movej(self, joint_or_point, tool=None, desc=None, sync=False): + if desc is None: desc = self.default_desc + if tool is None: tool = self.tool ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")") if sync: self.wait_end_of_move() return ret - def movel(self, point, tool, desc, sync=False): + def movel(self, point, tool=None, desc=None, sync=False): + if desc is None: desc = self.default_desc + if tool is None: tool = self.tool ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")") if sync: self.wait_end_of_move() return ret - def movec(self, point_interm, point_target, tool, desc, sync=False): + def movec(self, point_interm, point_target, tool=None, desc=None, sync=False): + if desc is None: desc = self.default_desc + if tool is None: tool = self.tool ret = self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")") if sync: self.wait_end_of_move() @@ -371,17 +390,20 @@ class RobotTCP(TcpDevice, Stoppable): def compose(self, pnt, frame = FRAME_DEFAULT, trsf = "tcp_t"): return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")") - def here(self, tool, frame): + def here(self, tool=None, frame=FRAME_DEFAULT): + if tool is None: tool = self.tool return self.eval_pnt("here(" + tool + ", " + frame + ")") - def joint_to_point(self, tool, frame, joint="tcp_j"): + def joint_to_point(self, tool=None, frame=FRAME_DEFAULT, joint="tcp_j"): + if tool is None: tool = self.tool return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")") - def point_to_joint(self, tool, initial_joint="tcp_j", point="tcp_p"): + def point_to_joint(self, tool=None, initial_joint="tcp_j", point="tcp_p"): + if tool is None: tool = self.tool if self.eval_bool("pointToJoint(" + tool + ", " + initial_joint + ", " + point +", j)"): return self.get_jnt() - def position(self, point, frame): + def position(self, point, frame=FRAME_DEFAULT): return self.eval_trf("position(" + point + ", " + frame + ")") #Profile @@ -434,9 +456,9 @@ class RobotTCP(TcpDevice, Stoppable): def _update_state(self): #self.setState(State.Busy if self.status=="move" else State.Ready) if self.state==State.Offline: - print "Communication resumed" + print "Communication resumed" if self.reset or (self.state==State.Offline): - self.get_task() + self.check_task() if self.current_task is not None: print "Ongoing task: " + self.current_task @@ -564,7 +586,8 @@ class RobotTCP(TcpDevice, Stoppable): for m in self.joint_motors: m.initialize() #Alignment - def align(self, point = None, frame = FRAME_DEFAULT, desc = DESC_FAST): + def align(self, point = None, frame = FRAME_DEFAULT, desc = None): + if desc is None: desc = self.default_desc if point is None: self.get_cartesian_pos() else: @@ -609,21 +632,29 @@ class RobotTCP(TcpDevice, Stoppable): self.task_kill(task) def get_task(self): - if self.current_task is not None: - return self.current_task - for task in self.high_level_tasks: - if self.get_task_status(task)[0]>=0: - self.current_task = task - log("Task detected: " + str(self.current_task), False) - return task - return None + return self.current_task + + def check_task(self): + if self.current_task is None: + for task in self.high_level_tasks: + if self.get_task_status(task)[0]>=0: + self.current_task = task + log("Task detected: " + str(self.current_task), False) + return self.current_task - def wait_task_finished (self): - while get_task() != None: - time.sleep(0.01) + def wait_task_finished (self, polling = None): + cur_polling = self.polling + if polling is not None: + self.polling = polling + try: + while self.get_task() != None: + time.sleep(self.polling_interval) + finally: + if polling is not None: + self.polling = cur_polling def assert_no_task(self): - task = self.get_task() + task = self.check_task() if task != None: raise Exception("Ongoing task: " + task) diff --git a/script/local.py b/script/local.py index 874d4a7..0e87ed0 100644 --- a/script/local.py +++ b/script/local.py @@ -64,8 +64,7 @@ except: print >> sys.stderr, traceback.format_exc() try: - robot.setPolling(DEFAULT_ROBOT_POLLING) - robot.high_level_tasks = ["getDewar", "putDewar", "putGonio", "getGonio", "robotRecover", "moveDewar", "movePark"] + robot.setPolling(DEFAULT_ROBOT_POLLING) robot.set_tool(TOOL_CALIBRATION) robot.set_motors_enabled(True) robot.set_joint_motors_enabled(True) diff --git a/script/motion/mount.py b/script/motion/mount.py index 6907f08..b595560 100644 --- a/script/motion/mount.py +++ b/script/motion/mount.py @@ -26,7 +26,7 @@ def mount(segment, puck, sample, force=False): location = get_current_point() print "Location: " + location - robot.move_dewar() + robot.move_dewar_wait() try: robot.get_dewar(segment, puck, sample) except: diff --git a/script/motion/unmount.py b/script/motion/unmount.py index e151134..ab9640e 100644 --- a/script/motion/unmount.py +++ b/script/motion/unmount.py @@ -32,13 +32,13 @@ def unmount(segment, puck, sample, force=False): raise try: - robot.move_dewar() + robot.move_dewar_wait() except: #TODO: recover raise try: - robot.pet_dewar(segment, puck, sample) + robot.put_dewar(segment, puck, sample) except: #robot.move_dewar() raise \ No newline at end of file diff --git a/script/test/TestCmdSynchronization.py b/script/test/TestCmdSynchronization.py new file mode 100644 index 0000000..41a51e2 --- /dev/null +++ b/script/test/TestCmdSynchronization.py @@ -0,0 +1,5 @@ +enable_motion() +while True: + + robot.move_dewar_wait() + robot.move_park()