things moved from scratchpad.py into their homes

1 files changed, 143 insertions, 44 deletions

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_()