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

sameside approaching

Browse Source 
Change-Id: I8b73d0ae0475bc727b49edd6f245c2204c51695b
This commit is contained in:
zolliker 2021-10-06 09:08:29 +02:00
parent 5b7f4604ad
commit 0d3df7be92
1 changed files with 38 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
secop_psi/phytron.py
View File

@ -48,11 +48,16 @@ class Motor(PersistentMixin, HasIodev, Drivable):
value = Parameter('angle', FloatRange(unit='deg'), poll=True)
target = Parameter('target angle', FloatRange(unit='deg'), readonly=False)
speed = Parameter('', FloatRange(0, 20, unit='deg/s'), readonly=False, poll=True)
accel = Parameter('', FloatRange(2, 250, unit='deg/s/s'), readonly=False, poll=True)
encoder_tolerance = Parameter('', FloatRange(unit='deg'), readonly=False, default=0.01)
zero = PersistentParam('', FloatRange(unit='deg'), readonly=False, default=0)
encoder = Parameter('encoder reading', FloatRange(unit='deg'), poll=True)
sameside_offset = Parameter('offset when always approaching from the same side',
FloatRange(unit='deg'), readonly=True, default=0)
iodevClass = PhytronIO
fast_pollfactor = 0.02
_sameside_pending = False
def earlyInit(self):
self.loadParameters()
@ -68,6 +73,7 @@ class Motor(PersistentMixin, HasIodev, Drivable):
return self.get(query)
def read_value(self):
prev_enc = self.encoder
pos = float(self.get('P20R')) + self.zero
if self.encoder_mode != 'NO':
enc = self.read_encoder()
@ -75,13 +81,22 @@ class Motor(PersistentMixin, HasIodev, Drivable):
enc = pos
status = self.get('=H')
if status == 'N':
if (self.encoder_mode == 'CHECK' and
abs(enc - pos) > self.speed * 0.1 + self.encoder_tolerance):
self.get('SN') # stop
self.status = self.Status.ERROR, 'encoder does not match pos'
if self.encoder_mode == 'CHECK':
e1, e2 = sorted((prev_enc, enc))
if e1 - self.encoder_tolerance <= pos <= e2 + self.encoder_tolerance:
self.status = self.Status.BUSY, 'driving'
else:
self.log.error('encoder lag: %g not within %g..%g' % (pos, e1, e2))
self.get('S') # stop
self.status = self.Status.ERROR, 'encoder lag error'
else:
self.status = self.Status.BUSY, 'driving'
else:
if self._sameside_pending:
# drive to real target
self.set('A', self.target - self.zero)
self._sameside_pending = False
return pos
if (self.encoder_mode == 'CHECK' and
abs(enc - pos) > self.encoder_tolerance):
self.status = self.Status.ERROR, 'encoder does not match pos'
@ -102,11 +117,26 @@ class Motor(PersistentMixin, HasIodev, Drivable):
raise HardwareError('speed factor does not match')
return float(self.set_get('P14S', int(value * self.speed_factor), 'P14R')) / self.speed_factor
def read_accel(self):
return float(self.get('P15R')) / self.speed_factor
def write_accel(self, value):
if abs(float(self.get('P03R')) * self.speed_factor - 1) > 0.001:
raise HardwareError('speed factor does not match')
return float(self.set_get('P15S', int(value * self.speed_factor), 'P15R')) / self.speed_factor
def write_target(self, value):
if self.status[0] == self.Status.ERROR:
raise HardwareError('need reset')
self.status = self.Status.BUSY, 'changed target'
if self.sameside_offset:
# drive first to target + sameside_offset
# we do not optimize when already driving from the right side
self._sameside_pending = True
self.set('A', value - self.zero + self.sameside_offset)
else:
self.set('A', value - self.zero)
return value
def write_zero(self, value):
self.zero = value
@ -114,7 +144,7 @@ class Motor(PersistentMixin, HasIodev, Drivable):
return Done
def stop(self):
self.get('SN')
self.get('S')
@Command
def reset(self):
@ -130,7 +160,8 @@ class Motor(PersistentMixin, HasIodev, Drivable):
self.set('P22S', enc)
self.saveParameters()
self.set('P20S', enc) # set pos to encoder
self.status = self.Status.IDLE, ''
self.read_value()
# self.status = self.Status.IDLE, ''
# TODO:
# '=E' electronics status