From 835da32571c08259fc81c89f711047a80bc16a5b Mon Sep 17 00:00:00 2001 From: "Wolfgang Draxinger (root@gar-ex-erdastep)" Date: Tue, 6 Dec 2011 19:38:31 +0100 Subject: things moved from scratchpad.py into their homes --- scratchpad.py | 187 ++++++++++++++++++++++++++++++++++++++++++++-------------- 1 file changed, 143 insertions(+), 44 deletions(-) (limited to 'scratchpad.py') diff --git a/scratchpad.py b/scratchpad.py index 3dd62e6..fa76ddf 100644 --- a/scratchpad.py +++ b/scratchpad.py @@ -3,27 +3,34 @@ import Queue import threading -class Action: +class Action(object): + def __init__(self): + self.aborted = False + self.started = False + def execute(self): self.aborted = False + self.started = True self.do_execute() def ended(self): + if not self.started: + False if self.aborted: return True - return do_ended() + return self.do_ended() def abort(self): self.aborted = True self.do_abort() - def do_execute(): + def do_execute(self): raise NotImplementedError - def do_ended(): + def do_ended(self): raise NotImplementedError - def do_abort(): + def do_abort(self): pass class NullAction(Action): @@ -35,25 +42,26 @@ class NullAction(Action): class Initiate(Action): def __init__(self, axes): + Action.__init__(self) self.axes = axes def do_execute(self): - for a in self.axis: - a.initiate() - def do_ended(): + for axis in self.axes: + axis.initiate() + def do_ended(self): all_stopped = not (True in [axis.running for axis in self.axes]) - if not all_stopped: - return False - self.aborted = False in [axis.initialized for axis in self.axes] - return True + if all_stopped and False in [axis.initialized for axis in self.axes]: + raise CycleAbort() + return all_stopped - def do_abort(): + def do_abort(self): for axis in self.axes: axis.halt() class GotoAbsolute(Action): def __init__(self, axes, target, speed = None): + Action.__init__(self) if len(axes) != len(target): raise ValueError() if speed and len(speed) != len(axes): @@ -80,6 +88,7 @@ class GotoAbsolute(Action): class EmergencyStop(Action): def __init__(self, axes): + Action.__init__(self) self.axes = axes def do_execute(self): @@ -96,52 +105,115 @@ class CycleFinished(Exception): pass class MotionControl(object): - def __init__(self, axes, constraints): + def __init__(self, axes, constraints = None): + import Queue + self.axes = axes self.constraints = constraints - import weakref, threading - self.on_cycle_started = Signal() - self.on_cycle_finished = Signal() - self.on_cycle_aborted = Signal() + self.action_queue = Queue.Queue() + self.abort_action = NullAction() + + self.onCycleStarted = Signal() + self.onCycleFinished = Signal() + self.onCycleAborted = Signal() + + self.active = False + self.target = None def __del__(self): self.abort() - def can_cycle_start(self): - return True # FIXME: + def __getattr__(self, name): + if name == 'position': + return [axis.position for axis in self.axes] - def prepare_cycle(self): - pass + def update(self): + for axis in self.axes: + axis.update() + + def set_target(self, target): + if isinstance(target, list): + if len(target) != 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] + + speed = None + current_position = self.position + if not None in current_position: + delta = [abs(a-b) for a,b in zip(target, 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)) + + def can_cycle_start(self): + if self.active: + return False + return True # FIXME: Add constraint tests here def start_cycle(self): - if not self.can_cycle_start() + import threading, weakref + + if not self.can_cycle_start(): return False - self.prepare_cycle() + + 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.daemon =True self.worker_thread.start() - self.on_cycle_started.send() + self.onCycleStarted.send() def abort(self): self.active = False self.worker_thread.join() def __del__(self): - self.active = False + self.abort() def cycle_worker(ref): abort_action = ref.abort_action try: import time - while ref.active: - time.sleep(0.01) - ref.on_cycle_finished.send() - + while True: + if not ref.active: + raise CycleAbort() + ref.update() + if not ref.current_action or ref.current_action.ended(): + if ref.action_queue.empty(): + break + ref.current_action = ref.action_queue.get_nowait() + + ref.current_action.execute() + + while True: + if not ref.active: + raise CycleAbort() + ref.update() + if ref.current_action.ended(): + break + + ref.action_queue.task_done() + + ref.onCycleFinished.send() except CycleAbort: ref.abort_action.execute() - ref.on_cycle_aborted.send() + ref.onCycleAborted.send() + + finally: + try: + while not ref.action_queue.empty(): + ref.action_queue.get_nowait() + ref.action_queue.task_done() + except: + pass + ref.active = False import threading import rpyc @@ -207,10 +279,10 @@ class Axis(object): self.onInitiatorPlus.send(self, active = self.initiator_plus) if last_initiator_error != self.initiator_error: - self.onInitiatorError(self, error = self.initiator_error) + self.onInitiatorError.send(self, error = self.initiator_error) if last_temperature_warning != self.temperature_warning: - self.onTemperatureWarning(self, warning = self.temperature_warning) + self.onTemperatureWarning.send(self, warning = self.temperature_warning) def wait_for_stop(self): self.update() @@ -229,25 +301,25 @@ class Axis(object): class PhytronAxis(Axis): INITIATOR_MINUS = 1 INITIATOR_PLUS = 2 - def __init__(self, ipcomm_axis, max_run_freq=None, initiator=INITIATOR_MINUS, initiator_position = 0, inverted = False): - super(PhytronAxis, self).__init__() + def __init__(self, ipcomm_axis, max_run_freq=None, initiator=INITIATOR_MINUS, initiator_position = 0, inverted = False, scale={}): + super(PhytronAxis, self).__init__(inverted = inverted, scale = scale) + self.ipcomm_axis = ipcomm_axis if not max_run_freq: max_run_freq = self.ipcomm_axis.getRunFrequency() self.max_run_freq = max_run_freq + self.initialisation = False self.initiator = initiator self.initiator_position = initiator_position - self.onInitialized.connect(self.set_initator_position) + self.onInitialized.connect(self.handle_initialized) - def set_initator_position(self, sender, initialized): - if sender == self: + def handle_initialized(self, sender, initialized): + if self.initialisation and sender == self and initialized: self.ipcomm_axis.setPosition(self.initiator_position) def do_update(self): - if self.running: - self.pos = self.ipcomm_axis.getPosition() - status = self.ipcomm_axis.getFullStatus() + self.position = self.ipcomm_axis.getPosition() self.running = status.running self.initializing = status.initializing self.initialized = status.initialized @@ -267,10 +339,12 @@ class PhytronAxis(Axis): self.ipcomm_axis.initializeMinus() if self.initiator == PhytronAxis.INITIATOR_PLUS: self.ipcomm_axis.initializePlus() + self.initialisation = True def set_initiator_position(self, sender, initialized): if sender == self and initialized: self.ipcomm_axis.setPosition(self.initiator_position) + self.position = self.initator_position def goto_absolute(self, target, speed = None): self.ipcomm_axis.stop() @@ -280,8 +354,33 @@ class PhytronAxis(Axis): self.ipcomm_axis.setRunFrequency(self.max_run_freq * speed) self.ipcomm_axis.gotoAbs(target) + if __name__ == '__main__': + import sys + from PyQt4 import QtGui + + app = QtGui.QApplication(sys.argv) + lcd = QtGui.QLCDNumber() + lcd.setFrameStyle(QtGui.QFrame.Plain) + lcd.setSegmentStyle(QtGui.QLCDNumber.Flat) + lcd.setNumDigits(6) + lcd.setSmallDecimalPoint(False) + lcd.setDecMode() + lcd.show() + + def show_position(sender, position): + lcd.display("%.3f" % (float(position)/8000.,) ) + ipcomm = Phytron.IPCOMM('/dev/ttyERDASTEP', axes=5) - axis = PhytronAxis(ipcomm[0]) - action = MotionAbsolute([axis,], [200000,]) - motctl = MotionControl([axis,]) + axes = [PhytronAxis(ipcomm[0], initiator_position = -200000, max_run_freq=1500)] + axes[0].onPosition.connect(show_position) + moctl = MotionControl(axes) + moctl.abort_action = EmergencyStop(axes) + moctl.action_queue.put(Initiate(axes)) + moctl.start_cycle() + if 1: + moctl.action_queue.put(GotoAbsolute(axes, [0], [0.5])) + moctl.action_queue.put(GotoAbsolute(axes, [200000], [1])) + moctl.action_queue.put(GotoAbsolute(axes, [-100000], [0.75])) + + app.exec_() -- cgit v1.2.3