diff --git a/robot/MXLAB/Controller1/usr/configs/controller.cfx b/robot/MXLAB/Controller1/usr/configs/controller.cfx index 78cebc8..749d384 100644 --- a/robot/MXLAB/Controller1/usr/configs/controller.cfx +++ b/robot/MXLAB/Controller1/usr/configs/controller.cfx @@ -33,5 +33,5 @@ - + \ No newline at end of file diff --git a/script/RobotSC.py b/script/RobotSC.py index 5a98ba8..7cae203 100644 --- a/script/RobotSC.py +++ b/script/RobotSC.py @@ -2,18 +2,17 @@ run("RobotTCP") simulation = True + class RobotSC(RobotTCP): def mount(self, puck, sample): - return self.execute('mount', puck, sample) + return self.execute('mount',segment, puck, sample) def firstmount(self, puck, sample): - return self.execute('firstmount', puck, sample) + return self.execute('firstmount', segment, puck, sample) def unmount(self, puck, sample): - return self.execute('unmount', puck, sample) - - - + return self.execute('unmount',segment, puck, sample) + def on_event(self,ev): #print "EVT: " + ev pass @@ -26,10 +25,12 @@ class RobotSC(RobotTCP): if self.state != State.Offline: self.get_joint_forces() for dev in [jf1, jf2, jf3, jf4,jf5, jf6, jfc]: - dev.update() + dev.update() + #print time.time() -start - + + @@ -38,10 +39,12 @@ if simulation: else: add_device(RobotSC("robot", "129.129.126.100:1000"), force = True) +robot.high_level_tasks = ["mount", "firstmount"] robot.setPolling(20) #robot.set_monitor_speed(20) + class jf1(ReadonlyRegisterBase): def doRead(self): return None if robot.joint_forces == None else robot.joint_forces[0] diff --git a/script/RobotTCP.py b/script/RobotTCP.py index 5a8f1f6..05810b5 100644 --- a/script/RobotTCP.py +++ b/script/RobotTCP.py @@ -19,6 +19,8 @@ class RobotTCP(TcpDevice, Stoppable): self.status = None self.lock = threading.Lock() self.joint_forces = None + self.running_task = False + self.high_level_tasks = [] def _sendReceive(self, msg_id, msg = "", timeout = None): tx = self.header if (self.header != None) else "" @@ -342,7 +344,7 @@ class RobotTCP(TcpDevice, Stoppable): #self.setState(State.Busy if self.status=="move" else State.Ready) if self.state==State.Offline: print "Communication resumed" - if not self.settled: self.setState(State.Busy) + if (not self.settled) or (self.running_task is not None): self.setState(State.Busy) if not self.empty: self.setState(State.Paused) else: self.setState(State.Ready) @@ -359,18 +361,30 @@ class RobotTCP(TcpDevice, Stoppable): ev = sts[6] if len(sts)>6 else "" if len(ev.strip()) >0: self.getLogger().info(ev) - self.on_event(ev) - self._update_state() + self.on_event(ev) + if ((self.running_task is not None) and (self.get_task_status(self.running_task)<=0)): + self.running_task = None + self._update_state() self.setCache({"powered": self.powered, "speed": self.speed, "empty": self.empty, - "settled": self.settled }, None) + "settled": self.settled, + "task": self.running_task }, None) + #print time.time() - start except: if self.state != State.Offline: print >> sys.stderr, "Update error: " + str(sys.exc_info()[1]) self.setState(State.Offline) + #High-level, exclusive motion task. + def start_task(self, program, *args, **kwargs): + for task in HIGH_LEVEL_TASKS: + if self.get_task_status(task)>=0: + raise Exception("Cannot start program - ongoing high-level task: " + task) + self.current_task = program + self.task_create(program, args, kwargs) + def on_event(self,ev): pass