351 lines
12 KiB
Python
351 lines
12 KiB
Python
#!/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
|