Added DC, setup, stow and disable (da1a3f44) · Commits · INAF / DISCOS

SRT/Servers/SRTMinorServo/src/SRTMinorServoTest/mscu/mscu.py

+7 −3

Original line number	Diff line number	Diff line
		@@ -2,6 +2,7 @@
		# Author: Marco Buttu <m.buttu@oa-cagliari.inaf.it>

		import socket
		import time
		import traceback
		import os
		import sys
		@@ -97,12 +98,15 @@ class MSCU(object):
		print "\nReceived message from %s: %r" % (connection.getpeername(), data)

		# Call the appropriate command whose name is the string cmd
		ans = getattr(servo, cmd)(cmd_num, *params)
		answers = getattr(servo, cmd)(cmd_num, *params)

		if not cmd in filtered:
		print "Answer from %s: %r" % (servo.name, ans)
		for answer in answers:
		print "Answer from %s: %r" % (servo.name, answer)

		connection.send('%s' %ans)
		for answer in answers:
		connection.send('%s' %answer)
		time.sleep(0.2)
		except (KeyboardInterrupt, SystemExit):
		raise
		except:

SRT/Servers/SRTMinorServo/src/SRTMinorServoTest/mscu/parameters.py

+7 −0

Original line number	Diff line number	Diff line
		@@ -45,6 +45,13 @@ axes = {
		'M3R': 1,
		}

		stow_position = {
		'PFP': [2730.15, 0, -195],
		'SRP': [-125, -125, -125, 0, 0, 0],
		'GFR': [0],
		'M3R': [0],
		}

		# Number of slaves of each minor servo
		number_of_slaves = {
		'PFP': 5,

SRT/Servers/SRTMinorServo/src/SRTMinorServoTest/mscu/servo.py

+103 −98

Original line number	Diff line number	Diff line
		from __future__ import division, with_statement
		import pdb
		import time
		import shelve
		from parameters import closers, app_nr, axes
		import operator
		from multiprocessing import Process, Value, Lock

		# REMOVE -------------------
		# Drive cabinet states
		dc_startup_state = 1
		dc_park_state = 3
		dc_ok = 0
		from parameters import closers, app_nr, axes, stow_position

		# Application states
		app_remote_auto = 4
		# END REMOVE ---------------

		class DriveCabinet(object):
		cab_state = {
		'ready': 0, # The servo ready to move
		'starting': 1, # The servo is performing a setup
		'block_removed': 2, # A setup is required
		'stow': 3, # Parked
		'power_failure': 4, # Power supply failure
		'disable': 5, # Disabled from other drive cabinet
		'reset_required': 6, # A clearemergency is required
		'blocked': 7, # Blocking condition
		}

		app_state = {}
		app_status = {}

		def __init__(self):
		self.cab_state = Value('i', DriveCabinet.cab_state['stow'])

		def set_state(self, state):
		self.cab_state.value = DriveCabinet.cab_state[state]


		class Servo(object):
		@@ -19,61 +36,57 @@ class Servo(object):
		self.id = address
		self.name = app_nr[self.id] # GRF, PFP, SRP, M3R
		self.axes = axes[self.name] # Number of axes
		self.history = History(self.name, self.axes)

		def set_app_state(self, value):
		self.app_state = int(value)

		def set_cab_state(self, value):
		self.cab_state = int(value)

		def set_app_status(self, value):
		self.app_status = int(value)
		self.stow_position = stow_position[self.name]
		self.history = History(self.axes)
		self.dc = DriveCabinet()
		self.stow(0)

		def getpos(self, cmd_num):
		data = self.history.get();
		answer = '?getpos' + ':%d=%d> %s' % (cmd_num, self.id, Servo.time())
		# Read the positions stored in a shelve db by a setpos command
		answer = '?getpos' + ':%d=%d> %s' % (cmd_num, self.id, Servo.ctime())
		# Read the position stored in a shelve db by a setpos command
		for position in data[1:]:
		answer += ',%s' %position
		else:
		answer += closers[0]

		return answer
		return [answer]

		def getappstatus(self, cmd_num):
		value = '0000030D'
		answer = '?getappstatus' + ':%d=%d> %s' %(cmd_num, self.id, value)
		return answer + closers[0]
		return [answer + closers[0]]

		def getstatus(self, cmd_num):
		# Ready
		app_state = app_remote_auto
		app_status = "FFFF"
		cab_state = dc_ok
		app_state = 4 # remote auto
		app_status = "FFFF" # Everything OK
		cab_state = self.dc.cab_state.value

		answer = '?getstatus' + ':%d=%d> ' % (cmd_num, self.id)
		answer += '%d,%d,%s,%d' %(Servo.time(), app_state, app_status, cab_state)
		answer += '%d,%d,%s,%d' %(Servo.ctime(), app_state, app_status, cab_state)

		# Read the positions stored in a shelve db by a setpos command
		# Read the position stored in a shelve db by a setpos command
		data = self.history.get()
		for position in data[1:]:
		answer += ',%s' %position
		else:
		answer += closers[0]
		return answer

		return [answer]

		def setpos(self, cmd_num, *params):
		positions = params[-self.axes:], params[0]
		self.history.insert(positions)
		timestamp, position = params[0], list(params[-self.axes:])
		self.history.insert(position, timestamp)
		answer = '@setpos' + ':%d=%d' %(cmd_num, self.id)
		for param in params:
		answer += ",%s" %param
		else:
		answer += closers[0]
		return answer
		return [answer.replace('@', '?'), answer]

		@staticmethod
		def _setup_process(cab_state):
		cab_state.value = DriveCabinet.cab_state['starting']
		time.sleep(5) # The setup takes 5 seconds
		cab_state.value = DriveCabinet.cab_state['ready']

		def setup(self, cmd_num, *params):
		answer = '@setup' + ':%d=%d' % (cmd_num, self.id)
		@@ -81,8 +94,9 @@ class Servo(object):
		answer += ",%s" %param
		else:
		answer += closers[0]
		return answer

		p = Process(target=Servo._setup_process, args=(self.dc.cab_state,))
		p.start()
		return [answer.replace('@', '?'), answer]

		def stow(self, cmd_num, *params):
		answer = '@stow' + ':%d=%d' % (cmd_num, self.id)
		@@ -90,7 +104,20 @@ class Servo(object):
		answer += ",%s" %param
		else:
		answer += closers[0]
		return answer
		self.dc.cab_state.value = DriveCabinet.cab_state['stow']
		self.history.insert(self.stow_position)
		return [answer.replace('@', '?'), answer]

		def disable(self, cmd_num, *params):
		answer = '@disable' + ':%d=%d' % (cmd_num, self.id)
		for param in params:
		answer += ",%s" %param
		else:
		answer += closers[0]
		self.dc.cab_state.value = DriveCabinet.cab_state['disable']
		self.history.insert(self.stow_position)
		return [answer.replace('@', '?'), answer]


		def clean(self, cmd_num, *params):
		answer = '@clean' + ':%d=%d' % (cmd_num, self.id)
		@@ -98,7 +125,7 @@ class Servo(object):
		answer += ",%s" %param
		else:
		answer += closers[0]
		return answer
		return [answer.replace('@', '?'), answer]

		def getspar(self, cmd_num, *params):
		answer = '?getspar' + ':%d=%d' % (cmd_num, self.id)
		@@ -108,72 +135,50 @@ class Servo(object):

		answer += '> %d' % (sum([ int(param) for param in params]))
		answer += closers[0]
		return answer
		return [answer]

		@staticmethod
		def time():
		"""Return the actual time in OMG format"""
		def ctime():
		"""Return the current time in OMG format"""
		acstime_ACE_BEGIN = 122192928000000000L
		return int(acstime_ACE_BEGIN + time.time() * 10000000L)


		class History(object):
		def __init__(self, servo_name, servo_axes):
		self.servo_name = servo_name
		self.servo_axes = servo_axes
		self.path = '/tmp/%s_positions.db' %servo_name
		db = shelve.open(self.path, writeback=True)
		db[servo_name] = [[Servo.time()] + [0]*servo_axes]
		db.close()

		def insert(self, positions, timestamp=None):
		target_time = timestamp if timestamp else Servo.time()
		db = shelve.open(self.path, writeback=True)
		list_of_lists = db[self.servo_name]
		index = None
		for idx, lst in enumerate(list_of_lists):
		if lst[0] <= target_time:
		if idx + 1 == len(list_of_lists):
		index = idx + 1
		break
		else:
		if list_of_lists[idx + 1][0] > target_time:
		index = idx + 1
		break
		else:
		continue
		else:
		index = 0
		break
		list_of_lists.insert(index, [target_time] + list(positions))
		db.close()


		def get(self, time_ref=Servo.time):
		"""Return the actual position as [timestamp, axis A, ..., axis N]"""
		timestamp = time_ref() if callable(time_ref) else time_ref
		db = shelve.open(self.path, writeback=True)
		list_of_lists = db[self.servo_name]
		index = None
		for idx, lst in enumerate(list_of_lists):
		if lst[0] <= timestamp:
		if idx + 1 == len(list_of_lists):
		index = idx
		break
		else:
		if list_of_lists[idx + 1][0] > timestamp:
		index = idx
		break
		lock = Lock()

		def __init__(self, axes):
		self.history = []
		self.insert(axes*[0])

		def insert(self, position, timestamp=None):
		target_time = timestamp if timestamp else Servo.ctime()
		data = [target_time] + list(position)
		with History.lock:
		self.history.append(data)
		self.history.sort(key=operator.itemgetter(0))
		self.history = self.history[-215:] # Last 215 positions

		def get(self, target_time=None):
		"""Return the position @target_time as [timestamp, axisA, ..., axisN]"""
		if target_time is None:
		target_time = Servo.ctime()
		size = len(self.history)
		idx = -1
		with History.lock:
		while idx >= -size:
		data = self.history[idx]
		timestamp, position = data[0], data[1:]
		if timestamp <= target_time:
		return data
		elif idx - 1 > -size:
		prev_timestamp = self.history[idx - 1]
		if prev_timestamp <= target_time:
		return self.history[idx - 1] # It's better to interpolate
		else:
		idx -= 1
		continue
		else:
		index = 0
		break
		value = list_of_lists[index]
		db[self.servo_name] = list_of_lists
		if callable(time_ref):
		db[self.servo_name] = list_of_lists[index::]
		db.update()
		db.close()
		return value
		return self.history[-size]
		pdb.set_trace()