pypid/controller.py

   1 # Copyright (C) 2011-2012 W. Trevor King <wking@tremily.us>
   2 #
   3 # This file is part of pypid.
   4 #
   5 # pypid is free software: you can redistribute it and/or modify it under the
   6 # terms of the GNU General Public License as published by the Free Software
   7 # Foundation, either version 3 of the License, or (at your option) any later
   8 # version.
   9 #
  10 # pypid is distributed in the hope that it will be useful, but WITHOUT ANY
  11 # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
  12 # A PARTICULAR PURPOSE.  See the GNU General Public License for more details.
  13 #
  14 # You should have received a copy of the GNU General Public License along with
  15 # pypid.  If not, see <http://www.gnu.org/licenses/>.
  16
  17 import time as _time
  18
  19 from aubio.aubioclass import pitch as _pitch
  20 from aubio.aubioclass import fvec as _fvec
  21 from numpy import array as _array
  22 from numpy import exp as _exp
  23 from numpy import linspace as _linspace
  24 from numpy import log as _log
  25 from scipy.interpolate import interp1d as _interp1d
  26
  27 from . import LOG as _LOG
  28 from .fit import ModelFitter as _ModelFitter
  29
  30
  31 class Controller (object):
  32     """PID control frontend.
  33
  34     backend: pypid.backend.Backend instance
  35         backend driving your particular harware
  36     setpoint: float
  37         initial setpoint in PV-units
  38     min: float
  39         minimum PV in PV-units (for sanity checks)
  40     max: float
  41         maximum PV in PV-units (for sanity checks)
  42     """
  43     def __init__(self, backend, setpoint=0.0, min=-float('inf'),
  44                  max=float('inf')):
  45         self._backend = backend
  46         self._setpoint = setpoint
  47         self._min_pv = min
  48         self._max_pv = max
  49
  50     # basic user interface methods
  51
  52     def get_pv(self):
  53         """Return the current process variable in PV-units
  54
  55         We should expose this to users, so they don't need to go
  56         mucking about in `._backend`.
  57         """
  58         return self._backend.get_pv()
  59
  60     def set_pv(self, setpoint, **kwargs):
  61         """Change setpoint to `setpoint` and wait for stability
  62         """
  63         self._backend.set_setpoint(setpoint)
  64         self.wait_for_stability(setpoint=setpoint, **kwargs)
  65
  66     def wait_for_stability(self, setpoint, tolerance=0.3, time=10.,
  67                            timeout=-1, sleep_time=0.1, return_data=False):
  68         """Wait until the PV is sufficiently stable
  69
  70         setpoint : float
  71             target PV in PV-units
  72         tolerance : float
  73             maximum allowed deviation from `setpoint` in PV-units
  74         time : float
  75             time the PV must remain in the allowed region before the
  76             signal is delared "stable"
  77         timeout : float
  78             maximum time to wait for stability.  Set to -1 to never
  79             timeout.
  80         sleep_time : float
  81             time in seconds to sleep between reads to avoid an
  82             overly-busy loop
  83         return_data : boolean
  84             if true, also return a list of `(timestamp, PV)` tuples
  85             read while waiting
  86
  87         Read the PV every `sleep_time` seconds until the PV has
  88         remained within `tolerance` of `setpoint` for `time`.  If the
  89         stability criteria are met, return `True` (stable).  If
  90         `timeout` seconds pass before the criteria are met, return
  91         `False` (not stable).
  92         """
  93         _LOG.debug(('wait until the PV is stable at {:n} +/- {:n} C '
  94                     'for {:n} seconds').format(setpoint, tolerance, time))
  95         stable = False
  96         if return_data:
  97             data = []
  98         start_time = _time.time()
  99         stable_time = start_time + time
 100         if timeout < 0:
 101             timeout_time = None
 102         else:
 103             timeout_time = start_time + timeout
 104         while True:
 105             PV = self.get_pv()
 106             in_range = abs(PV - setpoint) < tolerance
 107             t = _time.time()
 108             if return_data:
 109                 data.append((t, PV))
 110             if in_range:
 111                 if t >= stable_time:
 112                     _LOG.debug('process variable is stable')
 113                     stable = True
 114                     break  # in range for long enough
 115             else:
 116                 stable_time = t + time  # reset target time
 117             if timeout_time and t > timeout_time:
 118                 _LOG.debug('process variable is not stable')
 119                 break  # timeout
 120             _time.sleep(sleep_time)
 121         if return_data:
 122             return (stable, data)
 123         return stable
 124
 125     def is_stable(self, setpoint, time, **kwargs):
 126         return self.wait_for_stability(
 127             setpoint=setpoint, time=time, timeout=time, **kwargs)
 128
 129     def estimate_pv_sensitivity(self, values=10, sleep_time=0.1,
 130                                 max_repeats=10):
 131         PVs = []
 132         last_PV = None
 133         repeats = 0
 134         while True:
 135             PV = self.get_pv()
 136             if repeats == max_repeats:
 137                 last_PV = None
 138             if PV == last_PV:
 139                 repeats += 1
 140             else:
 141                 PVs.append(PV)
 142                 if len(PVs) > values:
 143                     break
 144                 repeats = 0
 145                 last_PV = PV
 146             _time.sleep(sleep_time)
 147         PVs = _array(PVs)
 148         return PVs.std()
 149
 150     # debugging methods
 151
 152     def check_feedback_terms(self):
 153         """Check a backend's interpretation of its PID feedback terms.
 154
 155         Some backends provide an interface to read out their PID
 156         feedback terms, but the interface is not always well
 157         documented.  This method reads out the terms, and compares
 158         them with our own calculations (when possible) to test the
 159         backend's interpretation.
 160         """
 161         c = self._backend.get_current()
 162         pid,prop,ntgrl,deriv = self._backend.get_feedback_terms()
 163         PV = self.get_pv()
 164         SP = self._backend.get_setpoint()
 165         if PV > SP:
 166             p,i,d = self._backend.get_down_gains()
 167         else:
 168             p,i,d = self._backend.get_up_gains()
 169         _LOG.info(('pid(read) {:n} =? sum(calc from terms) {:n} '
 170                    '=? cur(read) {:n} A').format(pid, prop+ntgrl+deriv, c))
 171         _LOG.info('read: p {:n}, i {:n}, d {:n}'.format(p,i,d))
 172         _LOG.info('calc: p {:n}'.format(p*(SP-PV)))
 173
 174     # tuning experiments and data processing
 175
 176     def get_step_response(self, mv_a, mv_b, sleep_time=0.1, stable_time=10.,
 177                           **kwargs):
 178         "Measure a step response for later analysis"
 179         _LOG.debug('measure step response')
 180         if 'time' in kwargs:
 181             raise ValueError(kwargs)
 182         kwargs['time'] = stable_time
 183         kwargs['sleep_time'] = sleep_time
 184         mode = self._backend.get_mode()
 185         if mode == 'manual':
 186             manual_mv = self._backend.get_mv()
 187         else:
 188             self._backend.set_mode('manual')
 189         _LOG.debug('set first current and wait for stability')
 190         self._backend.set_mv(mv_a)
 191         pv_a = self.get_pv()
 192         while not self.is_stable(pv_a, **kwargs):
 193             pv_a = self.get_pv()
 194         _LOG.debug('stabilized at {:n} {} with {:n} {}'.format(
 195                 pv_a, self._backend.pv_units, mv_a, self._backend.mv_units))
 196         _LOG.debug('set second MV and wait for stability')
 197         data = []
 198         start_time = _time.time()
 199         self._backend.set_mv(mv_b)
 200         pv_b = pv_a
 201         while True:
 202             stable,d = self.is_stable(pv_b, return_data=True, **kwargs)
 203             data.extend(d)
 204             pv_b = self.get_pv()
 205             if stable:
 206                 break
 207         _LOG.debug('stabilized at {:n} {} with {:n} {}'.format(
 208                 pv_b, self._backend.pv_units, mv_b, self._backend.mv_units))
 209         if mode == 'manual':
 210             self._backend.set_mv(manual_mv)
 211         else:
 212             self._backend.set_mode(mode)
 213         return data
 214
 215     @staticmethod
 216     def analyze_step_response(step_response, mv_shift):
 217         rates = [(PVb-PVa)/(tb-ta) for ((ta,PVa),(tb,PVb))
 218                  in zip(step_response, step_response[1:])]
 219         # TODO: averaging filter?
 220         max_rate_i = max_rate = 0
 221         for i,rate in enumerate(rates):
 222             if abs(rate) > max_rate:  # handle steps in both directions
 223                 max_rate_i = i
 224                 max_rate = abs(rate)
 225         max_rate_time,max_rate_temp = step_response[max_rate_i]  # TODO: avg i and i+1?
 226         time_a,PV_a = step_response[0]
 227         max_rate_time -= time_a
 228         dead_time = max_rate_time - (max_rate_temp - PV_a) / max_rate
 229         t_data = _array([t for t,PV in step_response[max_rate_i:]])
 230         PV_data = _array([PV for t,PV in step_response[max_rate_i:]])
 231         model = ExponentialModel(PV_data, info={'x data (s)': t_data})
 232         decay_time,PV0,PV8 = model.fit()
 233         process_gain = (PV8 - PV_a) / mv_shift
 234         return (process_gain, dead_time, decay_time, max_rate)
 235
 236     def get_bang_bang_response(self, dead_band=0.8, num_oscillations=10,
 237                                max_dead_band_time=30, sleep_time=0.1):
 238         orig_down_gains = self._backend.get_down_gains()
 239         orig_up_gains = self._backend.get_up_gains()
 240         _LOG.debug('measure bang-bang response')
 241         mode = self._backend.get_mode()
 242         if mode != 'PID':
 243             self._backend.set_mode('PID')
 244         i=0
 245         setpoint = self._backend.get_setpoint()
 246         self._backend.set_down_gains(float('inf'), float('inf'), 0)
 247         self._backend.set_up_gains(float('inf'), float('inf'), 0)
 248         start_time = _time.time()
 249         pv = self.get_pv()
 250         under_first = self._is_under(
 251             pv=pv, setpoint=setpoint, dead_band=dead_band)
 252         _LOG.debug('wait to exit dead band')
 253         t = start_time
 254         while under_first is None:
 255             if t - start_time > max_dead_band_time:
 256                 msg = 'still in dead band after after {:n} seconds'.format(
 257                     max_dead_band_time)
 258                 _LOG.error(msg)
 259                 raise ValueError(msg)
 260             _time.sleep(sleep_time)
 261             t = _time.time()
 262             pv = t.get_pv()
 263             under_first = self._is_under(
 264                 pv=pv, setpoint=setpoint, dead_band=dead_band)
 265         _LOG.debug('read {:d} oscillations'.format(num_oscillations))
 266         data = []
 267         under = under_first
 268         while i < num_oscillations*2 + 1:
 269             t = _time.time()
 270             pv = self.get_pv()
 271             # drop first half cycle (possibly includes ramp to setpoint)
 272             if i > 0:
 273                 data.append((t, pv))
 274             _under = self._is_under(
 275                 temp=temp, setpoint=setpoint, dead_band=dead_band)
 276             if _under is True and under is False:
 277                 _LOG.debug('transition to PV < SP (i={:d})'.format(i))
 278                 under = _under
 279                 i += 1
 280             elif under is False and is_under is True:
 281                 _LOG.debug('transition to PV > SP (i={:d})'.format(i))
 282                 under = _under
 283                 i += 1
 284             _time.sleep(sleep_time)
 285         self._backend.set_down_gains(*orig_down_gains)
 286         self._backend.set_up_gains(*orig_up_gains)
 287         if mode != 'PID':
 288             self._backend.set_mode(mode)
 289         return data
 290
 291     @staticmethod
 292     def analyze_bang_bang_response(bang_bang_response):
 293         t_data = _array([t for t,PV in bang_bang_response])
 294         PV_data = _array([PV for t,PV in bang_bang_response])
 295         amp = (PV_data.max() - PV_data.min()) / 2
 296         freq = Controller._get_frequency(x_data=t_data, y_data=PV_data)
 297         period = 1./freq
 298         return (amp, period)
 299
 300     def get_ultimate_cycle_response(self, proportional, period):
 301         orig_down_gains = self._backend.get_down_gains()
 302         orig_up_gains = self._backend.get_up_gains()
 303         _LOG.debug('measure ultimate cycle response')
 304         mode = self._backend.get_mode()
 305         if mode != 'PID':
 306             self._backend.set_mode('PID')
 307         # TODO...
 308         self._backend.set_down_gains(*orig_down_gains)
 309         self._backend.set_up_gains(*orig_up_gains)
 310         if mode != 'PID':
 311             self._backend.set_mode(mode)
 312         return data
 313
 314     @staticmethod
 315     def analyze_ultimate_cycle_response(ultimate_cycle_response):
 316         amp,period = Controller.analyze_bang_bang_response(
 317             ultimate_cycle_response)
 318         return period
 319
 320     # utility methods
 321
 322     def _wait_until_close(self, setpoint, tolerance=0.3, sleep_time=0.1):
 323         while abs(self.get_pv() - setpoint) > tolerance:
 324             _time.sleep(sleep_time)
 325
 326     def _time_function(self, function, args=(), kwargs=None, count=10):
 327         "Rough estimate timing of get_temp(), takes me about 0.1s"
 328         if kwargs is None:
 329             kwargs = {}
 330         start = _time.time()
 331         for i in range(count):
 332             function(*args, **kwargs)
 333         stop = _time.time()
 334         return float(stop-start)/count
 335
 336     def _is_under(self, pv=None, setpoint=None, dead_band=None):
 337         if pv is None:
 338             pv = self.get_pv()
 339         if setpoint is None:
 340             setpoint = self._backend.get_setpoint()
 341         low_pv = high_pv = setpoint
 342         if dead_band:
 343             low_pv -= dead_band
 344             high_pv += dead_band
 345         if pv < low_pv:
 346             return True
 347         elif pv > high_pv:
 348             return False
 349         return None
 350
 351     def _select_parameter(self, under_result=None, over_result=None,
 352                           dead_band_result=None, **kwargs):
 353         under = self._is_under(**kwargs)
 354         if under:
 355             return under_result
 356         elif under is False:
 357             return over_result
 358         return dead_band_result
 359
 360     @staticmethod
 361     def _resample_with_constant_dx(x_data, y_data):
 362         f = _interp1d(x_data, y_data)
 363         x = _linspace(x_data[0], x_data[-1], len(x_data))
 364         y = f(x)
 365         return x, y
 366
 367     @staticmethod
 368     def _get_frequency(x_data, y_data):
 369         x,y = Controller._resample_with_constant_dx(x_data, y_data)
 370         dx = x[1] - x[0]
 371         yvec = _fvec(len(y_data))
 372         mean = y.mean()
 373         for i,_y in enumerate(y_data):
 374             yvec.set(_y - mean, i)
 375         fake_sample_rate = 8000  # aubio is built for audio
 376         p = _pitch(mode='schmitt', bufsize=len(y_data), hopsize=len(y_data),
 377                    samplerate=fake_sample_rate, omode='freq', tolerance=0.1)
 378         freq = p(yvec) / (fake_sample_rate * dx)
 379         _LOG.debug('pitch: {:n}, sample rate {:n}'.format(freq, 1./dx))
 380         del(p)
 381         del(yvec)
 382         return freq
 383
 384     @staticmethod
 385     def _check_range(value, min, max):
 386         if value < min:
 387             raise ValueError('%g < %g' % (value, min))
 388         if value > max:
 389             raise ValueError('%g > %g' % (value, max))
 390
 391     def _check_pv(pv):
 392         self._check_rangee(pv, self._min_pv, self._max_pv)
 393
 394
 395 class ExponentialModel (_ModelFitter):
 396     "Exponential decay model"
 397     def model(self, params):
 398         tau,y0,y8 = params
 399         x_data = self.info['x data (s)']
 400         x0 = x_data[0]  # raw times in seconds are too far from the epoc
 401         a = 1 - y0/y8
 402         self._model_data[:] = y8*(1-a*_exp(-(x_data-x0)/tau))
 403         return self._model_data
 404
 405     def guess_initial_params(self, outqueue=None, **kwargs):
 406         x_data = self.info['x data (s)']
 407         y_data = self._data
 408         y8 = y_data[-1]
 409         x_mid = x_data[int(len(x_data)/2)]
 410         y_mid = y_data[int(len(y_data)/2)]
 411         x_start = x_data[0]
 412         y_start = y_data[0]
 413         tau = (x_mid - x_start)/_log((y_start-y8)/(y_mid-y8))
 414         return (tau, y_start, y8)
 415
 416     def guess_scale(self, params, outqueue=None, **kwargs):
 417         return (1., 1., 1.)