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

add sign to phytron motor

Browse Source 
+ fixes in ma10 config
This commit is contained in:
zolliker 2022-05-19 17:58:54 +02:00
parent 7b9d2d1701
commit 0972e8c7e8
3 changed files with 16 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cfg/main/ma10.cfg
View File

@ -67,5 +67,6 @@ uri = ma10-ts.psi.ch:3004
description = stick rotation description = stick rotation
class = secop_psi.phytron.Motor class = secop_psi.phytron.Motor
io = om_io io = om_io
sign = -1
encoder_mode = CHECK encoder_mode = CHECK

2
cfg/stick/ma10stick.cfg
View File

@ -10,7 +10,7 @@ service = stick
[ts] [ts]
class = secop_psi.sea.SeaReadable class = secop_psi.sea.SeaReadable
iodev = sea_stick io = sea_stick
sea_object = tt sea_object = tt
json_file = ma10.config.json json_file = ma10.config.json
rel_paths = ts rel_paths = ts

27
secop_psi/phytron.py
View File

@ -60,7 +60,8 @@ class Motor(PersistentMixin, HasIO, Drivable):
speed = Parameter('', FloatRange(0, 20, unit='deg/s'), readonly=False) speed = Parameter('', FloatRange(0, 20, unit='deg/s'), readonly=False)
accel = Parameter('', FloatRange(2, 250, unit='deg/s/s'), readonly=False) accel = Parameter('', FloatRange(2, 250, unit='deg/s/s'), readonly=False)
encoder_tolerance = Parameter('', FloatRange(unit='deg'), readonly=False, default=0.01) encoder_tolerance = Parameter('', FloatRange(unit='deg'), readonly=False, default=0.01)
zero = PersistentParam('', FloatRange(unit='deg'), readonly=False, default=0) offset = PersistentParam('', FloatRange(unit='deg'), readonly=False, default=0)
sign = PersistentParam('', IntRange(-1,1), readonly=False, default=1)
encoder = Parameter('encoder reading', FloatRange(unit='deg')) encoder = Parameter('encoder reading', FloatRange(unit='deg'))
sameside_offset = Parameter('offset when always approaching from the same side', sameside_offset = Parameter('offset when always approaching from the same side',
FloatRange(unit='deg'), readonly=False, default=0) FloatRange(unit='deg'), readonly=False, default=0)
@ -86,7 +87,7 @@ class Motor(PersistentMixin, HasIO, Drivable):
def read_value(self): def read_value(self):
prev_enc = self.encoder prev_enc = self.encoder
pos = float(self.get('P20R')) + self.zero pos = float(self.get('P20R')) * self.sign - self.offset
if self.encoder_mode != 'NO': if self.encoder_mode != 'NO':
enc = self.read_encoder() enc = self.read_encoder()
else: else:
@ -111,7 +112,7 @@ class Motor(PersistentMixin, HasIO, Drivable):
else: else:
if self._sameside_pending: if self._sameside_pending:
# drive to real target # drive to real target
self.set('A', self.target - self.zero) self.set('A', self.sign * (self.target + self.offset))
self._sameside_pending = False self._sameside_pending = False
return pos return pos
if (self.encoder_mode == 'CHECK' and if (self.encoder_mode == 'CHECK' and
@ -131,7 +132,7 @@ class Motor(PersistentMixin, HasIO, Drivable):
def read_encoder(self): def read_encoder(self):
if self.encoder_mode == 'NO': if self.encoder_mode == 'NO':
return self.value return self.value
return float(self.get('P22R')) + self.zero return float(self.get('P22R')) * self.sign - self.offset
def read_speed(self): def read_speed(self):
return float(self.get('P14R')) / self.speed_factor return float(self.get('P14R')) / self.speed_factor
@ -158,14 +159,14 @@ class Motor(PersistentMixin, HasIO, Drivable):
# drive first to target + sameside_offset # drive first to target + sameside_offset
# we do not optimize when already driving from the right side # we do not optimize when already driving from the right side
self._sameside_pending = True self._sameside_pending = True
self.set('A', value - self.zero + self.sameside_offset) self.set('A', self.sign * (value + self.offset + self.sameside_offset))
else: else:
self.set('A', value - self.zero) self.set('A', self.sign * (value + self.offset))
self.setFastPoll(True, self.fast_poll) self.setFastPoll(True, self.fast_poll)
return value return value
def write_zero(self, value): def write_offset(self, value):
self.zero = value self.offset = value
self.saveParameters() self.saveParameters()
return Done return Done
@ -177,16 +178,16 @@ class Motor(PersistentMixin, HasIO, Drivable):
"""reset error, set position to encoder""" """reset error, set position to encoder"""
self.read_value() self.read_value()
if self.status[0] == self.Status.ERROR: if self.status[0] == self.Status.ERROR:
enc = self.encoder - self.zero enc = self.encoder + self.offset
pos = self.value - self.zero pos = self.value + self.offset
if abs(enc - pos) > self.encoder_tolerance: if abs(enc - pos) > self.encoder_tolerance:
if enc < 0: if enc < 0:
while enc < 0: while enc < 0:
self.zero -= 360 self.offset += 360
enc += 360 enc += 360
self.set('P22S', enc) self.set('P22S', enc * self.sign)
self.saveParameters() self.saveParameters()
self.set('P20S', enc) # set pos to encoder self.set('P20S', enc * self.sign) # set pos to encoder
self.read_value() self.read_value()
# self.status = self.Status.IDLE, '' # self.status = self.Status.IDLE, ''