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