diff options
| -rw-r--r-- | MotionControl/Action.py | 2 | ||||
| -rw-r--r-- | MotionControl/Axis.py | 7 | ||||
| -rw-r--r-- | MotionControl/MotionStage.py | 96 | ||||
| -rwxr-xr-x | motiond/motiond | 44 |
4 files changed, 115 insertions, 34 deletions
diff --git a/MotionControl/Action.py b/MotionControl/Action.py index 963f34b..f0653a5 100644 --- a/MotionControl/Action.py +++ b/MotionControl/Action.py @@ -89,7 +89,7 @@ class EmergencyStop(Action): self.axes = axes def do_execute(self): - for a in axes: + for a in self.axes: a.emergency_stop() def do_ended(self): diff --git a/MotionControl/Axis.py b/MotionControl/Axis.py index ff8c900..4d669c5 100644 --- a/MotionControl/Axis.py +++ b/MotionControl/Axis.py @@ -2,6 +2,7 @@ class Axis(object): def __init__(self, inverted = False, scale={}): + from blinker import Signal self.inverted = inverted self.scale = scale self.position = None @@ -35,13 +36,13 @@ class Axis(object): self.do_update() if last_position != self.position: - self.onPosition.send(position = self.position) + self.onPosition.send(self, position = self.position) if last_running != self.running: if self.running: - self.onStarted.send() + self.onStarted.send(self) else: - self.onStopped.send() + self.onStopped.send(self) if last_initializing != self.initializing: self.onInitializing.send(self, initializing = self.initializing) diff --git a/MotionControl/MotionStage.py b/MotionControl/MotionStage.py index 937ce45..2e3583f 100644 --- a/MotionControl/MotionStage.py +++ b/MotionControl/MotionStage.py @@ -5,58 +5,95 @@ class CycleAbort(Exception): class MotionStage(object): def __init__(self, axes, constraints = None): - import Queue + from Action import EmergencyStop + from blinker import Signal - self.axes = axes + self.axes_idx = dict() + for i, axis in enumerate(axes): + self.axes_idx[axis] = i + self.axes = [axis for axis in axes] self.constraints = constraints - - self.action_queue = Queue.Queue() - self.abort_action = NullAction() + self.abort_action = EmergencyStop(self.axes) self.onCycleStarted = Signal() self.onCycleFinished = Signal() self.onCycleAborted = Signal() + self.onDestinationChanged = Signal() + self.onPositionChanged = Signal() + + for axis in self.axes: + axis.onPosition.connect(self.forward_onPosition) + + self.worker_thread = None self.active = False - self.target = None + self.destination = None + self.cycle_clear() + + self.update() def __del__(self): self.abort() - def __getattr__(self, name): - if name == 'position': - return [axis.position for axis in self.axes] + def cycle_clear(self): + import Queue + self.action_queue = Queue.Queue() + + def cycle_add_action(self, action): + if not self.action_queue: + self.cycle_clear() + self.action_queue.put(action) + + @property + def position(self): + return tuple([axis.position for axis in self.axes]) + + def forward_onPosition(self, sender, position): + self.onPositionChanged.send(self, axis=self.axes_idx[sender], position=position) def update(self): + old_position = self.position for axis in self.axes: axis.update() - - def set_target(self, target): - if isinstance(target, list): - if len(target) != len(self.axes): + if old_position != self.position: + self.onPositionChanged.send(self, position = self.position) + + def set_destination(self, destination): + current_position = self.position + if not self.destination: + self.update() + self.destination = current_position + if isinstance(destination, list) or isinstance(destination, tuple): + if len(destination) != len(self.axes): raise ValueError - self.target = target - if isinstance(target, dict): - for k,v in target: - self.target[k] = v - if isinstance(target, tuple): - self.target[target[0]] = target[1] + self.destination = tuple(destination) + if isinstance(destination, dict): + new_destination = list(self.destination) + for k in destination: + new_destination[k] = destination[k] + self.destination = tuple(new_destination) + + for i,dest in enumerate(self.destination): + self.onDestinationChanged.send(self, axis = i, destination = dest) speed = None - current_position = self.position if not None in current_position: - delta = [abs(a-b) for a,b in zip(target, current_position)] + delta = [abs(a-b) for a,b in zip(self.destination, current_position)] max_delta = max(delta) - speed = [float(d)/float(max_delta) for d in delta] - self.action_queue = Queue.Queue() - self.action_queue.put(GotoAbsolute(self.axes, self.target, speed)) + if max_delta > 0: + speed = [float(d)/float(max_delta) for d in delta] + + from Action import GotoAbsolute + + self.cycle_clear() + self.cycle_add_action(GotoAbsolute(self.axes, self.destination, speed)) def can_cycle_start(self): if self.active: return False return True # FIXME: Add constraint tests here - def start_cycle(self): + def cycle_start(self): import threading, weakref if not self.can_cycle_start(): @@ -64,17 +101,16 @@ class MotionStage(object): self.current_action = None self.active = True - self.worker_thread = threading.Thread(target = MotionControl.cycle_worker, name = "MotionControl.worker", args=(weakref.proxy(self),)) + self.worker_thread = threading.Thread(target = MotionStage.cycle_worker, name = "MotionControl.worker", args=(weakref.proxy(self),)) self.worker_thread.daemon =True self.worker_thread.start() self.onCycleStarted.send() def abort(self): + import threading self.active = False - self.worker_thread.join() - - def __del__(self): - self.abort() + if isinstance(self.worker_thread, threading.Thread): + self.worker_thread.join() def cycle_worker(ref): abort_action = ref.abort_action diff --git a/motiond/motiond b/motiond/motiond index 6c2cadb..3bfbde2 100755 --- a/motiond/motiond +++ b/motiond/motiond @@ -1,2 +1,46 @@ #!/usr/bin/python2 # -*- encoding: utf-8 -*- + +import Phytron + +import MotionControl.MotionStage, MotionControl.PhytronAxis, MotionControl.Service +from MotionControl.MotionStage import MotionStage +from MotionControl.PhytronAxis import PhytronAxis +from MotionControl.Service import Service + +ipcomm = Phytron.IPCOMM('/dev/ttyERDASTEP', axes=4) + +axis_x = PhytronAxis( ipcomm[0], \ + max_run_freq = 825, \ + initiator = PhytronAxis.INITIATOR_PLUS, \ + initiator_position = 221786, \ + scale = {'mm': 8000} ) + +axis_y = PhytronAxis( ipcomm[1], \ + max_run_freq = 825, \ + initiator = PhytronAxis.INITIATOR_PLUS, \ + initiator_position = 115256, \ + scale = {'mm': 8000} ) + +axis_z = PhytronAxis( ipcomm[2], \ + max_run_freq = 500, \ + initiator = PhytronAxis.INITIATOR_PLUS, \ +# FIXME: Z zero position needs yet to be determined + initiator_position = 217952, \ + scale = {'mm': 8000} ) + +axis_phi = PhytronAxis( ipcomm[3], \ + max_run_freq = 1000, \ + initiator = PhytronAxis.INITIATOR_MINUS, \ + initiator_position = -273432, \ + scale = {'°': 1600} ) + +stage_axes = (axis_x, axis_y, axis_z, axis_phi) + +Service.stage = MotionStage(stage_axes) + +if __name__ == "__main__": + from rpyc.utils.server import ThreadedServer + server = ThreadedServer(Service, port = 12345) + server.start() + |