116 lines
4.1 KiB
Python
116 lines
4.1 KiB
Python
# -*- coding: utf-8 -*-
|
|
# *****************************************************************************
|
|
#
|
|
# This program is free software; you can redistribute it and/or modify it under
|
|
# the terms of the GNU General Public License as published by the Free Software
|
|
# Foundation; either version 2 of the License, or (at your option) any later
|
|
# version.
|
|
#
|
|
# This program is distributed in the hope that it will be useful, but WITHOUT
|
|
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
|
|
# FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
|
|
# details.
|
|
#
|
|
# You should have received a copy of the GNU General Public License along with
|
|
# this program; if not, write to the Free Software Foundation, Inc.,
|
|
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
#
|
|
# Module authors:
|
|
# M. Zolliker <markus.zolliker@psi.ch>
|
|
#
|
|
# *****************************************************************************
|
|
"""use transducer and motor to adjust force"""
|
|
|
|
import time
|
|
from secop.core import Drivable, Parameter, FloatRange, Done, Attached, Command
|
|
|
|
|
|
class Uniax(Drivable):
|
|
motor = Attached()
|
|
transducer = Attached()
|
|
step = Parameter('maximum motor step', FloatRange(unit='deg'), default=5, readonly=False)
|
|
fine_step = Parameter('motor step for fine adjustment', FloatRange(unit='deg'), default=1, readonly=False)
|
|
tolerance = Parameter('force tolerance', FloatRange(), readonly=False, default=1)
|
|
filter_interval = Parameter('filter time', FloatRange(0, 60, unit='s'), readonly=False, default=1)
|
|
span = Parameter('difference between max and min', FloatRange(), needscfg=False)
|
|
pollinterval = 0.1
|
|
fast_pollfactor = 1
|
|
|
|
_target = None
|
|
_mot_target = None
|
|
_direction = 0
|
|
_last_era = 0
|
|
_cnt = 0
|
|
_sum = 0
|
|
_min = None
|
|
_max = None
|
|
_last_min = None
|
|
_last_max = None
|
|
|
|
def read_value(self):
|
|
force = self._transducer
|
|
mot = self._motor
|
|
era = int(time.time() / self.filter_interval)
|
|
newera = era != self._last_era
|
|
value = force.read_value()
|
|
if newera:
|
|
self._last_era = era
|
|
if self._min is None:
|
|
self._min = self._max = self._last_max = self._last_min = value
|
|
if self._cnt > 0:
|
|
self.value = self._sum / self._cnt
|
|
self._sum = self._cnt = 0
|
|
self.span = self._max - self._min
|
|
self._last_min = self._min
|
|
self._last_max = self._max
|
|
self._min = self._max = value
|
|
self._min = min(self._min, value)
|
|
self._max = max(self._max, value)
|
|
self._sum += value
|
|
self._cnt += 1
|
|
high_value = max(self._max, self._last_max)
|
|
low_value = min(self._min, self._last_min)
|
|
if self._target is None:
|
|
if mot.isBusy():
|
|
self.status = self.Status.IDLE, 'stopping'
|
|
else:
|
|
self.status = self.Status.IDLE, ''
|
|
return Done
|
|
step = 0
|
|
if self._direction > 0:
|
|
if high_value < self._target:
|
|
step = self.step
|
|
else:
|
|
if low_value > self._target:
|
|
step = -self.step
|
|
if not step:
|
|
if self._direction * (self._target - value) < self.tolerance:
|
|
self.stop()
|
|
self.status = self.Status.IDLE, 'target reached'
|
|
return Done
|
|
if newera:
|
|
step = self.fine_step * self._direction
|
|
if step and not mot.isBusy():
|
|
mot_target = mot.value + step
|
|
self._mot_target = mot.write_target(mot_target)
|
|
return Done
|
|
|
|
def write_target(self, target):
|
|
self._target = target
|
|
if target - self.value > 0:
|
|
self._direction = 1
|
|
else:
|
|
self._direction = -1
|
|
if self._motor.status[0] == self.Status.ERROR:
|
|
self._motor.reset()
|
|
self.status = self.Status.BUSY, 'changed target'
|
|
self._mot_target = self._motor.target
|
|
self.read_value()
|
|
return target
|
|
|
|
@Command()
|
|
def stop(self):
|
|
self._target = None
|
|
self._motor.stop()
|
|
self.status = self.Status.IDLE, 'stopped'
|