SwissMX/epics_widgets/MotorTweak.py

import logging
_log = logging.getLogger(__name__)

import math, os.path
from time import sleep
from PyQt5.QtCore import Qt, pyqtSignal
from PyQt5.QtGui import QPainter, QBrush, QColor, QPainterPath, QPen, QDoubleValidator
from PyQt5.QtWidgets import QMenu, QInputDialog, QAction, QToolTip
from PyQt5.uic import loadUiType
from epics import Motor

from app_utils import assert_tweaker_positions
Ui_MotorTweak, QWidget = loadUiType(os.path.join(os.path.dirname(__file__),'MotorTweak.ui'))
SPMG_STOP = 0
SPMG_PAUSE = 1
SPMG_MOVE = 2
SPMG_GO = 3

class MotorTweak(QWidget, Ui_MotorTweak):
  event_val = pyqtSignal(str, dict)
  event_rbv = pyqtSignal(str, float, dict)
  event_soft_limit = pyqtSignal(str, dict)
  event_high_hard_limit = pyqtSignal(str, dict)
  event_low_hard_limit = pyqtSignal(str, dict)
  event_axis_fault = pyqtSignal(str, dict)

  ui_set_drive_val = pyqtSignal(str)
  ui_update_label = pyqtSignal()
  ui_update_validator = pyqtSignal()

  def __init__(self, parent=None):
    super(MotorTweak, self).__init__(parent=parent)
    self.setupUi(self)
    self.ui_set_drive_val.connect(self._drive_val.setText, Qt.QueuedConnection)
    self.ui_update_label.connect(self._update_label_ui, Qt.QueuedConnection)
    self.ui_update_validator.connect(self._set_motor_validator_ui, Qt.QueuedConnection)
    self._wheel_tweaks = True
    self._auto_labels = True
    self._locked = False
    self._max_step = None
    self._label_style = 'basic'
    self._templates_source = {
      'basic': '<b>{label}</b> <font color="#080">{{rbv:.{precision}f}} {units}</font>',
      'small': '<small>{label} <font size="small" color="#080">{{rbv:.{precision}f}} {units}</font><small>',
      '2 lines': '<b>{label}</b><br><font size="small" color="#080">{{rbv:.{precision}f}} {units}</font>',
      'busy': '<b>{label}</b> <font color="#080">{{rbv:.{precision}f}} {units}</font>'
    }
    self._templates = {}


  def connect_motor(self, rec_name, **kwargs):
    # TODO: DO NOT USE Motor base class, but reduce to the only really needed PV: s.a. class QopticZoom(object)
    # TODO: have a own motor Class as class SimMotor:
    label=kwargs['label']
    self._label=label
    m = Motor(rec_name)
    m.get_position()
    self._ignore_limits = m.HLM == m.LLM  # if both high/low limits are equal they are meaningless
    self._motor = m
    self._rec_name = rec_name

    for attr in ['RTYP', 'JVEL', 'HLS', 'LLS', 'TWV', 'RBV', 'VAL', 'LVIO', 'HLM', 'LLM']:
      # _log.debug('connecting to field {}'.format(attr))
      m.PV(attr, connect=True)

    self.set_motor_validator()
    self._drive_val.setText(m.get('VAL', as_string=True))
    self._drive_val.returnPressed.connect(self.move_abs)

    tweak_min = kwargs.get("tweak_min", 0.0001)
    tweak_max = kwargs.get("tweak_max", 10.0)
    self._tweak_val.setText(m.get('TWV', as_string=True))
    self._tweak_val.setValidator(QDoubleValidator(tweak_min, tweak_max, 4, self._tweak_val))
    self._tweak_val.editingFinished.connect(lambda m=m: m.PV('TWV').put(self._tweak_val.text()))

    self.jog_reverse.hide()
    self.jog_forward.hide()
    # self.jog_forward.pressed.connect(lambda: self.jog('forward', True))
    # self.jog_reverse.pressed.connect(lambda: self.jog('reverse', True))
    # self.jog_forward.released.connect(lambda: self.jog('forward', False))
    # self.jog_reverse.released.connect(lambda: self.jog('reverse', False))

    self.tweak_forward.clicked.connect(lambda x: self.tweak('forward'))
    self.tweak_reverse.clicked.connect(lambda x: self.tweak('reverse'))

    self.bind_wheel()

    self.update_label_template()
    m.add_callback('VAL', self.set_val)
    m.add_callback('RBV', self.update_label)
    m.set_callback('HLS', self.update_label)
    m.set_callback('LLS', self.update_label)

    m.set_callback('VAL', self.emit_signals, {'source_field': 'VAL'})
    m.set_callback('RBV', self.emit_signals, {'source_field': 'RBV'})
    m.set_callback('HLS', self.emit_signals, {'source_field': 'HLS'})
    m.set_callback('LLS', self.emit_signals, {'source_field': 'LLS'})
    m.set_callback('LVIO', self.emit_signals, {'source_field': 'LVIO'})
    m.set_callback('STAT', self.emit_signals, {'source_field': 'STAT'})

    m.set_callback('HLM', self.set_motor_validator)


  def set_val(self, **kw):
    try: #pv-monitor-func
      v = kw['char_value']
      if isinstance(v, bytes):
        v = v.decode('utf-8', errors='ignore')
      _log.debug('updating VAL = {}'.format(v))
      self._val = float(v)
      self.ui_set_drive_val.emit(str(v))
    except Exception as e:
      _log.critical(f'{e}')


  def set_motor_validator(self, **kwargs):
    self.ui_update_validator.emit()

  def _set_motor_validator_ui(self):
    m = self._motor
    lineedit = self._drive_val
    min, max = m.PV('LLM').get(), m.PV('HLM').get()
    if min == max:
      min = -1e6
      max = 1e6
    lineedit.setValidator(QDoubleValidator(min, max, m.PV('PREC').get(), lineedit))


  def set_max_step(self, max_step):
    '''limit the total user move to max_step from the current position (None: no limit)'''
    self._max_step=max_step
    self._ref_pos=None if max_step is None else self.get_rbv()

  def _target_allowed(self, target):
    max_step=self._max_step
    if max_step is None or abs(target-self._ref_pos)<=max_step+1e-9:
      return True
    msg=f'{self._label}: total move {abs(target-self._ref_pos):.4g} exceeds the limit of {max_step:g} during data collection'
    _log.warning(msg)
    QToolTip.showText(self.mapToGlobal(self.rect().center()), msg, self)
    return False

  def tweak(self, direction):
    m=self._motor
    sign=1 if 'forw' in direction else -1
    if self._max_step is None or self._target_allowed(m.get('VAL')+sign*m.tweak_val):
      m.tweak(direction)

  def move_rel(self, dist):
    m=self._motor
    if self._max_step is not None and not self._target_allowed(m.get_position(readback=True)+dist):
      return
    rbv=m.get_position(readback=True)
    try:
      self._val=rbv+dist
    except AttributeError:
      pass
    m.move(self._val, ignore_limits=self._ignore_limits, relative=False)
    #old code move relative to VAL not relative to RBV
    #try:
    #  self._val+=dist
    #except AttributeError:
    #  pass
    #self._motor.move(dist, ignore_limits=True, relative=True)


  def stop(self):
    self._motor.stop()

  def is_done(self):
    m=self._motor
    m.refresh()
    return 1 == m.done_moving

  def get_rbv(self):
    #return self._motor.get('RBV')
    m=self._motor
    return m.get_position(readback=True)

  def get_val(self):
    #return self._motor.get('VAL')
    try:
      v=self._val
    except AttributeError:
      m=self._motor
      self._val=v=m.get('VAL')
    return v

  def move_abs(self, drive=None, wait=False, assert_position=False):
    if drive is None:
      drive = float(self._drive_val.text())
    if assert_position:
      wait=True
    if not self._target_allowed(drive):
      return
    try:
      self._val=drive
    except AttributeError:
      pass
    m=self._motor
    m.move(drive, wait=wait, ignore_limits=self._ignore_limits, relative=False)
    if assert_position:
      assert_tweaker_positions([(self, drive, 0.1)], timeout=1)

  def emit_signals(self, **kw):
    '''
    :param kw: info about the channel {
        'access': 'read-only',
        'char_value': '-0.105',
        'chid': 36984304,
        'count': 1,
        'enum_strs': None,
        'ftype': 20,
        'host': 'SAR-CPPM-EXPMX1.psi.ch:5064',
        'lower_alarm_limit': None,
        'lower_ctrl_limit': 0.0,
        'lower_disp_limit': 0.0,
        'lower_warning_limit': None,
        'motor_field': 'RBV',
        'nelm': 1,
        'precision': 3,
        'pvname': 'SAR-EXPMX:MOT_ROT_Y.RBV',
        'read_access': True,
        'severity': 0,
        'source_field': 'RBV',
        'status': 0,
        'timestamp': 1522314072.878331,
        'type': 'time_double',
        'typefull': 'time_double',
        'units': b'deg',
        'upper_alarm_limit': None,
        'upper_ctrl_limit': 0.0,
        'upper_disp_limit': 0.0,
        'upper_warning_limit': None,
        'value': -0.105,
        'write_access': False}


    :return:
    '''
    try: #pv-monitor-func
      field = kw['motor_field']
      src =  kw['source_field']
      kw['alias'] = self._label
      if field != src:
        return
      if field == 'VAL':
        self.event_val.emit(self._rec_name, kw)
      elif field == 'RBV':
        self.event_rbv.emit(kw['alias'], kw['value'], kw)
      elif field == 'LVIO':
        self.event_soft_limit.emit(self._rec_name, kw)
      elif field == 'HLS':
        self.event_high_hard_limit.emit(self._rec_name, kw)
        self.event_axis_fault.emit(self._rec_name, kw)
      elif field == 'LVIO':
        self.event_low_hard_limit.emit(self._rec_name, kw)
        self.event_axis_fault.emit(self._rec_name, kw)
      elif field == 'STAT':
        self.event_axis_fault.emit(self._rec_name, kw)
    except Exception as e:
      _log.critical(f'{e}')

  def update_label(self, **kwargs):
    try: #pv-monitor-func
      self.ui_update_label.emit()
    except Exception as e:
      _log.critical(f'{e}')

  def _update_label_ui(self):
    try:
      m = self._motor
      self.label.setText(self._templates[self._label_style].format(rbv=m.readback))
      self.jog_forward.setToolTip('jog forward at {:.3f} {}/s'.format(m.jog_speed, m.units))
      self.jog_reverse.setToolTip('jog reverse at {:.3f} {}/s'.format(m.jog_speed, m.units))
      self.tweak_forward.setToolTip('tweak forward by {:.3f} {}'.format(m.tweak_val, m.units))
      self.tweak_reverse.setToolTip('tweak reverse by {:.3f} {}'.format(m.tweak_val, m.units))
    except Exception as e:
      _log.critical(f'{e}')

  def update_jog_speed(self, event):
    m = self._motor
    speed = m.jog_speed
    sign = math.copysign(1, event.angleDelta().y())
    m.jog_speed = speed + (sign * 0.1 * speed)
    self.jog_forward.setToolTip('jog forward at {:.3f} {}/s'.format(m.jog_speed, m.units))
    self.jog_reverse.setToolTip('jog reverse at {:.3f} {}/s'.format(m.jog_speed, m.units))

  def tweak_event(self, event):
    m = self._motor
    sign = event.angleDelta().y()
    if self._max_step is not None and not self._target_allowed(m.get('VAL')+(m.tweak_val if sign>=0 else -m.tweak_val)):
      return
    if sign < 0:
      m.tweak_reverse = 1
    else:
      m.tweak_forward = 1

  def bind_wheel(self):
    self.tweak_forward.wheelEvent = self.tweak_event
    self.tweak_reverse.wheelEvent = self.tweak_event

  def jog(self, direction, enable):
    m = self._motor
    if 'forw' in direction:
      m.jog_forward = int(enable)
    else:
      m.jog_reverse = int(enable)

  def contextMenuEvent(self, event):
    m = self._motor
    menu = QMenu(self)
    menu.setTitle(self._label)

    lockmotor = QAction('lock motor', menu, checkable=True)
    lockmotor.setChecked(self._locked)
    menu.addAction(lockmotor)

    autolabelsAction = QAction('auto', menu, checkable=True)
    autolabelsAction.setChecked(self._auto_labels)
    menu.addAction(autolabelsAction)

    wheel_tweaks = QAction('Mouse wheel tweaks motor', menu, checkable=True)
    wheel_tweaks.setChecked(self._wheel_tweaks)
    menu.addAction(wheel_tweaks)

    stopmotorAction = QAction('Stopped', menu, checkable=True)
    stopmotorAction.setChecked(SPMG_STOP == m.stop_go)
    menu.addAction(stopmotorAction)
    changeprecAction = menu.addAction("Change Precision")
    changejogspeedAction = menu.addAction("Jog speed {:.3f} {}/s".format(m.jog_speed, m.units))
    changetweakstepAction = menu.addAction("Tweak step {:.3f} {}".format(m.tweak_val, m.units))

    tozeroAction = menu.addAction("move to Zero")


    action = menu.exec_(self.mapToGlobal(event.pos()))
    if action == lockmotor:
      self._locked = not self._locked
      if self._locked:
        self._controlbox.setDisabled(True)
      else:
        self._controlbox.setDisabled(False)
    elif action == changeprecAction:
      name = m.NAME
      prec = m.PREC
      msg = 'Precision for motor {}'.format(name)
      _log.debug('prec before %d', prec)
      prec, ok = QInputDialog.getInt(self, msg, msg, prec, 0, 10)
      _log.debug('prec after (%d) %d', ok, prec)
      if ok:
        m.put('PREC', prec, wait=True)

    elif action == changejogspeedAction:
      name = m.NAME
      speed = m.jog_speed
      msg = 'Jog speed for motor {}'.format(name)
      speed, ok = QInputDialog.getDouble(self, msg, msg, speed, 0.01, m.slew_speed, m.precision)
      if ok:
        m.put('JVEL', speed, wait=True)

    elif action == changetweakstepAction:
      name = m.NAME
      tv = m.tweak_val
      msg = 'Tweak step for motor {} at {} {}'.format(name, tv, m.units)
      tv, ok = QInputDialog.getDouble(self, msg, msg, tv, pow(10, -m.precision), m.slew_speed, m.precision)
      if ok:
        m.put('TWV', tv, wait=True)

    elif action == tozeroAction:
      self.move_abs(0.0)

    elif action == stopmotorAction:
      m.stop_go = SPMG_STOP if m.stop_go == SPMG_GO else SPMG_GO

    elif action == autolabelsAction:
      self._auto_labels = not self._auto_labels

    elif action == wheel_tweaks:
      self._wheel_tweaks = not self._wheel_tweaks
      self.bind_wheel()

    self.update_label_template()

  def resizeEvent(self, event):
    return # FIXME disable for the time being
    if not self._auto_labels:
      return

    w, h = event.size().width(), event.size().height()
    if w < 400:
      self.jog_reverse.hide()
      self.jog_forward.hide()
    else:
      self.jog_reverse.show()
      self.jog_forward.show()
    self.update_label()

  def update_label_template(self):
    m = self._motor
    source = self._templates_source
    target = self._templates

    for k in source:
      target[k] = source[k].format(
        label=self._label,
        precision=m.PREC,
        units=m.units)
    try:
      self.label.setText(target[self._label_style].format(rbv=m.readback))
    except ValueError as e:
      _log.error(f'{e}: label_style={self._label_style} readback={m.readback} motor={m}')

  def paintEvent(self, e):
    qp = QPainter()
    qp.begin(self)
    qp.setRenderHint(QPainter.Antialiasing)
    self._draw_limits(qp)
    qp.end()

  def _draw_limits(self, qp):
    m = self._motor
    width, height = self.size().width(), self.size().height()
    pad = 5
    rounding = 2
    size = 10
    if m.HLS:
      x, y, w, h = width - size, pad, size, height - 2 * pad
    elif m.LLS:
      x, y, w, h = 0, pad, size, height - 2 * pad
    else:
      return
    color = QColor('indianred')
    qp.setBrush(QBrush(color, Qt.SolidPattern))
    qp.setPen(QPen(color))
    path = QPainterPath()
    path.setFillRule(Qt.WindingFill)
    path.addRoundedRect(x, y, w, h, rounding, rounding)
    qp.drawPath(path)



if __name__ == "__main__":
  logging.basicConfig(level=logging.DEBUG, format='%(levelname)s:%(module)s:%(lineno)d:%(funcName)s:%(message)s ')
  _log.debug('start')
  def cb_rbv(*args,**kwargs):
    _log.debug(f'RBV{args} {kwargs}')



  import sys
  from PyQt5.QtWidgets import QApplication, QWidget, QLabel
  app=QApplication(sys.argv)
  mot_widget=MotorTweak()

  #mot_widget.event_val.connect(cb_rbv)
  mot_widget.event_rbv.connect(cb_rbv)

  pfx='SAR-EXPMX'
  mot_widget.connect_motor(f"{pfx}:MOT_FY", alias="fast_y", label="fast Y")
  mot_widget.setGeometry(50, 50, 520, 100)
  mot_widget.setWindowTitle("PyQt5 Example")
  mot_widget.show()
  sys.exit(app.exec_())