diff --git a/cfg/main/ma10.cfg b/cfg/main/ma10.cfg index 262a7f1..8a09a12 100644 --- a/cfg/main/ma10.cfg +++ b/cfg/main/ma10.cfg @@ -67,5 +67,6 @@ uri = ma10-ts.psi.ch:3004 description = stick rotation class = secop_psi.phytron.Motor io = om_io +sign = -1 encoder_mode = CHECK diff --git a/cfg/stick/ma10stick.cfg b/cfg/stick/ma10stick.cfg index 948114e..0fa393e 100644 --- a/cfg/stick/ma10stick.cfg +++ b/cfg/stick/ma10stick.cfg @@ -10,7 +10,7 @@ service = stick [ts] class = secop_psi.sea.SeaReadable -iodev = sea_stick +io = sea_stick sea_object = tt json_file = ma10.config.json rel_paths = ts diff --git a/secop_psi/phytron.py b/secop_psi/phytron.py index 13cb70b..805c940 100644 --- a/secop_psi/phytron.py +++ b/secop_psi/phytron.py @@ -60,7 +60,8 @@ class Motor(PersistentMixin, HasIO, Drivable): speed = Parameter('', FloatRange(0, 20, unit='deg/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) - 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')) sameside_offset = Parameter('offset when always approaching from the same side', FloatRange(unit='deg'), readonly=False, default=0) @@ -86,7 +87,7 @@ class Motor(PersistentMixin, HasIO, Drivable): def read_value(self): 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': enc = self.read_encoder() else: @@ -111,7 +112,7 @@ class Motor(PersistentMixin, HasIO, Drivable): else: if self._sameside_pending: # drive to real target - self.set('A', self.target - self.zero) + self.set('A', self.sign * (self.target + self.offset)) self._sameside_pending = False return pos if (self.encoder_mode == 'CHECK' and @@ -131,7 +132,7 @@ class Motor(PersistentMixin, HasIO, Drivable): def read_encoder(self): if self.encoder_mode == 'NO': return self.value - return float(self.get('P22R')) + self.zero + return float(self.get('P22R')) * self.sign - self.offset def read_speed(self): return float(self.get('P14R')) / self.speed_factor @@ -158,14 +159,14 @@ class Motor(PersistentMixin, HasIO, Drivable): # 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) + self.set('A', self.sign * (value + self.offset + self.sameside_offset)) else: - self.set('A', value - self.zero) + self.set('A', self.sign * (value + self.offset)) self.setFastPoll(True, self.fast_poll) return value - def write_zero(self, value): - self.zero = value + def write_offset(self, value): + self.offset = value self.saveParameters() return Done @@ -177,16 +178,16 @@ class Motor(PersistentMixin, HasIO, Drivable): """reset error, set position to encoder""" self.read_value() if self.status[0] == self.Status.ERROR: - enc = self.encoder - self.zero - pos = self.value - self.zero + enc = self.encoder + self.offset + pos = self.value + self.offset if abs(enc - pos) > self.encoder_tolerance: if enc < 0: while enc < 0: - self.zero -= 360 + self.offset += 360 enc += 360 - self.set('P22S', enc) + self.set('P22S', enc * self.sign) self.saveParameters() - self.set('P20S', enc) # set pos to encoder + self.set('P20S', enc * self.sign) # set pos to encoder self.read_value() # self.status = self.Status.IDLE, ''