linse/frappy
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first tries with uniax stick

Browse Source
This commit is contained in:
zolliker
2021-04-30 17:28:05 +02:00
parent ca6aebd290
commit 114af36ac6
5 changed files with 311 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

187
secop_psi/trinamic.py
View File

@ -25,6 +25,7 @@
import time
import os
import struct
import json
from secop.core import BoolType, Command, EnumType, FloatRange, IntRange, \
HasIodev, Parameter, Property, Drivable, TupleOf, Done
from secop.bytesio import BytesIO
@ -69,7 +70,9 @@ class AxisPar(Parameter):
return value * self.scale
def read(self, motor):
return round(self.from_raw(motor, motor.comm(self.GET, self.adr)), 3)
result = round(self.from_raw(motor, motor.comm(self.GET, self.adr)), 3)
# print('read', self.adr, result)
return result
def write(self, motor, value):
rvalue = self.to_raw(motor, value)
@ -136,12 +139,13 @@ class Motor(HasIodev, Drivable):
default=(8, 0))
value = Parameter('motor position', FloatRange(unit='deg'))
target = Parameter('_', FloatRange(unit='$'), default=0)
movelimit = Parameter('max. angle to drive in one go', FloatRange(unit='$'), default=400, readonly=False)
zero = Parameter('zero point', FloatRange(unit='$'), readonly=False, default=0)
tolerance = Parameter('positioning tolerance', FloatRange(unit='$'), readonly=False, default=0.9)
encoder = Parameter('encoder value', FloatRange(unit='$'), needscfg=False)
steppos = Parameter('motor steps position', FloatRange(unit='$'), needscfg=False)
at_upper_limit = Parameter('upper limit switch touched', BoolType(), needscfg=False)
at_lower_limit = Parameter('lower limit switch touched', BoolType(), needscfg=False)
# at_upper_limit = Parameter('upper limit switch touched', BoolType(), needscfg=False)
# at_lower_limit = Parameter('lower limit switch touched', BoolType(), needscfg=False)
pull_up = Parameter('activate pull up resistors', BoolType(), needscfg=False)
baudrate = Parameter('baud rate code', EnumType({'%d' % v: i for i, v in enumerate(BAUDRATES)}),
readonly=False, default=0, poll=True)
@ -165,34 +169,73 @@ class Motor(HasIodev, Drivable):
_targettime = None
_prevconn = None
iodevClass = BytesIO
_commcnt = 0
_pollcnt = 0
_lastpoll = 0
_calcTimeout = True
_poserror = None
save_filename = None
def earlyInit(self):
self.resetValues = dict(self.writeDict)
self.writeDict.update(self.loadParams())
def loadParams(self):
# the following should be moved to core functionality
savedir = os.path.join(CONFIG['logdir'], 'persistent')
os.makedirs(savedir, exist_ok=True)
self._save_filename = os.path.join(savedir, '%s.%s.json' % (self.DISPATCHER.equipment_id, self.name))
try:
with open(self._save_filename, 'r') as f:
save_dict = json.load(f)
except FileNotFoundError:
save_dict = {}
writeDict = {}
for pname, value in save_dict.items():
pobj = self.parameters[pname]
val = pobj.datatype.import_value(value)
if pobj.readonly:
try:
pobj.value = val
except Exception as e:
self.log.warning('can not restore %r to %r' % (pname, val))
else:
writeDict[pname] = val
return writeDict
def saveParams(self):
save_dict = {}
for pname, pobj in self.parameters.items():
if pname not in ('target', 'internal_target'):
save_dict[pname] = pobj.export_value()
tmpfile = self._save_filename + '.tmp'
try:
with open(tmpfile, 'w') as f:
json.dump(save_dict, f, indent=2)
f.write('\n')
os.rename(tmpfile, self._save_filename)
finally:
try:
os.remove(tmpfile)
except FileNotFoundError:
pass
def startModule(self, started_callback):
loaded_pos = None
try:
print(self.save_filename())
with open(self.save_filename()) as f:
loaded_pos = float(f.read())
except FileNotFoundError:
pass
super().startModule(started_callback)
encoder = self.read_encoder()
if loaded_pos is not None:
diff = loaded_pos - encoder + 180
self._initialize = True
def cb(self=self, started_callback=started_callback, encoder=self.encoder, zero=self.zero):
encoder += self.zero - zero
diff = encoder - self.encoder + 180
if abs(diff % 360 - 180) < self.enc_tolerance:
diff = round(diff / 180) * 180
diff = round((diff - 180) / 360) * 360
if diff:
self.comm(SET_AXIS_PAR, 209, AxisZeroPar.to_raw(self, encoder + diff))
self._poserror = 'reset'
else:
msg = 'loaded pos (%g) does not match encoder reading' % loaded_pos
self.status = self.Status.ERROR, msg
self.log.error(msg + ' (%g)' % encoder)
self.log.error('encoder: saved value (%g) does not match reading (%g)' % (encoder, self.encoder))
self._poserror = 'need_reset'
self.comm(SET_GLOB_PAR, 2255, 1)
self._initialize = False
self._prevconn = self._iodev._conn
started_callback()
super().startModule(cb)
def comm(self, cmd, adr=0, value=0):
if self._calcTimeout:
@ -203,7 +246,6 @@ class Motor(HasIodev, Drivable):
raise CommunicationFailedError('unsupported baud rate: %d' % baudrate)
self._iodev.timeout = 0.03 + 200 / baudrate
self._commcnt = self._commcnt + 1
bank = adr // 1000
iadr = adr % 1000
for itry in range(3):
@ -225,47 +267,37 @@ class Motor(HasIodev, Drivable):
return result
def read_value(self):
now = time.time()
if now > self._lastpoll + 1:
print(now - self._lastpoll, self._commcnt, self._pollcnt)
self._commcnt = 0
self._pollcnt = 0
self._lastpoll = now
self._pollcnt = self._pollcnt + 1
init = self.comm(GET_GLOB_PAR, 2255) # bank 2 adr 255
if not init or self._prevconn != self._iodev._conn:
# reset to initial parameters when either motor lost power or connection has changed
self.log.info('reset initial values')
for pname, value in self.resetValues.items():
getattr(self, 'write_' + pname)(value)
self.comm(SET_GLOB_PAR, 2255, 1)
self._prevconn = self._iodev._conn
rawenc = self.comm(GET_AXIS_PAR, 209)
rawpos = self.comm(GET_AXIS_PAR, 1)
self.encoder = AxisZeroPar.from_raw(self, rawenc)
self.save_pos()
self.steppos = AxisZeroPar.from_raw(self, rawpos)
initialized = self._initialize or self.comm(GET_GLOB_PAR, 2255) # bank 2 adr 255
if initialized and self._prevconn == self._iodev._conn: # no power loss or connection interrupt
self.saveParams()
else:
self.log.info('set to previous saved values')
for pname, value in self.loadParams().items():
try:
getattr(self, 'write_' + pname)(value)
except Exception as e:
self.log.warning('can not write %r to %r' % (value, pname))
self.comm(SET_GLOB_PAR, 2255, 1)
self._prevconn = self._iodev._conn
if abs(rawenc - rawpos) > 128:
return self.encoder
return self.steppos
def save_filename(self):
savedir = os.path.join(CONFIG['logdir'], 'persistent')
os.makedirs(savedir, exist_ok=True)
return os.path.join(savedir, '%s.%s' % (type(self).__name__, self.name))
def save_pos(self):
with open(self.save_filename(), 'w') as f:
f.write('%g' % self.value)
def read_status(self):
if not self._targettime:
if self._poserror:
return self.Status.ERROR, 'encoder does not match internal pos'
return self.status
self.get_limits()
if self.at_lower_limit or self.at_upper_limit:
self.stop()
return self.Status.ERROR, 'at limit'
# self.get_limits()
# if self.at_lower_limit or self.at_upper_limit:
# self.stop()
# return self.Status.ERROR, 'at limit'
target_reached = self.read_target_reached()
if target_reached or self.read_move_status():
self._targettime = None
@ -283,10 +315,20 @@ class Motor(HasIodev, Drivable):
rawpos = self.comm(GET_AXIS_PAR, 1)
if abs(rawenc - rawpos) > 128:
# adjust missing steps
missing_steps = round((rawenc - rawpos) / 256.0)
self.log.warning('correct %s missing steps', missing_steps)
rawpos += missing_steps * 256
missing_steps = (rawenc - rawpos) / 256.0
#if self._poserror == 'reset':
print(missing_steps, self._poserror)
if abs(missing_steps) < 10 or self._poserror == 'reset':
self.log.warning('correct missing steps (%.1f deg)', AxisDiffPar.from_raw(self, missing_steps))
self._poserror = None
self.status = self.Status.IDLE, ''
else:
self._poserror = 'need_reset'
raise ValueError('encoder does not match internal pos')
rawpos += round(missing_steps) * 256
self.comm(SET_AXIS_PAR, 1, rawpos)
if abs(target - self.encoder) > self.movelimit:
raise ValueError('can not move more than %s deg' % self.movelimit)
rawtarget = round((target - self.zero) / self.fact)
delay = (abs(rawtarget - rawpos) / 30.5 / self.maxspeed
+ self.maxspeed / self.acceleration / 15.25 + 0.5)
@ -298,19 +340,19 @@ class Motor(HasIodev, Drivable):
self.status = self.Status.BUSY, 'changed target'
return target
def get_limits(self):
self.input_bits = self.comm(GET_IO, 255)
bits = (~ self.input_bits) & 0xf
self.at_lower_limit = bool(bits & self.limit_pin_mask[0])
self.at_upper_limit = bool(bits & self.limit_pin_mask[1])
def read_at_upper_limit(self):
self.get_limits()
return Done
def read_at_lower_limit(self):
self.get_limits()
return Done
# def get_limits(self):
# self.input_bits = self.comm(GET_IO, 255)
# bits = (~ self.input_bits) & 0xf
# self.at_lower_limit = bool(bits & self.limit_pin_mask[0])
# self.at_upper_limit = bool(bits & self.limit_pin_mask[1])
#
# def read_at_upper_limit(self):
# self.get_limits()
# return Done
#
# def read_at_lower_limit(self):
# self.get_limits()
# return Done
def write_pull_up(self, value):
self.comm(SET_IO, 0, int(value))
@ -321,13 +363,18 @@ class Motor(HasIodev, Drivable):
def read_baudrate(self):
reply = self.comm(GET_GLOB_PAR, 65)
print('BAUD', reply)
return reply
@Command(FloatRange())
def set_zero(self, value):
self.zero += value - self.read_value()
@Command()
def reset(self):
if self._poserror:
self._poserror = 'reset'
self.write_target(self.encoder)
@Command
def stop(self):
self.comm(MOTOR_STOP)