def setUp(self):
self.deferred = Deferred()
self.doneOrder = []
self.failOrder = []
self.cmdQueue = CommandQueue(
killFunc=self.killFunc,
priorityDict = cmdPriorityDict
)
self.commandsLeft = False
def __init__(self, name, host, port, callFunc=None):
"""!Construct a ScaleDevice
Inputs:
@param[in] name name of device
@param[in] host host address of scaling ring controller
@param[in] port port of scaling ring controller
@param[in] callFunc function to call when state of device changes;
note that it is NOT called when the connection state changes;
register a callback with "conn" for that task.
"""
# the mitutoyos will be zeroed when the scaling ring is moved to 20
# so this zero point keep track fo the offset.
# if I could preset the mitutoyo's this would be unnecessary
# the preset command "CP**" doesn't seem to work.
self.zeroPoint = 20.0 # mm. Position where scale = 1
self.encPos = [None]*6
self.devCmdQueue = CommandQueue({})
TCPDevice.__init__(self,
name = name,
host = host,
port = port,
callFunc = callFunc,
cmdInfo = (),
)
def setupCmdQueue(self):
cmdQueue = CommandQueue(
priorityDict = {
"init" : CommandQueue.Immediate,
# all other commands have an equal (default) priority
}
)
return cmdQueue
def __init__(self, name, host, port, measScaleDev=None, nomSpeed=NOM_SPEED, callFunc=None):
"""!Construct a ScaleDevice
Inputs:
@param[in] name name of device
@param[in] host host address of scaling ring controller
@param[in] port port of scaling ring controller
@param[in] measScaleDev instance of a MeasScaleDev (access to the mitutoyos for servoing)
@param[in] nom_speed nominal speed at which to move (this can be modified via the speed command)
@param[in] callFunc function to call when state of device changes;
note that it is NOT called when the connection state changes;
register a callback with "conn" for that task.
"""
self.targetPos = None
# holds a userCommand for "move"
# set done only when move has reached maxIter
# or is within tolerance
self.iter = 0
self.moveUserCmd = expandUserCmd(None)
self.moveUserCmd
self.nomSpeed = nomSpeed
self.measScaleDev = measScaleDev
self.status = Status()
# all commands of equal priority
# except stop kills a running (or pending move) move
# priorityDict = {"stop": CommandQueue.Immediate}
priorityDict = {
"stop":1,
"status":1,
"move":1,
"speed":1,
}
self.devCmdQueue = CommandQueue(
priorityDict,
killFunc = self.killFunc,
)
# stop will kill a running move
# else everything queues with equal prioirty
self.devCmdQueue.addRule(CommandQueue.KillRunning, ["stop"], ["move"])
TCPDevice.__init__(self,
name = name,
host = host,
port = port,
callFunc = callFunc,
cmdInfo = (),
)
def __init__(self, name, host, port, callFunc=None):
"""!Construct a LCODevice
Inputs:
@param[in] name name of device
@param[in] host host address of tcs controller
@param[in] port port of tcs controller
@param[in] callFunc function to call when state of device changes;
note that it is NOT called when the connection state changes;
register a callback with "conn" for that task.
"""
self.status = Status()
self._statusTimer = Timer()
self.waitRotCmd = UserCmd()
self.waitRotCmd.setState(self.waitRotCmd.Done)
self.waitRotTimer = Timer()
# self.waitFocusCmd = UserCmd()
# self.waitFocusCmd.setState(self.waitFocusCmd.Done)
self.waitOffsetCmd = UserCmd()
self.waitOffsetCmd.setState(self.waitOffsetCmd.Done)
self.waitOffsetTimer = Timer()
self.rotDelay = False
self.devCmdQueue = CommandQueue({}) # all commands of equal priority
self.lastGuideRotApplied = None
self.doGuideRot = True
TCPDevice.__init__(self,
name = name,
host = host,
port = port,
callFunc = callFunc,
cmdInfo = (),
)
def __init__(self, name, host, port, callFunc=None):
"""!Construct a LCODevice
Inputs:
@param[in] name name of device
@param[in] host host address of m2 controller
@param[in] port port of m2 controller
@param[in] callFunc function to call when state of device changes;
note that it is NOT called when the connection state changes;
register a callback with "conn" for that task.
"""
self.status = Status()
self._statusTimer = Timer()
self.waitMoveCmd = UserCmd()
self.waitMoveCmd.setState(self.waitMoveCmd.Done)
# self.waitGalilCmd = UserCmd()
# self.waitGalilCmd.setState(self.waitGalilCmd.Done)
# give status commands equal priority so they don't kill eachother
priorityDict = {
"status": 1,
"speed": 1,
"stop": 1,
"move": 1,
"galil": 1,
"offset": 1,
}
self.devCmdQueue = CommandQueue(priorityDict) # all commands of equal priority
TCPDevice.__init__(self,
name = name,
host = host,
port = port,
callFunc = callFunc,
cmdInfo = (),
)
class CmdQueueTest(unittest.TestCase):
"""Unit test for CommandQueue
"""
def setUp(self):
self.deferred = Deferred()
self.doneOrder = []
self.failOrder = []
self.cmdQueue = CommandQueue(
killFunc=self.killFunc,
priorityDict = cmdPriorityDict
)
self.commandsLeft = False
def tearDown(self):
self.deferred = None
self.doneOrder = []
self.failOrder = []
self.cmdQueue = None
def addCmdsToQueue(self, cmdStrList, timeStep=0.02):
# add commands to the queue every timeStep seconds
cmdStrListCopy = cmdStrList[:]
self.commandsLeft = True
def addInOrder():
if not cmdStrListCopy:
# all commands have been added to the queue
self.commandsLeft = False
return
else:
cmdStr = cmdStrListCopy.pop(0)
self.cmdQueue.addCmd(self.makeCmd(cmdStr), nullCallFunc)
# print 'adding', cmdStr
# print 'command queue!', [x.cmdVerb for x in self.cmdQueue.cmdQueue]
Timer(timeStep, addInOrder)
addInOrder()
def addToQueue(self, cmdStr):
self.cmdQueue.addCmd(self.makeCmd(cmdStr), nullCallFunc)
# Timer(0., self.cmdQueue.addCmd, self.makeCmd(cmdStr), nullCallFunc)
def cmdCallback(self, cmd):
# allow commands to run for 0.15 seconds before killing
# print cmd.cmdVerb, cmd.state
# print 'q: ', [x.cmd.cmdVerb for x in self.cmdQueue.cmdQueue]
if cmd.isDone:
# print 'cmd %s finished with state %s and txtMsg=%s'%(cmd.cmdVerb, cmd.state, cmd.textMsg)
if cmd.didFail:
self.failOrder.append(cmd.cmdVerb)
else:
self.doneOrder.append(cmd.cmdVerb)
elif cmd.isActive:
Timer(0.15, self.setDone, cmd)
if (not self.cmdQueue.cmdQueue) and (self.cmdQueue.currExeCmd.cmd.isDone) and not self.commandsLeft: # must be last command and its done
# the queue is empty and last command is done, end the test
self.deferred.callback('go')
def killFunc(self, killMe, killedBy):
killMe.setState(killMe.Cancelled)
def setDone(self, cmd):
if not cmd.isDone:
cmd.setState(cmd.Done)
def makeCmd(self, cmd):
newCmd = UserCmd(userID = 0, cmdStr = cmd)
newCmd.cmdVerb = cmd
newCmd.addCallback(self.cmdCallback)
return newCmd
def testNoRules(self):
cmdsIn = ['hia', 'hib', 'meda', 'medb', 'lowa', 'lowb']
def checkResults(cb):
self.assertEqual(cmdsIn, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testPrioritySort(self):
cmdsIn = ['hia', 'meda', 'lowa', 'lowb', 'medb', 'hib']
cmdsOut = ['hia', 'hib', 'meda', 'medb', 'lowa', 'lowb']
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testLowPriorityFirstIn(self):
cmdsIn = ['lowb', 'meda', 'lowa']
def checkResults(cb):
self.assertEqual(cmdsIn, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testImmediate(self):
cmdsIn = ['hia', 'meda', 'lowa', 'lowb', 'medb', 'hib', 'killa']
cmdsOut = ['killa']
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testDoubleImmediate(self):
cmdsIn = ['hia', 'meda', 'lowa', 'lowb', 'medb', 'hib', 'killa', 'killb']
cmdsOut = ['killb']
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testKillRule(self):
cmdsIn = ['meda', 'medb']
cmdsOut = ['medb']
self.cmdQueue.addRule(
action = CommandQueue.KillRunning,
newCmds = ['medb'],
queuedCmds = ['meda'],
)
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testSupersede(self):
cmdsIn = ['hia', 'hib', 'meda', 'medb', 'meda']
cmdsOut = ['hia', 'hib', 'medb', 'meda']
self.cmdQueue.addRule(
action = CommandQueue.CancelQueued,
newCmds = ['meda'],
queuedCmds = ['meda'],
)
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testQueueSame(self):
cmdsIn = ['hia', 'hia']
cmdsOut = ['hia', 'hia']
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testCancelNewRule(self):
cmdsIn = ['meda', 'hib', 'meda', 'medb']
cmdsOut = ['meda', 'hib', 'meda']
self.cmdQueue.addRule(
action = CommandQueue.CancelNew,
newCmds = ['medb'],
queuedCmds = ['meda'],
)
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testCancelQueuedRule(self):
cmdsIn = ['meda', 'hib', 'meda', 'medb']
cmdsOut = ['meda', 'hib', 'medb']
self.cmdQueue.addRule(
action = CommandQueue.CancelQueued,
newCmds = ['medb'],
queuedCmds = ['meda'],
)
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testBadRule(self):
try:
self.cmdQueue.addRule(
action = 'badRule',
newCmds = ['medb'],
queuedCmds = ['meda'],
)
except RuntimeError:
self.assertTrue(True)
else:
self.assertTrue(False)
def testRuleUnrecognizedCmd(self):
try:
self.cmdQueue.addRule(
action = CommandQueue.CancelNew,
newCmds = ['randomCmd'],
queuedCmds = ['meda'],
)
except RuntimeError:
self.assertTrue(True)
else:
self.assertTrue(False)
def testStartUnrecognizedCmd(self):
self.addToQueue('randomCmd')
def checkResults(cb):
self.assertEqual(['randomCmd'], self.doneOrder)
self.deferred.addCallback(checkResults)
return self.deferred
def testUnrecognizedCmdSort(self):
cmdsIn = ['hia', 'randomCmd', 'meda', 'lowa', 'lowb', 'medb', 'hib']
cmdsOut = ['hia', 'hib', 'meda', 'medb', 'lowa', 'lowb', 'randomCmd']
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testCancelAllQueuedRule(self):
cmdsIn = ['meda', 'hib', 'meda', 'medb']
cmdsOut = ['meda', 'medb']
self.cmdQueue.addRule(
action = CommandQueue.CancelQueued,
newCmds = ['medb'],
queuedCmds = 'all',
)
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testCancelNewRuleRunning(self):
cmdsIn = ['meda', 'medb']
cmdsOut = ['meda']
self.cmdQueue.addRule(
action = CommandQueue.CancelNew,
newCmds = ['medb'],
queuedCmds = ['meda'],
)
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testCancelAllNewRule(self):
cmdsIn = ['meda', 'medb', 'randomCmd', 'hib', 'meda', 'medb']
cmdsOut = ['meda']
self.cmdQueue.addRule(
action = CommandQueue.CancelNew,
newCmds = 'all',
queuedCmds = ['meda'],
)
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testCancelAllQueuedRule(self):
cmdsIn = ['meda', 'medb', 'randomCmd', 'hib', 'meda', 'medb']
cmdsOut = ['meda', 'medb']
self.cmdQueue.addRule(
action = CommandQueue.CancelQueued,
newCmds = ['medb'],
queuedCmds = 'all',
)
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testCancelAllQueuedAndCancelNewRule(self):
cmdsIn = ['hia', 'medb', 'randomCmd', 'hib', 'meda', 'medb']
cmdsOut = ['hia', 'hib', 'medb', 'meda', 'randomCmd']
self.cmdQueue.addRule(
action = CommandQueue.CancelQueued,
newCmds = ['medb'],
queuedCmds = 'all',
)
self.cmdQueue.addRule(
action = CommandQueue.CancelNew,
newCmds = ['medb'],
queuedCmds = ['meda'],
)
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
# for cmd in cmdsIn:
# self.addToQueue(cmd)
return self.deferred
def testOverDefinition(self):
self.cmdQueue.addRule(
action = CommandQueue.CancelNew,
newCmds = ['medb'],
queuedCmds = 'all',
)
try:
self.cmdQueue.addRule(
action = CommandQueue.KillRunning,
newCmds = 'all',
queuedCmds = ['medb'],
)
except RuntimeError as e:
print e
self.assertTrue(True)
else:
self.assertTrue(False)
def testAllowedDoubleAllDefinition(self):
self.cmdQueue.addRule(
action = CommandQueue.CancelNew,
newCmds = ['medb'],
queuedCmds = 'all',
)
try:
self.cmdQueue.addRule(
action = CommandQueue.CancelNew,
newCmds = 'all',
queuedCmds = ['medb'],
)
except RuntimeError as e:
print e
self.assertTrue(False)
else:
self.assertTrue(True)
def testCancelAllNewRule(self):
cmdsIn = ['meda', 'randx', 'randx']
cmdsOut = ['meda', 'randx']
def checkResults(cb):
self.assertEqual(cmdsOut, self.doneOrder)
self.deferred.addCallback(checkResults)
self.addCmdsToQueue(cmdsIn)
return self.deferred
class TCSDevice(TCPDevice):
"""!A Device for communicating with the LCO TCS."""
def __init__(self, name, host, port, callFunc=None):
"""!Construct a LCODevice
Inputs:
@param[in] name name of device
@param[in] host host address of tcs controller
@param[in] port port of tcs controller
@param[in] callFunc function to call when state of device changes;
note that it is NOT called when the connection state changes;
register a callback with "conn" for that task.
"""
self.status = Status()
self._statusTimer = Timer()
self.waitRotCmd = UserCmd()
self.waitRotCmd.setState(self.waitRotCmd.Done)
self.waitRotTimer = Timer()
# self.waitFocusCmd = UserCmd()
# self.waitFocusCmd.setState(self.waitFocusCmd.Done)
self.waitOffsetCmd = UserCmd()
self.waitOffsetCmd.setState(self.waitOffsetCmd.Done)
self.waitOffsetTimer = Timer()
self.rotDelay = False
self.devCmdQueue = CommandQueue({}) # all commands of equal priority
self.lastGuideRotApplied = None
self.doGuideRot = True
TCPDevice.__init__(self,
name = name,
host = host,
port = port,
callFunc = callFunc,
cmdInfo = (),
)
@property
def currExeDevCmd(self):
return self.devCmdQueue.currExeCmd.cmd
@property
def currDevCmdStr(self):
return self.currExeDevCmd.cmdStr
# @property
# def atFocus(self):
# if self.status.statusFieldDict["focus"].value and abs(self.targFocus - self.status.statusFieldDict["focus"].value) < FocusPosTol:
# return True
# else:
# return False
@property
def isTracking(self):
if not self.waitOffsetCmd.isDone:
# offsets do not trigger tcs tracking state, so we fake it here
return False
return self.status.statusFieldDict["state"].value == Tracking
@property
def isSlewing(self):
if not self.waitOffsetCmd.isDone or not self.status.isClamped:
# if clamp is not on, then we are moving the rotator
return True
else:
return False
def init(self, userCmd=None, timeLim=None, getStatus=True):
"""Called automatically on startup after the connection is established.
Only thing to do is query for status or connect if not connected
"""
log.info("%s.init(userCmd=%s, timeLim=%s, getStatus=%s)" % (self, userCmd, timeLim, getStatus))
# print("%s.init(userCmd=%s, timeLim=%s, getStatus=%s)" % (self, userCmd, timeLim, getStatus))
userCmd = expandUserCmd(userCmd)
# if not self.isConnected:
# # time lim handled by lco.deviceCmd
# return self.connect(userCmd=userCmd)
if getStatus:
return self.getStatus(userCmd=userCmd)
else:
userCmd.setState(userCmd.Done)
return userCmd
def getStatus(self, userCmd=None):
"""Return current telescope status. Continuously poll.
"""
log.info("%s.getStatus(userCmd=%s)" % (self, userCmd)) # logging this will flood the log
userCmd = expandUserCmd(userCmd)
if not self.conn.isConnected:
userCmd.setState(userCmd.Failed, "Not Connected to TCS")
return userCmd
self._statusTimer.cancel() # incase a status is pending
userCmd = expandUserCmd(userCmd)
userCmd.addCallback(self._statusCallback)
# record the present RA, DEC (for determining when offsets are done)
# self.status.previousRA = self.status.statusFieldDict["ra"].value
# self.status.previousDec = self.status.statusFieldDict["dec"].value
if self.status.statusFieldDict["mpos"].value is None:
self.status.previousRA, self.status.previousDec = None, None
else:
self.status.previousRA = self.status.statusFieldDict["mpos"].value[0]
self.status.previousDec = self.status.statusFieldDict["mpos"].value[1]
# gather list of status elements to get
devCmdList = [DevCmd(cmdStr=cmdVerb) for cmdVerb in self.status.statusFieldDict.keys()]
LinkCommands(userCmd, devCmdList)
for devCmd in devCmdList:
self.queueDevCmd(devCmd)
if not self.status.isClamped or self.rotDelay:
# rotator is moving, get status frequently
pollTime = PollTimeRot
elif self.isSlewing:
# slewing, get status kinda frequently
pollTime = PollTimeSlew
elif self.isTracking:
# tracking, get status less frequently
pollTime = PollTimeTrack
else:
# idle, get status infrequently (as things shouldn't be changing fast)
pollTime = PollTimeIdle
self._statusTimer.start(pollTime, self.getStatus)
return userCmd
def _statusCallback(self, cmd):
"""! When status command is complete, send info to users, and check if any
wait commands need to be set done
"""
# print("tcs status callback", cmd)
if cmd.isDone and not cmd.didFail:
# do we want status output so frequently? probabaly not.
# perhaps only write status if it has changed...
statusStr = self.status.getStatusStr()
if statusStr:
self.writeToUsers("i", statusStr, cmd)
if self.waitOffsetCmd.isActive and not True in self.status.axesSlewing():
self.waitOffsetCmd.setState(self.waitOffsetCmd.Done)
if self.waitRotCmd.isActive and not self.rotDelay and self.status.isClamped: #not self.status.rotMoving: #and self.status.rotOnTarget :
print("set rot command done", self.rotDelay, self.status.isClamped, self.status.rotMoving)
self.waitRotCmd.setState(self.waitRotCmd.Done)
def target(self, ra, dec, doHA, userCmd=None):
"""Set coordinates for a slew.
@param[in] ra: right ascension decimal degrees
@param[in] dec: declination decimal degrees
@param[in] doHA: if True, use degrees in hour angle rather than ra.
@param[in] userCmd: a twistedActor BaseCommand.
"""
log.info("%s.slew(userCmd=%s, ra=%.2f, dec=%.2f)" % (self, userCmd, ra, dec))
userCmd = expandUserCmd(userCmd)
if not self.conn.isConnected:
userCmd.setState(userCmd.Failed, "Not Connected to TCS")
return userCmd
if doHA:
enterRa = "HAD %.8f"%ra
else:
enterRa = "RAD %.8f"%ra
enterDec = "DECD %.8f"%dec
enterEpoch = "MP %.2f"%2000 # LCO: HACK should coords always be 2000?
devCmdList = [DevCmd(cmdStr=cmdStr) for cmdStr in [enterRa, enterDec, enterEpoch]]#, cmdSlew]]
# set userCmd done only when each device command finishes
# AND the pending slew is also done.
# when the last dev cmd is done (the slew), set axis cmd statue to slewing
LinkCommands(userCmd, devCmdList) #LCO: HACK don't wait for a slew to finish + [self.waitSlewCmd])
for devCmd in devCmdList:
self.queueDevCmd(devCmd)
statusStr = self.status.getStatusStr()
if statusStr:
self.writeToUsers("i", statusStr, userCmd)
return userCmd
def slewOffset(self, ra, dec, userCmd=None):
"""Offset telescope in right ascension and declination.
@param[in] ra: right ascension in decimal degrees
@param[in] dec: declination in decimal degrees
@param[in] userCmd a twistedActor BaseCommand
@todo, consolidate similar code with self.target?
"""
log.info("%s.slewOffset(userCmd=%s, ra=%.6f, dec=%.6f)" % (self, userCmd, ra, dec))
userCmd = expandUserCmd(userCmd)
# zero the delta computation so the offset isn't marked done immediately
self.status.previousDec = ForceSlew
self.status.previousRA = ForceSlew
if not self.conn.isConnected:
userCmd.setState(userCmd.Failed, "Not Connected to TCS")
return userCmd
if not self.waitOffsetCmd.isDone:
self.waitOffsetCmd.setState(self.waitOffsetCmd.Cancelled, "Superseded by new offset")
waitOffsetCmd = UserCmd()
self.waitOffsetCmd = waitOffsetCmd
enterRa = "OFRA %.8f"%(ra*ArcSecPerDeg)
enterDec = "OFDC %.8f"%(dec*ArcSecPerDeg) #lcohack
devCmdList = [DevCmd(cmdStr=cmdStr) for cmdStr in [enterRa, enterDec, CMDOFF]]
# set userCmd done only when each device command finishes
# AND the pending slew is also done.
# set an offset done after 6 seconds no matter what
def setWaitOffsetCmdDone(aWaitingOffsetCmd):
print("wait offset command state", aWaitingOffsetCmd.state)
if not aWaitingOffsetCmd.isDone:
print("Wait offset timed out!!!!")
self.writeToUsers("w", "Text=OFFSET SET DONE ON TIMER.")
aWaitingOffsetCmd.setState(aWaitingOffsetCmd.Done, "offset set done on a timer")
self.waitOffsetTimer.start(8, setWaitOffsetCmdDone, waitOffsetCmd)
# self.waitOffsetCmd.setTimeLimit(6)
LinkCommands(userCmd, devCmdList + [self.waitOffsetCmd])
for devCmd in devCmdList:
self.queueDevCmd(devCmd)
statusStr = self.status.getStatusStr()
if statusStr:
self.writeToUsers("i", statusStr, userCmd)
return userCmd
def rotOffset(self, rot, userCmd=None, force=False):
"""Offset telescope rotator. USE APGCIR cmd
which holds current
@param[in] rot: in decimal degrees
@param[in] userCmd a twistedActor BaseCommand
"""
# LCOHACK: allow offsets only if over 5 arcseconds!
# if True:
# #! lupton!
# userCmd = expandUserCmd(userCmd)
# self.writeToUsers("w", "Rotator offset %.6f bypassed"%rot)
# userCmd.setState(userCmd.Done)
# return userCmd
if not self.conn.isConnected:
userCmd.setState(userCmd.Failed, "Not Connected to TCS")
return userCmd
if not self.waitRotCmd.isDone:
# rotator is unclamped, a move is in progress
userCmd.setState(userCmd.Failed, "Rotator is unclamped (already moving?)")
return userCmd
# if abs(rot) < MinRotOffset and not force:
if not self.doGuideRot:
# set command done, rotator offset is miniscule
self.writeToUsers("w", "Guide rot not enabled, not applying", userCmd)
userCmd.setState(userCmd.Done)
return userCmd
if abs(rot) > MaxRotOffset:
# set command failed, rotator offset is too big
self.writeToUsers("w", "Rot offset greater than max threshold", userCmd)
userCmd.setState(userCmd.Failed, "Rot offset %.4f > %.4f"%(rot, MaxRotOffset))
return userCmd
### print time since last rot applied from guider command
if not force:
if self.lastGuideRotApplied is None:
self.lastGuideRotApplied = time.time()
else:
tnow = time.time()
infoStr = "time since last guide rot update: %.2f"%(tnow-self.lastGuideRotApplied)
print(infoStr)
log.info(infoStr)
self.lastGuideRotApplied = tnow
# apgcir requires absolute position, calculate it
# first get status
newPos = self.status.rotPos - rot
# rotStart = time.time()
# def printRotSlewTime(aCmd):
# if aCmd.isDone:
# rotTime = time.time() - rotStart
# print("rot: off, time, speed: %.5f %.5f %5f"%(newPos, rotTime, newPos/rotTime))
waitRotCmd = UserCmd()
self.waitRotCmd = waitRotCmd
# calculate time limit for rot move:
rotTimeLimBuffer = 2 # check for clamp after 4 seconds
self.rotDelay = True
print("setting rot delay true")
def setRotBufferOff():
print("setting rot delay false")
print("rot buffer Off (clamped?)", self.status.isClamped)
self.rotDelay = False
self.waitRotTimer.start(rotTimeLimBuffer, setRotBufferOff)
self.waitOffsetCmd.setTimeLimit(rotTimeLimBuffer + 20)
self.status.setRotOffsetTarg(rot)
enterAPGCIR = DevCmd(cmdStr="APGCIR %.8f"%(newPos))
LinkCommands(userCmd, [enterAPGCIR, self.waitRotCmd])
# begin the dominos game
self.queueDevCmd(enterAPGCIR)
statusStr = self.status.getStatusStr()
if statusStr:
self.writeToUsers("i", statusStr, userCmd)
return userCmd
def handleReply(self, replyStr):
"""Handle a line of output from the device. Called whenever the device outputs a new line of data.
@param[in] replyStr the reply, minus any terminating \n
Tasks include:
- Parse the reply
- Manage the pending commands
- Output data to users
- Parse status to update the model parameters
- If a command has finished, call the appropriate command callback
"""
log.info("%s read %r, currCmdStr: %s" % (self, replyStr, self.currDevCmdStr))
replyStr = replyStr.strip()
if replyStr == "-1":
# error
errorStr = "handleReply failed for %s with -1"%self.currDevCmdStr
if self.waitOffsetCmd.isActive:
self.waitOffsetCmd.setState(self.waitOffsetCmd.Failed, errorStr)
if self.waitRotCmd.isActive:
# note the clamp should still execute!!!!
self.waitRotCmd.setState(self.waitRotCmd.Failed, errorStr)
self.currExeDevCmd.setState(self.currExeDevCmd.Failed, errorStr)
return
statusField = self.status.statusFieldDict.get(self.currDevCmdStr, None)
if statusField:
# a piece of status was requested, parse it
# how to check for error condition? parsing -1 as a float will still work.
statusField.setValue(replyStr)
self.currExeDevCmd.setState(self.currExeDevCmd.Done)
elif replyStr == "0":
# this was a command, a "0" is expected
self.currExeDevCmd.setState(self.currExeDevCmd.Done)
else:
log.info("unexpected reply" % replyStr)
#self.currExeDevCmd.setState(self.currExeDevCmd.Failed, "Unexpected reply %s for %s"%(replyStr, self.currDevCmdStr))
def queueDevCmd(self, devCmd):
"""Add a device command to the device command queue
@param[in] devCmd: a twistedActor DevCmd.
"""
log.info("%s.queueDevCmd(devCmd=%r, devCmdStr=%r, cmdQueue: %r"%(self, devCmd, devCmd.cmdStr, self.devCmdQueue))
# append a cmdVerb for the command queue (other wise all get the same cmdVerb and cancel eachother)
# could change the default behavior in CommandQueue?
devCmd.cmdVerb = devCmd.cmdStr
def queueFunc(devCmd):
# all tcs commands return immediately so set a short timeout
devCmd.setTimeLimit(SEC_TIMEOUT)
devCmd.setState(devCmd.Running)
self.startDevCmd(devCmd.cmdStr)
self.devCmdQueue.addCmd(devCmd, queueFunc)
def startDevCmd(self, devCmdStr):
"""
@param[in] devCmdStr a line of text to send to the device
"""
devCmdStr = devCmdStr.upper() # lco uses all upper case
log.info("%s.startDevCmd(%r)" % (self, devCmdStr))
try:
if self.conn.isConnected:
log.info("%s writing %r" % (self, devCmdStr))
if CMDOFF.upper() == devCmdStr:
self.waitOffsetCmd.setState(self.waitOffsetCmd.Running)
elif "CIR" in devCmdStr:
self.waitRotCmd.setState(self.waitRotCmd.Running)
self.conn.writeLine(devCmdStr)
else:
self.currExeDevCmd.setState(self.currExeDevCmd.Failed, "Not connected to TCS")
except Exception as e:
self.currExeDevCmd.setState(self.currExeDevCmd.Failed, textMsg=strFromException(e))
class ScaleDevice(TCPDevice):
"""!A Device for communicating with the LCO Scaling ring."""
validCmdVerbs = ["move", "stop", "status", "speed"]
def __init__(self, name, host, port, measScaleDev=None, nomSpeed=NOM_SPEED, callFunc=None):
"""!Construct a ScaleDevice
Inputs:
@param[in] name name of device
@param[in] host host address of scaling ring controller
@param[in] port port of scaling ring controller
@param[in] measScaleDev instance of a MeasScaleDev (access to the mitutoyos for servoing)
@param[in] nom_speed nominal speed at which to move (this can be modified via the speed command)
@param[in] callFunc function to call when state of device changes;
note that it is NOT called when the connection state changes;
register a callback with "conn" for that task.
"""
self.targetPos = None
# holds a userCommand for "move"
# set done only when move has reached maxIter
# or is within tolerance
self.iter = 0
self.moveUserCmd = expandUserCmd(None)
self.moveUserCmd
self.nomSpeed = nomSpeed
self.measScaleDev = measScaleDev
self.status = Status()
# all commands of equal priority
# except stop kills a running (or pending move) move
# priorityDict = {"stop": CommandQueue.Immediate}
priorityDict = {
"stop":1,
"status":1,
"move":1,
"speed":1,
}
self.devCmdQueue = CommandQueue(
priorityDict,
killFunc = self.killFunc,
)
# stop will kill a running move
# else everything queues with equal prioirty
self.devCmdQueue.addRule(CommandQueue.KillRunning, ["stop"], ["move"])
TCPDevice.__init__(self,
name = name,
host = host,
port = port,
callFunc = callFunc,
cmdInfo = (),
)
def _addMeasScaleDev(self, measScaleDev):
"""Add a way to add a measScaleDev exposfacto,
really this is only for use with the device wrappers.
Any real use should specify measScaleDev in __init__
"""
self.measScaleDev = measScaleDev
def killFunc(self, doomedCmd, killerCmd):
doomedCmd.setState(doomedCmd.Failed, "Killed by %s"%(str(killerCmd)))
@property
def motorPos(self):
"""Position reported by the motor (Keithly)
"""
return self.status.position
@property
def encPos(self):
"""Average position of the 3 mitutoyo encoders
"""
return self.measScaleDev.position
@property
def scaleZeroPos(self):
return self.measScaleDev.zeroPoint
@property
def currExeDevCmd(self):
return self.devCmdQueue.currExeCmd.cmd
@property
def currDevCmdStr(self):
return self.currExeDevCmd.cmdStr
@property
def isMoving(self):
return self.moveUserCmd.isActive
def init(self, userCmd=None, timeLim=None, getStatus=False):
"""Called automatically on startup after the connection is established.
Only thing to do is query for status or connect if not connected
"""
log.info("%s.init(userCmd=%s, timeLim=%s, getStatus=%s)" % (self, userCmd, timeLim, getStatus))
userCmd = expandUserCmd(userCmd)
# stop, set speed, then status?
stopCmd = self.queueDevCmd("stop", userCmd)
speedCmd = self.queueDevCmd("speed %.4f"%self.nomSpeed, userCmd)
statusCmd = self.queueDevCmd("status", userCmd)
LinkCommands(userCmd, [stopCmd, speedCmd, statusCmd])
return userCmd
# if getStatus:
# return self.getStatus(userCmd=userCmd)
# else:
# userCmd.setState(userCmd.Done)
# return userCmd
def getStatus(self, userCmd=None, timeLim=None, linkState=True):
"""!Get status of the device. If the device is
busy (eg moving), send the cached status
note that during moves the thread_ring_axis actual_position gets
periodically output and thus updated in the status
"""
# note measScaleDevice could be read even if
# scaling ring is moving. do this at somepoint?
userCmd = expandUserCmd(userCmd)
if timeLim is None:
timeLim = 2
if self.isMoving:
self.writeToUsers("i", "text=showing cached status", userCmd)
self.writeStatusToUsers(userCmd)
userCmd.setState(userCmd.Done)
else:
# get a completely fresh status from the device
statusDevCmd = self.queueDevCmd("status", userCmd)
# get encoder values too
encStatusDevCmd = self.measScaleDev.getStatus()
statusDevCmd.addCallback(self._statusCallback)
statusDevCmd.setTimeLimit(timeLim)
if linkState:
LinkCommands(userCmd, [statusDevCmd, encStatusDevCmd])
return userCmd
else:
# return the device command to be linked outside
return statusDevCmd
def _statusCallback(self, statusCmd):
# if statusCmd.isActive:
# # not sure this is necessary
# # but ensures we get a 100% fresh status
# self.status.flushStatus()
if statusCmd.isDone and not statusCmd.didFail:
# write the status we have to users
# if this was a status, write output to users
# and set the current axis back to the thread ring
self.status.setThreadAxisCurrent()
# full status is checked in handleReply, and
# another status is commanded if the current status
# is munged
# statusError = self.status.checkFullStatus()
# if statusError:
# print("status Error")
# self.writeToUsers("w", statusError, statusCmd.userCmd)
self.writeStatusToUsers(statusCmd.userCmd)
def getStateKW(self):
# determine time remaining in this state
timeElapsed = time.time() - self.status._timeStamp
# cannot have negative time remaining
timeRemaining = max(0, self.status._totalTime - timeElapsed)
return "ThreadringState=%s, %i, %i, %.2f, %.2f"%(
self.status._state, self.status.currIter, self.status.maxIter, timeRemaining, self.status._totalTime
)
def getFaultStr(self):
faultList = []
for axis, val in self.status.dict.iteritems():
if hasattr(val, "iteritems"):
for key, value in val.iteritems():
if "_fault" in key and bool(value):
# fault value is non zero or not None
faultList.append("%s %s %i"%(axis, key, val))
if not faultList:
# no faults
return None
else:
faultStr = ",".join(faultList)
return "ScaleRingFaults=%s"%faultStr
def statusStr(self):
kwList = []
threadRingPos = "%.4f"%self.encPos if self.encPos else "nan"
desThreadRingPos = "%.4f"%self.targetPos if self.targetPos else "nan"
kwList.append("ThreadRingMotorPos=%.4f"%self.motorPos)
kwList.append("ThreadRingEncPos=%s"%threadRingPos)
kwList.append("ThreadRingSpeed=%.4f"%self.status.speed)
kwList.append("ThreadRingMaxSpeed=%.4f"%self.status.maxSpeed)
kwList.append("DesThreadRingPos=%s"%desThreadRingPos)
kwList.append("ScaleZeroPos=%.4f"%self.scaleZeroPos)
kwList.append("instrumentNum=%i"%21)#21)#self.status.cartID)
kwList.append("CartLocked=%s"%"T" if self.status.locked else "F")
kwList.append("CartLoaded=%s"%"T" if self.status.loaded else "F")
return "; ".join(kwList)
def writeStatusToUsers(self, userCmd=None):
"""Write the current status to all users
"""
faultStr = self.getFaultStr()
if faultStr is not None:
self.writeToUsers("w", faultStr, userCmd)
statusError = self.status.checkFullStatus()
if statusError:
self.writeToUsers("w", statusError)
statusKWs = self.statusStr()
self.writeToUsers("i", statusKWs, userCmd)
self.writeState(userCmd)
def writeState(self, userCmd=None):
stateKW = self.getStateKW()
self.writeToUsers("i", stateKW, userCmd)
def speed(self, speedValue, userCmd=None):
"""Set the desired move speed for the thread ring
@param[in] speedValue: a float, scale value to be converted to steps and sent to motor
@param[in] userCmd: a twistedActor BaseCommand
"""
speedValue = float(speedValue)
userCmd = expandUserCmd(userCmd)
if self.isMoving:
userCmd.setState(userCmd.Failed, "Cannot set speed, device is busy moving")
return userCmd
elif float(speedValue) > self.status.maxSpeed:
userCmd.setState(userCmd.Failed, "Max Speed Exceeded: %.4f > %.4f"%(speedValue, self.status.maxSpeed))
return userCmd
else:
speedDevCmd = self.queueDevCmd("speed %.6f"%speedValue, userCmd)
statusDevCmd = self.queueDevCmd("status", userCmd)
statusDevCmd.addCallback(self._statusCallback)
LinkCommands(userCmd, [speedDevCmd, statusDevCmd])
return userCmd
def getMoveCmdStr(self):
"""Determine difference between current and
desired position and send move command as an
offset.
This is hacky. I'm only allowed to command absolute positions
on the scaling ring so i need to determine the absolute position wanted
based on the offset I measure...gah.
"""
offset = self.targetPos - self.encPos
absMovePos = self.status.position + offset
return "move %.6f"%(absMovePos)
def move(self, position, userCmd=None):
"""!Move to a position
@param[in] postion: a float, position to move to (mm)
@param[in] userCmd: a twistedActor BaseCommand
"""
log.info("%s.move(postion=%.6f, userCmd=%s)" % (self, position, userCmd))
userCmd=expandUserCmd(userCmd)
if self.isMoving:
userCmd.setState(userCmd.Failed, "Cannot move, device is busy moving")
return userCmd
# verify position is in range
minPos, maxPos = self.status.moveRange
if not minPos<=position<=maxPos:
userCmd.setState(userCmd.Failed, "Move %.6f not in range [%.4f, %.4f]"%(position, minPos, maxPos))
return userCmd
self.iter = 1
self.targetPos = position
self.moveUserCmd = userCmd
print("moving threadring to: ", self.targetPos)
if not self.moveUserCmd.isActive:
self.moveUserCmd.setState(self.moveUserCmd.Running)
self.writeToUsers("i", "DesThreadRingPos=%.4f"%self.targetPos)
moveDevCmd = self.queueDevCmd(self.getMoveCmdStr(), self.moveUserCmd)
moveDevCmd.addCallback(self._moveCallback)
return userCmd
def home(self, userCmd=None):
log.info("%s.home(userCmd=%s)" % (self, userCmd))
setCountCmd = self.measScaleDev.setCountState()
def finishHome(_statusCmd):
if _statusCmd.didFail:
userCmd.setState(userCmd.Failed, "status failed.")
elif _statusCmd.isDone:
# assert that the encoders are really reading
# zeros
if not numpy.all(numpy.abs(self.measScaleDev.encPos) < 0.001):
userCmd.setState(userCmd.Failed, "zeros failed to set: %s"%str(self.measScaleDev.encPos))
else:
userCmd.setState(userCmd.Done)
def getStatus(_zeroEncCmd):
if _zeroEncCmd.didFail:
userCmd.setState(userCmd.Failed, "Encoder zero failed.")
elif _zeroEncCmd.isDone:
statusCmd = self.getStatus()
statusCmd.addCallback(finishHome)
def zeroEncoders(_moveCmd):
if _moveCmd.didFail:
userCmd.setState(userCmd.Failed, "Failed to move scaling ring to home position")
elif _moveCmd.isDone:
# zero the encoders in 1 second (give the ring a chance to stop)
def zeroEm():
zeroEncCmd = self.measScaleDev.setZero()
zeroEncCmd.addCallback(getStatus)
reactor.callLater(1., zeroEm)
def moveThreadRing(_setCountCmd):
if _setCountCmd.didFail:
userCmd.setState(userCmd.Failed, "Failed to set Mitutoyo EV counter into counting state")
elif _setCountCmd.isDone:
moveCmd = self.queueDevCmd("move %.6f"%self.measScaleDev.zeroPoint, userCmd)
moveCmd.addCallback(zeroEncoders)
setCountCmd.addCallback(moveThreadRing)
def _moveCallback(self, moveCmd):
if moveCmd.isActive:
self.status.setThreadAxisCurrent() # should already be there but whatever
# set state to moving, compute time, etc
time4move = abs(self.targetPos-self.status.position)/float(self.status.speed)
# update command timeout
moveCmd.setTimeLimit(time4move+2)
self.status.setState(self.status.Moving, self.iter, time4move)
self.writeState(self.moveUserCmd)
if moveCmd.didFail:
self.moveUserCmd.setState(self.moveUserCmd.Failed, "Failed to move scaling ring.")
elif moveCmd.isDone:
# get the status of the scaling ring
moveStatusCmd = self.queueDevCmd("status", self.moveUserCmd)
moveStatusCmd.addCallback(self._statusCallback)
moveStatusCmd.addCallback(self._readEncs)
def _readEncs(self, moveStatusCmd):
if moveStatusCmd.didFail:
self.moveUserCmd.setState(self.moveUserCmd.Failed, "Failed to get scaling ring status after move.")
elif moveStatusCmd.isDone:
# move's done, read the encoders
# and get scaling ring status
readEncCmd = self.measScaleDev.getStatus()
readEncCmd.addCallback(self._moveIter)
def _moveIter(self, readEncCmd):
"""Encoders have been read, decide to move again or finish the user
commanded move
"""
if readEncCmd.didFail:
self.moveUserCmd.setState(self.moveUserCmd.Failed, "Failed to read encoders after scaling ring move.")
elif readEncCmd.isDone:
atMaxIter = self.iter > self.status.maxIter
withinTol = numpy.abs(self.targetPos - self.encPos) < self.status.moveTol
if atMaxIter:
self.writeToUsers("i", "text='Max iter reached for scaling ring move.'")
if atMaxIter or withinTol:
# the move is done
self.iter = 0
self.status.setState(self.status.Done, self.iter)
self.writeState(self.moveUserCmd)
self.moveUserCmd.setState(self.moveUserCmd.Done)
else:
# command another move iteration
self.iter += 1
self.writeToUsers("i", "text='Begining scaling ring move iteration %i'"%self.iter)
moveDevCmd = self.queueDevCmd(self.getMoveCmdStr(), self.moveUserCmd)
moveDevCmd.addCallback(self._moveCallback)
def stop(self, userCmd=None):
"""Stop any scaling movement, cancelling any currently executing
command and any commands waiting on queue
@param[in] userCmd: a twistedActor BaseCommand
"""
userCmd=expandUserCmd(userCmd)
# kill any commands pending on the queue
# nicely kill move command if it's running
# if not self.currExeDevCmd.userCmd.isDone:
# self.currExeDevCmd.userCmd.setState(self.currExeDevCmd.userCmd.Failed, "Killed by stop")
if self.moveUserCmd.isActive:
self.moveUserCmd.setState(self.moveUserCmd.Cancelled, "Scaling ring move cancelled by stop command.")
stopDevCmd = self.queueDevCmd("stop", userCmd)
statusDevCmd = self.queueDevCmd("status", userCmd)
statusDevCmd.addCallback(self._statusCallback)
LinkCommands(userCmd, [stopDevCmd, statusDevCmd])
return userCmd
def handleReply(self, replyStr):
"""Handle a line of output from the device.
@param[in] replyStr the reply, minus any terminating \n
"""
log.info("%s.handleReply(replyStr=%s)" % (self, replyStr))
replyStr = replyStr.strip().lower()
# print(replyStr, self.currExeDevCmd.cmdStr)
if not replyStr:
return
if self.currExeDevCmd.isDone:
# ignore unsolicited output?
log.info("%s usolicited reply: %s for done command %s" % (self, replyStr, str(self.currExeDevCmd)))
self.writeToUsers("i", "%s usolicited reply: %s for done command %s" % (self, replyStr, str(self.currExeDevCmd)))
return
if replyStr == "ok":
# print("got ok", self.currExeDevCmd.cmdStr)
if self.currExeDevCmd.cmdStr == "status":
# if this is a status, verify it was not mangled before setting done
# if it is mangled try again
try:
statusError = self.status.checkFullStatus()
except MungedStatusError as statusError:
# status was munged, try again
print("statusError", statusError)
self.writeToUsers("w", statusError, self.currExeDevCmd.userCmd)
self.status.nIter += 1
if self.status.nIter > self.status.maxIter:
self.currExeDevCmd.setState(self.currExeDevCmd.Failed, "Maximum status iter reached. Status output generally mangled?")
else:
self.writeToUsers("w", "scale status mangle trying again iter %i"%self.status.nIter, self.currExeDevCmd.userCmd)
print("status munged, rewriting status")
log.info("%s writing %r iter %i" % (self, "status", self.status.nIter))
self.conn.writeLine("status")
return
print("status done and good")
self.currExeDevCmd.setState(self.currExeDevCmd.Done)
elif replyStr == self.currExeDevCmd.cmdStr:
# command echo
pass
elif "error" in replyStr:
self.currExeDevCmd.setState(self.currExeDevCmd.Failed, replyStr)
elif self.currExeDevCmd.cmdStr == "status":
# only parse lines if we asked for status
try:
parsed = self.status.parseStatusLine(replyStr)
except Exception as e:
errMsg = "Scale Device failed to parse: %s"%str(replyStr)
print(traceback.print_exc(file=sys.stdout))
log.error(errMsg)
self.writeToUsers("w", errMsg)
# if not parsed:
# print("%s.handleReply unparsed line: %s" % (self, replyStr))
# log.info("%s.handleReply unparsed line: %s" % (self, replyStr))
# def sendCmd(self, devCmdStr, userCmd):
# """!Execute the command
# @param[in] devCmdStr a string to send to the scale controller
# @param[in] userCmd a user command
# """
# if not self.conn.isConnected:
# log.error("%s cannot write %r: not connected" % (self, userCmd.cmdStr))
# userCmd.setState(userCmd.Failed, "not connected")
# return
# if not self.currCmd.isDone:
# log.error("%s cannot write %r: existing command %r not finished" % (self, userCmd.cmdStr, self.currCmd.cmdStr))
# userCmd.setState(userCmd.Failed, "device is busy")
# return
# self.currCmd = userCmd
# self.currDevCmdStr = devCmdStr
# log.info("%s writing %s" % (self, devCmdStr))
# self.conn.writeLine(devCmdStr)
def queueDevCmd(self, devCmdStr, userCmd):
"""Add a device command to the device command queue
@param[in] devCmdStr: a command string to send to the device.
@param[in] userCmd: a UserCmd associated with this device (probably but
not necessarily linked. Used here for writeToUsers
reference.
"""
log.info("%s.queueDevCmd(devCmdStr=%r, cmdQueue: %r"%(self, devCmdStr, self.devCmdQueue))
# append a cmdVerb for the command queue (otherwise all get the same cmdVerb and cancel eachother)
# could change the default behavior in CommandQueue?
cmdVerb = devCmdStr.split()[0]
assert cmdVerb in self.validCmdVerbs
devCmd = DevCmd(cmdStr=devCmdStr)
devCmd.cmdVerb = cmdVerb
devCmd.userCmd = userCmd
def queueFunc(devCmd):
# when the command is ready run this
# everything besides a move should return quickly
devCmd.setTimeLimit(SEC_TIMEOUT)
devCmd.setState(devCmd.Running)
if cmdVerb == "status":
# wipe status, to ensure we've
# gotten a full status when done.
self.status.flushStatus()
self.startDevCmd(devCmd.cmdStr)
self.devCmdQueue.addCmd(devCmd, queueFunc)
return devCmd
def startDevCmd(self, devCmdStr):
"""
@param[in] devCmdStr a line of text to send to the device
"""
devCmdStr = devCmdStr.lower()
log.info("%s.startDevCmd(%r)" % (self, devCmdStr))
try:
if self.conn.isConnected:
log.info("%s writing %r" % (self, devCmdStr))
self.conn.writeLine(devCmdStr)
else:
self.currExeDevCmd.setState(self.currExeDevCmd.Failed, "Not connected")
except Exception as e:
self.currExeDevCmd.setState(self.currExeDevCmd.Failed, textMsg=strFromException(e))
class MeasScaleDevice(TCPDevice):
"""!A Device for communicating with the LCO Scaling ring."""
def __init__(self, name, host, port, callFunc=None):
"""!Construct a ScaleDevice
Inputs:
@param[in] name name of device
@param[in] host host address of scaling ring controller
@param[in] port port of scaling ring controller
@param[in] callFunc function to call when state of device changes;
note that it is NOT called when the connection state changes;
register a callback with "conn" for that task.
"""
# the mitutoyos will be zeroed when the scaling ring is moved to 20
# so this zero point keep track fo the offset.
# if I could preset the mitutoyo's this would be unnecessary
# the preset command "CP**" doesn't seem to work.
self.zeroPoint = 20.0 # mm. Position where scale = 1
self.encPos = [None]*6
self.devCmdQueue = CommandQueue({})
TCPDevice.__init__(self,
name = name,
host = host,
port = port,
callFunc = callFunc,
cmdInfo = (),
)
@property
def position(self):
# return the average value of all encoder positions
# with respect to the zeroPoint
# this is used for servoing the scaling ring
if not self.isHomed:
return None
else:
return numpy.mean(self.encPos) + self.zeroPoint
@property
def isHomed(self):
if None in self.encPos:
return False
else:
return True
@property
def currExeDevCmd(self):
return self.devCmdQueue.currExeCmd.cmd
@property
def currDevCmdStr(self):
return self.currExeDevCmd.cmdStr
def init(self, userCmd=None, timeLim=3, getStatus=True):
"""Called automatically on startup after the connection is established.
Only thing to do is query for status or connect if not connected
getStatus ignored?
"""
log.info("%s.init(userCmd=%s, timeLim=%s, getStatus=%s)" % (self, userCmd, timeLim, getStatus))
userCmd = expandUserCmd(userCmd)
self.getStatus(userCmd) # status links the userCmd
return userCmd
def getStatus(self, userCmd=None, timeLim=1, linkState=True):
"""!Read all enc positions 1-6 channels, 3 physical gauges.
"""
# first flush the current status to ensure we don't
# have stale values
print("reading migs!")
userCmd = expandUserCmd(userCmd)
self.encPos = [None]*6
statusDevCmd = self.queueDevCmd(READ_ENC, userCmd)
statusDevCmd.addCallback(self._statusCallback)
statusDevCmd.setTimeLimit(timeLim)
if linkState:
LinkCommands(userCmd, [statusDevCmd])
return userCmd
else:
# return the device command to be linked outside
return statusDevCmd
def setCountState(self, userCmd=None, timeLim=1):
"""!Set the Mitutoyo EV counter into the counting state,
this is required after a power cycle
"""
userCmd = expandUserCmd(userCmd)
countDevCmd = self.queueDevCmd(COUNTING_STATE, userCmd)
currValDevCmd = self.queueDevCmd(DISPLAY_CURR, userCmd)
currValDevCmd.addCallback(self._statusCallback)
countDevCmd.setTimeLimit(timeLim)
currValDevCmd.setTimeLimit(timeLim)
LinkCommands(userCmd, [countDevCmd, currValDevCmd])
return userCmd
def setZero(self, userCmd=None, timeLim=1):
"""!Set the Mitutoyo EV counter into the counting state,
this is required after a power cycle
"""
userCmd = expandUserCmd(userCmd)
zeroDevCmd = self.queueDevCmd(ZERO_SET, userCmd)
zeroDevCmd.setTimeLimit(timeLim)
LinkCommands(userCmd, [zeroDevCmd])
return userCmd
def _statusCallback(self, statusCmd):
# if statusCmd.isActive:
# # not sure this is necessary
# # but ensures we get a 100% fresh status
# self.status.flushStatus()
if statusCmd.isDone and not statusCmd.didFail:
self.writeStatusToUsers(statusCmd.userCmd)
print("mig values,", self.encPos)
print("done reading migs")
def writeStatusToUsers(self, userCmd=None):
self.writeToUsers("i", "ScaleZeroPos=%.4f"%self.zeroPoint)
self.writeToUsers("i", self.encPosKWStr, userCmd)
severity = "i"
if not self.isHomed:
severity = "w"
self.writeToUsers(severity, self.encHomedKWStr, userCmd)
@property
def encPosKWStr(self):
encPosStr = []
for encPos in self.encPos[:3]:
if encPos is None:
encPosStr.append("?")
else:
encPos += self.zeroPoint
encPosStr.append("%.3f"%encPos)
return "ScaleEncPos=" + ", ".join(encPosStr[:3])
@property
def encHomedKWStr(self):
homedInt = 1 if self.isHomed else 0
return "ScaleEncHomed=%i"%homedInt
def setEncValue(self, serialStr):
"""Figure out which gauge this line corresponds to and set the value
"""
gaugeStr, gaugeVal = serialStr.split(",")
if "error" in gaugeVal.lower():
gaugeVal = None
else:
gaugeVal = float(gaugeVal)
gaugeInd = int(gaugeStr.strip("GN0")) - 1
self.encPos[gaugeInd] = gaugeVal
def handleReply(self, replyStr):
"""Handle a line of output from the device.
@param[in] replyStr the reply, minus any terminating \n
"""
log.info("%s.handleReply(replyStr=%s)" % (self, replyStr))
replyStr = replyStr.strip()
# print(replyStr, self.currExeDevCmd.cmdStr)
if not replyStr:
return
if self.currExeDevCmd.isDone:
# ignore unsolicited output?
log.info("%s usolicited reply: %s for done command %s" % (self, replyStr, str(self.currExeDevCmd)))
self.writeToUsers("i", "%s usolicited reply: %s for done command %s" % (self, replyStr, str(self.currExeDevCmd)))
return
if "error 15" in replyStr.lower():
self.writeToUsers("w", "Mitutoyo Error 15, not in counting state (was it power cycled?). Homing necessary.")
self.encPos = [None]*6
elif "error" in replyStr.lower():
# some other error?
self.writeToUsers("w", "Mitutoyo EV counter Error output: " + replyStr)
if self.currExeDevCmd.cmdStr == READ_ENC:
# all encoders values have been read
# set command done
self.setEncValue(replyStr)
# was this the 6th value read? if so we are done
if replyStr.startswith(ENCVAL_PREFIX+"%i"%6):
self.currExeDevCmd.setState(self.currExeDevCmd.Done)
if self.currExeDevCmd.cmdStr in [COUNTING_STATE, ZERO_SET, DISPLAY_CURR]:
if replyStr == SUCESS:
# successful set into counting state
self.currExeDevCmd.setState(self.currExeDevCmd.Done)
def queueDevCmd(self, devCmdStr, userCmd):
"""Add a device command to the device command queue
@param[in] devCmdStr: a command string to send to the device.
@param[in] userCmd: a UserCmd associated with this device (probably but
not necessarily linked. Used here for writeToUsers
reference.
"""
log.info("%s.queueDevCmd(devCmdStr=%r, cmdQueue: %r"%(self, devCmdStr, self.devCmdQueue))
#print("%s.queueDevCmd(devCmdStr=%r, cmdQueue: %r"%(self, devCmdStr, self.devCmdQueue))
# append a cmdVerb for the command queue (otherwise all get the same cmdVerb and cancel eachother)
# could change the default behavior in CommandQueue?
devCmd = DevCmd(cmdStr=devCmdStr)
devCmd.userCmd = userCmd
devCmd.cmdVerb = devCmdStr
self.devCmdQueue.addCmd(devCmd, self.startDevCmd)
return devCmd
def startDevCmd(self, devCmd):
"""
@param[in] devCmd a dev command
"""
log.info("%s.startDevCmd(%r)" % (self, devCmd.cmdStr))
#print("%s.startDevCmd(%r)" % (self, devCmd.cmdStr))
try:
if self.conn.isConnected:
log.info("%s writing %r" % (self, devCmd.cmdStr))
devCmd.setState(devCmd.Running)
self.conn.writeLine(devCmd.cmdStr)
else:
self.currExeDevCmd.setState(self.currExeDevCmd.Failed, "Not connected")
except Exception as e:
self.currExeDevCmd.setState(self.currExeDevCmd.Failed, textMsg=strFromException(e))
class M2Device(TCPDevice):
"""!A Device for communicating with the M2 process."""
def __init__(self, name, host, port, callFunc=None):
"""!Construct a LCODevice
Inputs:
@param[in] name name of device
@param[in] host host address of m2 controller
@param[in] port port of m2 controller
@param[in] callFunc function to call when state of device changes;
note that it is NOT called when the connection state changes;
register a callback with "conn" for that task.
"""
self.status = Status()
self._statusTimer = Timer()
self.waitMoveCmd = UserCmd()
self.waitMoveCmd.setState(self.waitMoveCmd.Done)
# self.waitGalilCmd = UserCmd()
# self.waitGalilCmd.setState(self.waitGalilCmd.Done)
# give status commands equal priority so they don't kill eachother
priorityDict = {
"status": 1,
"speed": 1,
"stop": 1,
"move": 1,
"galil": 1,
"offset": 1,
}
self.devCmdQueue = CommandQueue(priorityDict) # all commands of equal priority
TCPDevice.__init__(self,
name = name,
host = host,
port = port,
callFunc = callFunc,
cmdInfo = (),
)
@property
def isBusy(self):
return self.status.state == Moving
@property
def isOff(self):
return self.status.galil == Off
@property
def isDone(self):
# only done when state=done and galil=off
return not self.isBusy and self.isOff
@property
def currExeDevCmd(self):
return self.devCmdQueue.currExeCmd.cmd
@property
def currDevCmdStr(self):
return self.currExeDevCmd.cmdStr
# @property
# def atFocus(self):
# if self.status.statusFieldDict["focus"].value and abs(self.targFocus - self.status.statusFieldDict["focus"].value) < FocusPosTol:
# return True
# else:
# return False
def init(self, userCmd=None, timeLim=None, getStatus=True):
"""Called automatically on startup after the connection is established.
Only thing to do is query for status or connect if not connected
"""
log.info("%s.init(userCmd=%s, timeLim=%s, getStatus=%s)" % (self, userCmd, timeLim, getStatus))
# print("%s.init(userCmd=%s, timeLim=%s, getStatus=%s)" % (self, userCmd, timeLim, getStatus))
userCmd = expandUserCmd(userCmd)
# if not self.isConnected:
# return self.connect(userCmd=userCmd)
# get the speed on startup
# ignore getStatus flag, just do it always
self.queueDevCmd("speed")
return self.getStatus(userCmd=userCmd)
# userCmd.setState(userCmd.Done)
# return userCmd
def getStatus(self, userCmd=None):
"""Return current telescope status. Continuously poll.
"""
log.info("%s.getStatus(userCmd=%s)" % (self, userCmd)) # logging this will flood the log
# print("%s.getStatus(userCmd=%s)" % (self, userCmd))
userCmd = expandUserCmd(userCmd)
if not self.conn.isConnected:
userCmd.setState(userCmd.Failed, "Not Connected to M2")
return userCmd
self._statusTimer.cancel() # incase a status is pending
userCmd = expandUserCmd(userCmd)
# userCmd.addCallback(self._statusCallback)
# gather list of status elements to get
statusCmd = self.queueDevCmd("status", userCmd)
LinkCommands(userCmd, [statusCmd])
return userCmd
def processStatus(self, replyStr):
# print("procesStatus", replyStr)
self.status.parseStatus(replyStr)
# do we want status output so frequently? probabaly not.
# perhaps only write status if it has changed...
# but so far status is a small amount of values
# so its probably ok
statusStr = self.status.getStatusStr()
if statusStr:
userCmd = self.currExeDevCmd.userCmd
if self.waitMoveCmd.isActive:
userCmd = self.waitMoveCmd
# write as debug during moves
self.writeToUsers("d", statusStr, userCmd)
else:
self.writeToUsers("i", statusStr, userCmd)
if self.waitMoveCmd.isActive:
if not self.isBusy:
# move is done
if not self.isOff:
# move just finished but galil is not off, turn it off
self.queueDevCmd("galil off")
else:
# move is done and galil is off, set wait move command as done
self.waitMoveCmd.setState(self.waitMoveCmd.Done)
if not self.isDone:
# keep polling until done
self._statusTimer.start(PollTime, self.getStatus)
def stop(self, userCmd=None):
userCmd = expandUserCmd(userCmd)
if not self.waitMoveCmd.isDone:
self.waitMoveCmd.setState(self.waitMoveCmd.Cancelled, "Stop commanded")
#print("sec stop commanded")
stopCmd = self.queueDevCmd("stop", userCmd)
galilOffCmd = self.queueDevCmd("galil off", userCmd)
status = self.queueDevCmd("status", userCmd)
status2 = self.queueDevCmd("status", userCmd)
# first status gets the error state
# second status clears it
LinkCommands(userCmd, [stopCmd, status, galilOffCmd, status2])
return userCmd
def focus(self, focusValue, offset=False, userCmd=None):
"""Command an offset or absolute focus move
@param[in] focusValue: float, focus value in microns
@param[in] offset, if true this is offset, else absolute
@param[in] userCmd: a twistedActor BaseCommand
WARNING!!!
At APO increasing focus corresponds to decreasing M1-M2 dist.
The mirror controller at LCO convention is the opposite.
"""
log.info("%s.focus(userCmd=%s, focusValue=%.2f, offset=%s)" % (self, userCmd, focusValue, str(bool(offset))))
# if this focus value is < 50 microns
userCmd = expandUserCmd(userCmd)
if offset:
deltaFocus = focusValue
else:
deltaFocus = self.status.secFocus - focusValue
if abs(deltaFocus) < MIN_FOCUS_MOVE:
# should focus be cancelled or just set to done?
self.writeToUsers("w", "Focus offset below threshold of < %.2f, moving anyways."%MIN_FOCUS_MOVE, userCmd)
# userCmd.setState(userCmd.Done)
# return userCmd
# focusDir = 1 # use M2's natural coordinates
# focusDir = -1 # use convention at APO
return self.move(valueList=[focusValue], offset=offset, userCmd=userCmd)
def move(self, valueList, offset=False, userCmd=None):
"""Command an offset or absolute orientation move
@param[in] valueList: list of 1 to 5 values specifying pistion(um), tiltx("), tilty("), transx(um), transy(um)
@param[in] offset, if true this is offset, else absolute
@param[in] userCmd: a twistedActor BaseCommand
Note: increasing distance eg pistion means increasing spacing between primary and
secondary mirrors.
"""
log.info("%s.move(userCmd=%s, valueList=%s, offset=%s)" % (self, userCmd, str(valueList), str(bool(offset))))
userCmd = expandUserCmd(userCmd)
if not self.waitMoveCmd.isDone:
userCmd.setState(userCmd.Failed, "Mirror currently moving")
return userCmd
if not 1<=len(valueList)<=5:
userCmd.setState(userCmd.Failed, "Must specify 1 to 5 numbers for a move")
return userCmd
self.waitMoveCmd = UserCmd()
self.waitMoveCmd.userCmd = userCmd # for write to users
self.status.desOrientation = self.status.orientation[:]
if offset:
# if offset is specified, offset from current value
for ii, value in enumerate(valueList):
self.status.desOrientation[ii] += value
else:
# if absolute is wanted, overwrite all those
# specified
for ii, value in enumerate(valueList):
self.status.desOrientation[ii] = value
cmdType = "offset" if offset else "move"
strValList = " ".join(["%.2f"%val for val in valueList])
cmdStr = "%s %s"%(cmdType, strValList)
moveCmd = self.queueDevCmd(cmdStr, userCmd)
statusCmd = self.queueDevCmd("status", userCmd)
# status immediately to see moving state
# determine total time for move
# just use focus distance as proxy (ignore)
galilOverHead = 2 # galil take roughly 2 secs to boot up.
extraOverHead = 2 #
self.status._moveTimeTotal = self.getTimeForMove()
timeout = self.status._moveTimeTotal+galilOverHead+extraOverHead
userCmd.setTimeLimit(timeout)
LinkCommands(userCmd, [moveCmd, statusCmd, self.waitMoveCmd])
return userCmd
def getTimeForMove(self):
dist2Move = numpy.max(numpy.abs(numpy.subtract(self.status.desOrientation, self.status.orientation)))
time4Move = dist2Move / self.status.speed
return time4Move
def handleReply(self, replyStr):
"""Handle a line of output from the device. Called whenever the device outputs a new line of data.
@param[in] replyStr the reply, minus any terminating \n
Tasks include:
- Parse the reply
- Manage the pending commands
- Parse status to update the model parameters
"""
log.info("%s read %r, currCmdStr: %s" % (self, replyStr, self.currDevCmdStr))
# print("%s read %r, currCmdStr: %s" % (self, replyStr, self.currDevCmdStr))
replyStr = replyStr.strip()
if not replyStr:
return
if self.currExeDevCmd.isDone:
# ignore unsolicited ouput
return
if "error" in replyStr.lower():
# error
self.writeToUsers("w", "Error in M2 reply: %s, current cmd: %s"%(replyStr, self.currExeDevCmd.cmdStr))
# if this was a speed command, set it
if self.currDevCmdStr.lower() == "speed":
self.status.speed = float(replyStr)
elif self.currDevCmdStr.lower() == "status":
self.processStatus(replyStr)
# only one line is ever returned after a request
# so if we got one, then the request is done
self.currExeDevCmd.setState(self.currExeDevCmd.Done)
def queueDevCmd(self, cmdStr, userCmd=None):
"""Add a device command to the device command queue
@param[in] cmdStr, string to send to the device.
"""
log.info("%s.queueDevCmd(cmdStr=%r, cmdQueue: %r"%(self, cmdStr, self.devCmdQueue))
# print("%s.queueDevCmd(devCmd=%r, devCmdStr=%r, cmdQueue: %r"%(self, devCmd, devCmd.cmdStr, self.devCmdQueue))
# append a cmdVerb for the command queue (other wise all get the same cmdVerb and cancel eachother)
# could change the default behavior in CommandQueue?
userCmd = expandUserCmd(userCmd)
devCmd = DevCmd(cmdStr)
devCmd.cmdVerb = cmdStr.split()[0]
devCmd.userCmd = userCmd
def queueFunc(devCmd):
self.startDevCmd(devCmd)
self.devCmdQueue.addCmd(devCmd, queueFunc)
return devCmd
def startDevCmd(self, devCmd):
"""
@param[in] devCmdStr a line of text to send to the device
"""
devCmdStr = devCmd.cmdStr.lower() # m2 uses all lower case
log.info("%s.startDevCmd(%r)" % (self, devCmdStr))
try:
if self.conn.isConnected:
log.info("%s writing %r" % (self, devCmdStr))
# set move command to running now. Bug if set earlier race condition
# with status
if "move" in devCmdStr.lower() or "offset" in devCmdStr.lower():
self.waitMoveCmd.setState(self.waitMoveCmd.Running)
self.status.state = Moving
self.writeToUsers("i", self.status.secStateStr(), devCmd.userCmd)
# if "galil" in devCmdStr.lower():
# self.waitGalilCmd.setState(self.waitGalilCmd.Running)
self.conn.writeLine(devCmdStr)
else:
self.currExeDevCmd.setState(self.currExeDevCmd.Failed, "Not connected to M2")
except Exception as e:
self.currExeDevCmd.setState(self.currExeDevCmd.Failed, textMsg=strFromException(e))
