#%TITLE% ICE_VREG.MAC
#
# TODO:
#
#%DT%Extra motor_par() parameters %DD%
# %DL%
#  %DT% motor_par(motor,"closed_loop")%DD%
#       Returns non null if regulation is on.
#  %DT% motor_par(motor,"closed_loop", 1)%DD%
#  %DT% motor_par(motor,"closed_loop", 0)%DD%
#       Switch on/off the regulation
# %XDL%
#
#%XDL%
#
#%END%
#

need ice


#%IU%
#%MDESC%
# MACRO MOTOR: 
# Called by spec after reading the config file
#
def icepapvreg_config(num,type,p1,p2,p3) '{

 # p1==controller index p2==number of motors supported
 if(type=="ctrl") {
 }

 # p1==unit p2==module p3==channel
 if(type=="mot") {
  local mne
  local dev
  local adr
  local ans
  local src
  local adr2
  local m, i

  mne  = motor_mne(num)


  # check that the regulation source has been defined
  src = icepap__toupper(motor_par(num, "source"))
  if(src == "ABSENC") {
   printf("\"%s\": using for regulation source: \"%s\"\n", mne, src)
  } else if(src == "ENCIN") {
   _icepap_err
   printf("not supported \"source\" parameter for motor: \"%s\"\n",mne)
   return ".error."
  } else if(src == "INPOS") {
   _icepap_err
   printf("not supported \"source\" parameter for motor: \"%s\"\n",mne)
   return ".error."
  } else if(src == "SOFT") {
   _icepap_err
   printf("not supported \"source\" parameter for motor: \"%s\"\n",mne)
   return ".error."
  } else {
   _icepap_err
   printf("invalid or missing \"source\" parameter for motor: \"%s\"\n",mne)
   return ".error."
  }

  # check that the gain has been defined
  if(motor_par(num, "gain") == 0) {
   _icepap_err
   printf("invalid or missing \"gain\" parameter for motor: \"%s\"\n",mne)
   return ".error."
  }

  # at this point we should have a valid configuration
  dev  = motor_par(num, "address")
  adr  = motor_par(num, "module")*10
  adr += motor_par(num, "channel")
  
  # configure the regulation source
  ans=_icepap_query(dev, "#" adr, sprintf("VCONFIG SOURCE %s", src), silent)
  if(index(ans, "ERROR")) {
   _icepap_err
   printf("invalid or missing \"source\" parameter for motor: \"%s\"\n",mne)
   return ".error."
  }

  # look for any real motor control (same axis but as icepap macro motor)
  for(i=0, m=-1; i<MOTORS;i++) {
   if(motor_par(i, "device_id") != "icepap") { continue }
   if(motor_par(i, "address")   != dev)      { continue }
   adr2  = motor_par(i, "module")*10
   adr2 += motor_par(i, "channel")
   if(adr != adr2)                           { continue }

   # we got it
   printf("\"%s\": found real motor: \"%s\"\n", mne, motor_mne(i))
   m = i
   break;
  }
  motor_par(num, "realmot", m, "add")

 }

}'


#%IU%
#%MDESC%
# MACRO MOTOR: 
# Called by spec after reading the config file, after calling _config()
# and only if parameters are set in the config file for a motor.
#
def icepapvreg_par(num,key,todo,p1) '{
 local mne
 local dev
 local adr
 local ans
 local silent

 silent = 1 


 #
 #
 #

 # return new motor_par() argins handled 
 if (key == "?" && todo == "get") {
  return("ICEPAP VREG SPECIFIC: closed_loop, vconfig")
 }


 #
 #
 #

 # get the motor mnemonic string
 mne  = motor_mne(num)

 # get the address
 dev  = motor_par(num, "address")
 adr  = motor_par(num, "module")*10
 adr += motor_par(num, "channel")



 #
 #
 #
 if (key == "vconfig") {
   return(_icepap_query(dev, adr, "?VCONFIG"))
 }


 #
 #
 #
 if (key == "closed_loop") {
  local cmd

  # switch on/off regulation mode
  if(todo == "set") {
   local cmd
   local tau
   local m

   # switch off regulation mode
   if(p1 == 0) {

    # the unique way it to stop the "motion"
    ans=_icepap_query(dev, "#" adr, "STOP")
    if(index(ans,"ERROR")) {
     _icepap_err
     printf("unable to stop regulation on motor: \"%s\"\n",mne)
    }
 
    # enable real motor if any
    if((m = motor_par(num, "realmot")) != -1) {
     _icepap_warn
     printf("re-enabling motor: \"%s\"\n", motor_mne(m))
     motor_par(m, "disable", 0)

     # restore velocity of the axis will be the maximum velocity
     icepap_cmd(m, "slew_rate", motor_par(m, "config_velocity"))

     # update silently positions
     read_motors(0x06)
    }
   }
  

   # switch on regulation mode
   if(p1 == 1) {
    local pos

    # configure gain at very last moment, just in case it has been changed
    # dynamically through motor_par() calls
    cmd = sprintf("VCONFIG GAIN %f", motor_par(num, "gain"))
    ans = _icepap_query(dev, "#" adr, cmd, silent)
    if(index(ans,"ERROR")) {
     _icepap_err
     printf("unable to set regulation gain for motor: \"%s\"\n",mne)
     return(0)
    }

    # non mandatory tau parameter 
    tau = motor_par(num, "tau")
    # default seems to be 0.1 in IcePAP DRIVER but neutral value should be 1
    if(tau == 0) { tau = 1 }
    cmd = sprintf("VCONFIG TAU %f", tau)
    ans = _icepap_query(dev, "#" adr, cmd, silent)
    if(index(ans,"ERROR")) {
     _icepap_err
     printf("unable to set regulation tau for motor: \"%s\"\n",mne)
     return(0)
    }

    # the velocity of the axis will be the maximum velocity
    # while regulating
    cmd = sprintf("VELOCITY %d", motor_par(num, "velocity"))
    ans = _icepap_query(dev, "#" adr, cmd, silent)
    if(index(ans,"ERROR")) {
     _icepap_err
     printf("unable to set max velocity for regulation motor: \"%s\"\n",mne)
     return(0)
    }

    # doing a first motion with switch on the regulation
    pos = icepapvreg_cmd(num, "position")
    _icepapvreg_vmove(num, pos)

    # enable real motor if any
    if((m = motor_par(num, "realmot")) != -1) {
     _icepap_warn
     printf("disabling motor: \"%s\"\n", motor_mne(m))
     motor_par(m, "disable", 1)
    }
   }
  }


  # Return the current state of closed loop
  ans=_icepap_query(dev, adr, "?VMOVE", silent)
  return(index(ans,"ERROR")?0:1)
 }
}'


#%IU%
#%MDESC%
# MACRO MOTOR: 
# Called by spec on motor operation.
# 
def icepapvreg_cmd(num,key,p1,p2) '{
 local mne
 local dev
 local adr
 local ans
 local cmd
 local silent


 silent = 1 


 #
 # Handle actions at controller level
 #
 if(num == "..") { 
  return
 }


 #
 # Handle actions at motor level
 #

 # get the motor mnemonic string
 mne  = motor_mne(num)

 # get the address
 dev  = motor_par(num, "address")
 adr  = motor_par(num, "module")*10
 adr += motor_par(num, "channel")


 #
 # return the current motor position in mm or deg
 #
 if (key == "position") {
  local pos
  
  # get setpoint for regulation
  ans = _icepap_query(dev, adr, "?VMOVE", silent)
  if(!index(ans,"ERROR")) {
   if(sscanf(ans, "%f", pos) != 1) {
    _icepap_err
    printf("unable to get position for: \"%s\"\n",mne)
    return ".error."
   }
   return _icepapvreg_stps2pos(num, pos)
  }

  # if not in regulation mode, emulate the position
  # TODO: the source should be taken from ?VCONFIG SOURCE
  ans = _icepap_query(dev, adr, "?ENC ABSENC")
  if(sscanf(ans, "%f", pos) != 1) {
   _icepap_err
   printf("unable to get emulated position for: \"%s\"\n",mne)
   return ".error."
  }
  return _icepapvreg_stps2pos(num, pos)
 }




 #
 # start a motion (p1==abs pos, p2==rel pos, with pos in mm or deg)
 #
 if (key == "start_one") {

  if(motor_par(num, "closed_loop") == 0) {
   _icepap_err
   printf("unauthorized motion on: \"%s\", switch closed loop on first\n",mne)
   return ".error."
  }

  return(_icepapvreg_vmove(num, p1))
 }


}'


#%IU%(motor, user_abs_pos)
#
#
#
def _icepapvreg_vmove(num, p1) '{
 local dev
 local adr
 local pos
 local cmd
 local ans


 dev  = motor_par(num, "address")
 adr  = motor_par(num, "module")*10
 adr += motor_par(num, "channel")
 pos  = icepap_round(p1*motor_par(num,"step_size"))
 cmd  = sprintf("VMOVE %d", pos)
 ans  = _icepap_query(dev, "#" adr, cmd)
 if(index(ans,"ERROR")) {
  local mne

  _icepap_err
  mne = motor_mne(num)
  printf("unable to move motor: \"%s\"\n",mne)
  return ".error."
 }
}'


#%IU%(motor, steps)
#%MDESC%
# Returns the given position converted in user units.
# The float value is returned as a string to be compatible with macro motors.
#
def _icepapvreg_stps2pos(num, pos) '{
  local pos_str

  pos_str = sprintf("%.15g", pos / motor_par(num,"step_size"))
  return(pos_str)
}'




#%MACROS%
#%IMACROS%
#%AUTHOR% MP BLISS (Original 2014/10).
# %BR%$Revision: $ / $Date:$
#%TOC%