Addpython motor simulation for taipan
This commit is contained in:
Douglas Clowes
350
site_ansto/instrument/TEST_SICS/fakeGalil/galilmotor.py
Normal file
350
site_ansto/instrument/TEST_SICS/fakeGalil/galilmotor.py
Normal file
@@ -0,0 +1,350 @@
|
||||
#!/usr//bin/env python
|
||||
# vim: ts=8 sts=4 sw=4 expandtab
|
||||
# Author: Douglas Clowes (dcl@ansto.gov.au) 2012-06-29
|
||||
import inspect
|
||||
import time
|
||||
import math
|
||||
|
||||
class GalilMotor(object):
|
||||
"""Galil Motor Object"""
|
||||
|
||||
def __init__(self, theAxis):
|
||||
print "GalilMotor ctor: %s" % theAxis
|
||||
self.axis = theAxis
|
||||
self.motorState = "OFF"
|
||||
self.stepsPerX = 25000.0
|
||||
self.cntsPerX = 8192.0
|
||||
self.speed = 25000
|
||||
self.accel = 2500
|
||||
self.decel = 2500
|
||||
self.lowerLim = -20.0
|
||||
self.upperLim = 20.0
|
||||
self.hasAbsEnc = True
|
||||
self.unitsHome = 0.0
|
||||
self.absEncHome = 100 * 1000 * 1000
|
||||
self.motorHome = 0.0
|
||||
self.currentSteps = 0
|
||||
self.targetSteps = self.currentSteps
|
||||
self.configDone = False
|
||||
self.switches = 44
|
||||
self.stopcode = 0
|
||||
self.stopCallback = None
|
||||
self.moveCallback = None
|
||||
self.trace = False
|
||||
|
||||
def doTrace(self, arg):
|
||||
print "Trace %s: %s" % (self.axis, arg),
|
||||
if self.trace:
|
||||
print "(ON =>",
|
||||
else:
|
||||
print "(OFF =>",
|
||||
if arg == "ON":
|
||||
self.trace = True
|
||||
else:
|
||||
self.trace = False
|
||||
if self.trace:
|
||||
print "ON)"
|
||||
else:
|
||||
print "OFF)"
|
||||
|
||||
def doConfigure(self, arg):
|
||||
print "Configure %s: %s" % (self.axis, arg)
|
||||
the_parts = arg.split(",")
|
||||
configDoneAlready = self.configDone
|
||||
for part in the_parts:
|
||||
(lh, rh) = part.split("=")
|
||||
if lh == "SPX":
|
||||
self.stepsPerX = float(rh)
|
||||
self.configDone = True
|
||||
elif lh == "CPX":
|
||||
self.cntsPerX = float(rh)
|
||||
self.configDone = True
|
||||
elif lh == "RL":
|
||||
self.lowerLim = float(rh)
|
||||
self.configDone = True
|
||||
elif lh == "FL":
|
||||
self.upperLim = float(rh)
|
||||
self.configDone = True
|
||||
elif lh == "UH":
|
||||
self.unitsHome = float(rh)
|
||||
self.configDone = True
|
||||
elif lh == "EH":
|
||||
self.hasAbsEnc = True
|
||||
self.absEncHome = float(rh)
|
||||
self.configDone = True
|
||||
elif lh == "MH":
|
||||
self.hasAbsEnc = False
|
||||
self.motorHome = float(rh)
|
||||
self.configDone = True
|
||||
else:
|
||||
print "Unrecognized config item \"%s\" in \"%s\"" % (lh, arg)
|
||||
if self.configDone and not configDoneAlready:
|
||||
self.currentSteps = 0
|
||||
self.targetSteps = 0
|
||||
|
||||
def getEncoder(self):
|
||||
'''Calculate the encoder from the currentSteps'''
|
||||
value = self.absEncHome + self.currentSteps * (self.cntsPerX / self.stepsPerX)
|
||||
return int(value)
|
||||
|
||||
def getPosition(self):
|
||||
'''Calculate the unit position'''
|
||||
value = self.unitsHome + self.currentSteps / self.stepsPerX
|
||||
value = self.getEncoder() - self.absEncHome
|
||||
value /= self.cntsPerX
|
||||
value += self.unitsHome
|
||||
return value
|
||||
|
||||
def doIteration(self, now):
|
||||
'''If moving then update speed and position using
|
||||
v = u + at
|
||||
s = ut + 1/2 at**2 = (u + v)t / 2
|
||||
v**2 = u**2 + 2as
|
||||
'''
|
||||
if self.motorState in ["MOVING"]:
|
||||
t = now - self.motorStartTime
|
||||
distance = abs(self.targetSteps - self.motorStartSteps)
|
||||
topSpeed = math.sqrt(2.0 * distance / ( 1.0 / self.accel + 1.0 / self.decel))
|
||||
if topSpeed < 1e-6:
|
||||
topSpeed = 0
|
||||
if topSpeed > self.speed:
|
||||
topSpeed = self.speed
|
||||
t1 = topSpeed / self.accel
|
||||
s1 = 0.5 * self.accel * t1**2
|
||||
t3 = topSpeed / self.decel
|
||||
s3 = 0.5 * self.decel * t3**2
|
||||
s2 = distance - s1 - s3
|
||||
if s2 < 1e-6:
|
||||
s2 = 0
|
||||
if s2 == 0 or topSpeed == 0:
|
||||
t2 = 0
|
||||
else:
|
||||
t2 = s2 / topSpeed
|
||||
if t >= (t1 + t2 + t3):
|
||||
# Finished
|
||||
s = distance
|
||||
v = 0
|
||||
self.motorState = "STOPPED"
|
||||
self.stopcode = 1
|
||||
self.switches = 44 # stopped, both limits clear
|
||||
elif t >= (t1 + t2):
|
||||
# Ramp Down
|
||||
dt = t - (t1 + t2)
|
||||
# v = u + at
|
||||
v = topSpeed - self.decel * dt
|
||||
# s = (u + v)t / 2
|
||||
s = s1 + s2 + 0.5 * (topSpeed + v) * dt
|
||||
elif t >= t1:
|
||||
# Run Flat
|
||||
v = topSpeed
|
||||
s = s1 + topSpeed * (t - t1)
|
||||
else:
|
||||
# Ramp Up
|
||||
v = 0.0 + self.accel * t
|
||||
s = 0.5 * self.accel * t**2
|
||||
if self.speed < 1e-6:
|
||||
self.percentVelocity = 100.0
|
||||
else:
|
||||
self.percentVelocity = 100.0 * v / self.speed
|
||||
if distance < 1e-6:
|
||||
self.percentDistance = 100.0
|
||||
else:
|
||||
self.percentDistance = 100.0 * s / distance
|
||||
if self.targetSteps > self.motorStartSteps:
|
||||
self.currentSteps = self.motorStartSteps + int(s)
|
||||
if self.currentSteps >= self.targetSteps:
|
||||
self.motorState = "STOPPED"
|
||||
self.stopcode = 1
|
||||
self.switches = 44 # stopped, both limits clear
|
||||
elif self.targetSteps < self.motorStartSteps:
|
||||
self.currentSteps = self.motorStartSteps - int(s)
|
||||
if self.currentSteps <= self.targetSteps:
|
||||
self.motorState = "STOPPED"
|
||||
self.stopcode = 1
|
||||
self.switches = 44 # stopped, both limits clear
|
||||
else:
|
||||
self.motorState = "STOPPED"
|
||||
self.stopcode = 1
|
||||
self.switches = 44 # stopped, both limits clear
|
||||
currentPosition = self.getPosition()
|
||||
if not self.moveCallback == None:
|
||||
self.moveCallback(self)
|
||||
# TODO Forward Limit
|
||||
# TODO Reverse Limit
|
||||
if self.motorState == "STOPPED":
|
||||
print "Axis %s stopped at:" % self.axis, currentPosition
|
||||
self.currentSteps = self.targetSteps
|
||||
if not self.stopCallback == None:
|
||||
self.stopCallback(self)
|
||||
else:
|
||||
pass
|
||||
|
||||
def doAssignment(self, cmd, arg):
|
||||
'''This is for commands like "PAA=1234567" which are for the motor.
|
||||
The axis and the assignment operator have already been removed.
|
||||
'''
|
||||
if self.trace:
|
||||
print "Trace ASG %s: %s" % (self.axis, str(cmd) + " " + str(arg))
|
||||
if cmd == "PA":
|
||||
self.targetSteps = int(arg)
|
||||
elif cmd == "PR":
|
||||
self.targetSteps = self.currentSteps + int(arg)
|
||||
elif cmd == "SP":
|
||||
self.speed = max(1, abs(int(arg)))
|
||||
elif cmd == "AC":
|
||||
self.accel = max(1, abs(int(arg)))
|
||||
elif cmd == "DC":
|
||||
self.decel = max(1, abs(int(arg)))
|
||||
else:
|
||||
print "Unknown assignment", cmd, arg
|
||||
|
||||
def doQuery(self, arg):
|
||||
if arg == "TD":
|
||||
response = self.currentSteps
|
||||
elif arg == "ZZ":
|
||||
response = self.getPosition()
|
||||
elif arg == "TP":
|
||||
response = self.getEncoder()
|
||||
elif arg == "SP":
|
||||
response = self.speed
|
||||
elif arg == "AC":
|
||||
response = self.accel
|
||||
elif arg == "DC":
|
||||
response = self.decel
|
||||
elif arg == "TS":
|
||||
response = self.switches
|
||||
elif arg == "SC":
|
||||
response = self.stopcode
|
||||
elif arg == "MO":
|
||||
if self.motorState in ["OFF"]:
|
||||
response = 1
|
||||
else:
|
||||
response = 0
|
||||
elif arg == "BG":
|
||||
if self.motorState in ["MOVING"]:
|
||||
response = 1
|
||||
else:
|
||||
response = 0
|
||||
else:
|
||||
print "Unknown Query:", arg
|
||||
response = None
|
||||
if self.trace:
|
||||
print "Trace QRY %s: %s" % (self.axis, str(arg) + " = " + str(response))
|
||||
return response
|
||||
|
||||
def doCommandTD(self, arg):
|
||||
return self.doQuery("TD")
|
||||
|
||||
def doCommandZZ(self, arg):
|
||||
result = self.doQuery("ZZ")
|
||||
print "Position:", result
|
||||
return result
|
||||
|
||||
def doCommandTP(self, arg):
|
||||
return self.doQuery("TP")
|
||||
|
||||
def doCommandST(self, arg):
|
||||
if self.motorState in ["MOVING"]:
|
||||
self.motorState = "STOPPED"
|
||||
self.stopcode = 4
|
||||
self.switches = 44
|
||||
return True
|
||||
|
||||
def doCommandBG(self, arg):
|
||||
if self.motorState in ["ON", "STOPPED"]:
|
||||
self.motorState = "MOVING"
|
||||
self.stopcode = 0
|
||||
self.switches = 140 # moving, both limits clear
|
||||
self.motorStartTime = time.time()
|
||||
self.motorStartSteps = self.currentSteps
|
||||
return True
|
||||
return False
|
||||
|
||||
def doCommandSH(self, arg):
|
||||
if self.motorState in ["ON", "OFF", "STOPPED"]:
|
||||
self.motorState = "ON"
|
||||
return True
|
||||
return False
|
||||
|
||||
def doCommandMO(self, arg):
|
||||
if self.motorState in ["ON", "OFF", "STOPPED"]:
|
||||
self.motorState = "OFF"
|
||||
return True
|
||||
return False
|
||||
|
||||
def doCommandPA(self, arg):
|
||||
self.targetSteps = int(arg)
|
||||
return True
|
||||
|
||||
def doCommandPR(self, arg):
|
||||
self.targetSteps = self.currentSteps + int(arg)
|
||||
return True
|
||||
|
||||
def doCommand(self, cmd, arg):
|
||||
'''This is for commands like "BGA" which are for the motor.
|
||||
The axis and other cruft have already been removed.
|
||||
'''
|
||||
action = "inspect.ismethod(GalilMotor.doCommand%s)" % cmd
|
||||
if eval(action):
|
||||
if self.trace:
|
||||
data = cmd
|
||||
if arg:
|
||||
data = data + " " + arg
|
||||
print "Trace CMD: \"" + data + "\""
|
||||
action = "response = self.doCommand%s(%s)" % (cmd, arg)
|
||||
exec action
|
||||
return response
|
||||
else:
|
||||
print "Unknown Action:", eval(action), "for cmd:", action
|
||||
|
||||
if __name__ == '__main__':
|
||||
motor = GalilMotor("A")
|
||||
motor.doConfigure("SPX=25000.0,CPX=8192.0,RL=-10.0,FL=10.0,UH=0.0,EH=100000000")
|
||||
print "SP", motor.doAssignment("SP", "25000")
|
||||
print "AC", motor.doAssignment("AC", "25000")
|
||||
print "DC", motor.doAssignment("DC", "25000")
|
||||
print "PA", motor.doAssignment("PA", "100000")
|
||||
print "SH", motor.doCommand("SH", None)
|
||||
print "BG", motor.doCommand("BG", None)
|
||||
print "MO", motor.doQuery("MO")
|
||||
print "BG", motor.doQuery("BG")
|
||||
count = 0
|
||||
while motor.doQuery("BG") and count <= 100:
|
||||
t = 0.1 * count
|
||||
motor.doIteration(motor.motorStartTime + t)
|
||||
rnge = 70
|
||||
vspot = int(0.01 * rnge * motor.percentVelocity)
|
||||
sspot = int(0.01 * rnge * motor.percentDistance)
|
||||
bth = "b"
|
||||
if sspot >= 0:
|
||||
ess = "s"
|
||||
else:
|
||||
ess = "*"
|
||||
bth = "B"
|
||||
sspot = 0
|
||||
if vspot >= 0:
|
||||
vee = "v"
|
||||
else:
|
||||
vee = "^"
|
||||
bth = "B"
|
||||
vspot = 0
|
||||
if vspot < sspot:
|
||||
line = " "*(vspot) + vee + " "*(sspot - vspot - 1) + ess + " "*(rnge - sspot) + "|"
|
||||
line += " %2d %2d" % (vspot, sspot)
|
||||
elif vspot > sspot:
|
||||
line = " "*(sspot) + ess + " "*(vspot - sspot - 1) + vee + " "*(rnge - vspot) + "|"
|
||||
line += " %2d %2d" % (sspot, vspot)
|
||||
else:
|
||||
line = " "*(sspot) + bth + " "*(rnge - sspot) + "|"
|
||||
line += " %2d" % (sspot)
|
||||
print "%3.2f: %s" % (t, line)
|
||||
count += 1
|
||||
print "_MO", motor.doQuery("MO")
|
||||
print "_BG", motor.doQuery("BG")
|
||||
print "TP", motor.doCommand("TP", None)
|
||||
print "TD", motor.doCommand("TD", None)
|
||||
print "ZZ", motor.doCommand("ZZ", None)
|
||||
print "_ZZ", motor.doQuery("ZZ")
|
||||
print "MO", motor.doCommand("MO", None)
|
||||
print "_MO", motor.doQuery("MO")
|
||||
print "Iterations:", count
|
||||
Reference in New Issue
Block a user