#%TITLE% UDIFF_MM.MAC
#%NAME% %B%udiff_mm.mac%B% - Microdiff TANGO attribute macro motors
#%DESCRIPTION%
# The macro set offers the possibility to set-up the microdiff TANGO attributes
# as macro motors. Only read/write TANGO attributes can be used as a macro
# motor.%BR%
# The macro motor controller defined in spec config is called %B%udiff_mot%B%
# and has as the ADDR the device server device (e.g. id29/microdiff/1)
# For each motor the %B%root_attr%B% additional parameter shoud be
# configured (e.g.MOTPAR:root_attr = Phi). If the end of the attribute is not
# Position, another additional parameter %B%add_attr%B% shoud beconfigured.
# Thus the name of the attribute sent to the device server is the combination
# of root_attr and add_attr. To move the minikappa motors simultaneously
# when calibrating the minicapppa with STAC, the %B%move_sync%B% parameter
# specifies the motor name as in the minikappasetup.
#%END%

#%IU% (motn, type, unit, module, chan)
#%MDESC% Called after reading the config file. If return the string ".error.",
#spec will consider the channel unusable. We only use motn and type parameters.
def udiff_mot_config(motn, type, unit, module, chan) '{
global UDIFF_PARAMS

  if (UDIFF_PARAMS["debug"] != 0) {
    tty_cntl("md");printf ("udiff_config %s\n", type);tty_cntl("me")
    tty_cntl("md");printf ("udiff_config mot %s\n", motn);tty_cntl("me")
  }

  if (type == "ctrl")
    return

  if (type == "mot") {
    if (motor_par(motn,"device_id") != "udiff_mot")
      return ".error."

    if (udiff_getprops(motn) == -1)
     return ".error."
  }
  if (type == "cnt") {
    if (counter_par(motn,"device_id") != "udiff_mot")
      return ".error."

    if (udiff_getpropsc(motn) == -1)
      return ".error."
  }
}'

#%IU% (motn, key, p1, p2)
#%MDESC% Execute the command %B%key%B% for the motor %B%motn%B%, with
#parameters %B%p1%B% and %B%p2%B% if needed. The keys used are as follows:%BR%
#%B%"position"%B% - must return the current motor position in dial units.
#Parameters:none.%BR%
#%B%"get_status%B%" - get the move and limit status of individual motors. Must
#return bit mask - bits set to 0x02 - moving, 0x04 - low limit active,
#0x08 - high limit is active, 0x10 - "emergency stop" and 0x20 - motor fault,
#0 otherwise. Use the MotorStates tango attribute decoded for each motor.
#Parameters:none.%BR%
#%B%"prestart_one%B%" - sent for each motor that needs to be moved - we need
#to test the microdiff state. If different that 7 (STANDBY), the move is
#canceled.
#Parameters:none.%BR%
#%B%"start_one"%B% and %B%"set_position"%B% -start a move for one motor.
#Parameters: p1 - target position in dial units.%BR%
#%B%"abort_one"%B% - sent when motors are halted, either by a ^C from the
#keyboard or by a stop() command - only general reset of the microdiff
#available.
#Parameters:none.%BR%
#Return the string ".error.", spec will consider the channel unusable. 
def udiff_mot_cmd(motn, key, p1, p2) '{
local dev attr ret _pos add_attr motstat[]
global ESRF_ERR

  if (UDIFF_PARAMS["debug"] != 0) {
    tty_cntl("md");printf ("udiff_mot_cmd %s\n", key);tty_cntl("me")
  }
  if (UDIFF_PARAMS["debug"])
    p "-------------> key: ", key

  dev = UDIFF_PARAMS[motn]["tango_dev"]
  if (key == "position") {
    if (UDIFF_PARAMS[motn]["add_attr"]) {
      attr = sprintf("%s%s", \
	UDIFF_PARAMS[motn]["root_attr"], UDIFF_PARAMS[motn]["add_attr"])
    } else
      attr = sprintf("%sPosition", UDIFF_PARAMS[motn]["root_attr"])
    ret = tango_get(dev, attr)

    if (TANGO_ERR != "0") {
      print_tango_err()
      motor_par(motn,"disable",1)
      return(0x20)
    }
    if (UDIFF_PARAMS["debug"]) {
      print "-------------> motor", motor_mne(motn)
      print "-------------> attr: ", attr
      print "-------------> returning: ", ret
    }
    return (ret)
  } else if (key == "get_status") {
    if (UDIFF_PARAMS[motn]["add_attr"])
      return(0)
    attr = "MotorStates"

    if (UDIFF_PARAMS[motn]["mdtype"])
      tango_get(dev, attr, motstat)
    else
      ret =  tango_get(dev, attr)

    if (TANGO_ERR != "0") {
      tty_cntl("md")
      printf ("motor %s:",motor_mne(motn))
      print_tango_err()
      tty_cntl("me")
      return(0x20)
    }
    if (UDIFF_PARAMS[motn]["mdtype"]) {
      _newstat = get_state(motstat, motn)
    } else {
     _newstat = get_state(ret, motn)
    }

    if (UDIFF_PARAMS["debug"])
      p "-------------> stat: " _newstat

    if ((_newstat == 4) || (_newstat == "Moving"))
      return (0x2)

    if ((_newstat == 5) || (_newstat == "LowLim"))
      return(0x4)

    if (_newstat == "HighLim")
      return(0x8)

    if ((_newstat == 0) || (_newstat == 1) || \
	(_newstat == "Initializing") || (_newstat == "Fault"))
      return(0x20)
    return(0)
  } else if (key == "prestart_one") {
    wait_ready()
    ret = tango_get(dev, "State")
    
    if (TANGO_ERR != "0") {
      printf ("motor %s cannot move:", motor_mne(motn))
      print_tango_err()
      return ".error."
    }
    if (ret != 7) {
      ret = tango_get(dev, "Status")
      tty_cntl("md")
      printf ("motor %s cannot move - microdiff state: %s\n", \
	motor_mne(motn), ret)
      tty_cntl("me")
      return ".error."
    }
  } else if ((key == "start_one") || (key == "set_position")) {
    if (p2 == 0) {
      #do nothing if counter
      return
    }
    if (!UDIFF_PARAMS[motn]["root_attr"])
      return(0)
    if (UDIFF_PARAMS[motn]["add_attr"]) {
      attr = sprintf("%s%s", \
	UDIFF_PARAMS[motn]["root_attr"], UDIFF_PARAMS[motn]["add_attr"])
    } else
      attr = sprintf("%sPosition", UDIFF_PARAMS[motn]["root_attr"])

    if ((UDIFF_PARAMS[motn]["move_sync"]) && (KAPPA_MOT_DOTC == 1)) {
      moveMiniKappaCentered_mm(motor_mne(motn),p1)
      if (UDIFF_PARAMS["debug"]) {
        p "-------------> minikappa sync move"
        p "-------------> motor: " motor_mne(motn)
        p "-------------> position:    ", p1
      }
    } else {
      #esrf_io(sprintf("%s/%sPosition",dev,attr), "DevWrite", p1)
      ret = tango_put(dev,attr,p1)
      if (UDIFF_PARAMS["debug"]) {
        p "-------------> command: ", attr
        p "-------------> send:    ", p1
        p "-------------> got:     ", ret
      }
      if (TANGO_ERR != "0") {
        tty_cntl("md")
        printf ("motor %s:",motor_mne(motn))
        print_tango_err()
        tty_cntl("me")
        return ".error."
      }
    }
  } else if (key == "abort_one") {
    printf ("-------------------stopping\n")
    if (UDIFF_PARAMS[motn]["mdtype"])
      tango_io (dev, "abort")
    else
      tango_io (dev, "EmergencyStop")
    if (TANGO_ERR != "0") {
      tty_cntl("md")
      printf ("motor %s:", motor_mne(motn))
      print_tango_err()
      tty_cntl("me")
      return ".error."
    }
  } else if (key == "counts") {
    #macro counter
    dev = counter_par(motn,"address")
    if (UDIFF_PARAMS[motn]["mdtype"])
      ret = tango_io(dev, "readPhotodiodeSignal",1)
    else
      ret = tango_io (dev, "ReadPhotodiodeSignal",1)
    if (TANGO_ERR != "0") {
      tty_cntl("md")
      printf ("counter %s:",cnt_mne(motn))
      print_tango_err()
      tty_cntl("me")
      return ".error."
    }
    return ret
  }
}'

#%IU% (motn)
#%MDESC% Get the motor properties "tango_dev", "root_attr", "add_attr".
def udiff_getprops(motn) '{
local dev mdtype
global UDIFF_PARAMS


  if (UDIFF_PARAMS["debug"] != 0) {
    tty_cntl("md");printf ("udiff_getprops %d\n", motn);tty_cntl("me")
  }
  dev = motor_par(motn,"address")
  #dev = udiff_mot_ADDR
  tango_io(dev,"timeout",10)
  if (TANGO_ERR != "0") {
    print_tango_err()
    motor_par(motn,"disable",1)
    return(-1)
  }

  if (split(dev, mdtype,":") > 1)
    UDIFF_PARAMS[motn]["mdtype"] = 1
  UDIFF_PARAMS[motn]["tango_dev"] = dev
  UDIFF_PARAMS[motn]["root_attr"] = motor_par(motn,"root_attr")
  UDIFF_PARAMS[motn]["add_attr"] = motor_par(motn,"add_attr")
  UDIFF_PARAMS[motn]["move_sync"] = motor_par(motn,"move_sync")
  motor_par(motn,"disable",0)
  return(0)

}'

#%IU% (mot)
#%MDESC% Get the counter properties
def udiff_getpropsc(ctn) '{
local dev
global UDIFF_PARAMS

  if (UDIFF_PARAMS["debug"] != 0) {
    tty_cntl("md");printf ("udiff_getpropc %d\n", ctn);tty_cntl("me")
  }
  dev = counter_par(ctn,"address")
  tango_io(dev,"timeout",10)
  if (TANGO_ERR != "0") {
    print_tango_err()
    counter_par(ctn,"disable",1)
    return(-1)
  }
  counter_par(ctn,"disable",0)
  return(0)
}'

#%IU% (_stat, motn)
#%MDESC% Return the state of the motor %B%motn%B% extracted from the
#string(MD2) or array(MD3) %B%_stat%B%
def get_state(_stat, motn) '{
global UDIFF_PARAMS
local attr nb abstat ts i

  attr = UDIFF_PARAMS[motn]["root_attr"]

  if (UDIFF_PARAMS[motn]["mdtype"]) {
    for(i in _stat) {
      abstat = _stat[i]
      split(abstat,ts,"=")
      if (ts[0] == attr)
        return(ts[1])
    }
  } else {
    nb = split(_stat, abstat)
    for (i = 0; i<nb; i++) {
      split(abstat[i],ts,"=")
      if (ts[0] == attr)
      return(ts[1])
    }
  }

  return(-1)
}'

#%IMACROS%
#%TOC%
#%DEPENDENCIES% microdiff.mac%BR%
#%AUTHOR% A.Beteva%BR%
#$Revision: 2.1 $ $Date: 2016/12/02 10:22:40 $
#%END%
#%LOG%
#$Log: udiff_mm.mac,v $
#Revision 2.1  2016/12/02 10:22:40  guilloud
#changed TANGO_ERR test.
#
#Revision 2.0  2012/11/22 14:17:01  beteva
#changed commands to cope with the new MD2 interface.
#Old interface still usable.
#
#Revision 1.7  2011/10/26 14:59:17  beteva
#added minikappa synchronous move handling
#
#Revision 1.6  2011/04/13 16:06:15  beteva
#do nothing if counter when start_one executed
#
#Revision 1.5  2010/04/09 14:33:36  beteva
#added diode macro counter
#
#Revision 1.4  2009/04/08 12:00:35  beteva
#added debugging features
#
#Revision 1.3  2009/02/09 14:03:42  beteva
#changed Reset to EmergencyStop for aborting the movement.
#
#Revision 1.2  2009/01/28 14:24:38  beteva
#added handling for attributes, different from Position
#
#Revision 1.1  2008/12/18 13:58:17  beteva
#Initial revision
#