#%TITLE% attocube.mac
#%NAME%
#   Macros to work with attocube
#%CATEGORY% Positioning
#
#%DESCRIPTION%
#
# attosetup unit serline motor axis [motor axis] ...
#%END%

need srssim.mac 

def attosetup '{
  global ATTO[]
  local unit nmot mne axis par[] serline npar srsunit srsport srspars
  unit = ($# > 0) ? $1 : getval("Attocube unit", 0)
  serline = ($# > 1) ? "$2" : getval("Attocube serial line", \
				     ATTO[unit]["serline"])
  ATTO_SWITCH[unit] = ""

  ATTO[unit]["serline"] = serline
  if (substr(serline, 1, 6) == "SRSSIM") {
    npar = split(serline, par, ":")
    srsunit = (npar > 1) ? par[1] : 0 
    srsport = (npar > 2) ? par[2] : 10
    srspars = "baud=38400 idn=ver forcewait nostrip echo"
    if (srssim_register_module("atto", srsunit, srsport, srspars) == -1) {
      printf("Error registering atto module in SRS-SIM unit %d port %d\n", \
             srsunit, srsport)
      exit
    }
  }
  else ser_par(serline,"flush")

  ATTO[unit]["init"] = 1
  if (unit >= ATTO["nrunit"])
    ATTO["nrunit"] = unit + 1

  if (($# > 2) && ($# % 2 != 0)) {
    print "Invalid parameters: $*"
    print "Usage: attosetup unit serline mot1 axis1 mot2 axis2 ..."
    exit
  }

  split("$*", par)
  ATTO[unit]["nrmot"] = nmot = 0
  while (1) {
    if ($# > 2) {
      if (nmot == ($# - 2) / 2)
        break
      mne = par[2 + 2 * nmot]
      axis = par[2 + 2 * nmot + 1] + 0
    } else {
      mne = getval(sprintf("Motor mne %d [Enter to end]", nmot + 1), "")
      if (mne == "")
        break
      axis = getval(sprintf("Axis number %d", nmot + 1), ATTO[mne]["axis"])
    }
    if ((motor_num(mne) == -1) || (motor_mne(motor_num(mne)) != mne)) {
      print "Invalid motor:", mne
      exit
    } else if ((axis < 1) || (axis > 6)) {
      print "Invalid axis:", axis
      exit
    }
    ATTO[mne]["mnum"] = motor_num(mne)
    ATTO[mne]["axis"] = axis
    ATTO[mne]["unit"] = unit
    ATTO[unit][nmot] = mne
    ATTO[unit]["nrmot"] = ++nmot

    if (atto_mne_cmd(mne, "getm %d") == -1)
      exit
  }

  attoon

  setup_tail("atto")
}'

def attounsetup '{
  attooff
  unglobal ATTO
}'

def attoon '{
  local unit i mne serline
  
  for (unit = 0; unit < ATTO["nrunit"]; unit++) {
    if (ATTO[unit]["init"] != 1)
      continue

    #first flush the serial line
    serline = ATTO[unit]["serline"]
    if (substr(serline, 1, 7) != "SRSSIM:") {
      ser_par(serline, "flush")
    }
    for (i = 0; i < ATTO[unit]["nrmot"]; i++) {
      mne = ATTO[unit][i]
      if (atto_mne_cmd(mne, "setm %d stp") == -1)
        exit

      ATTO[mne]["on"] = 1
      cdef("user_getangles", sprintf("atto_getangles %s; ", mne), mne, 0x01)
      cdef("user_checkall",  sprintf("atto_checkall  %s; ", mne), mne, 0x01)
      cdef("user_motorsrun", sprintf("atto_motorsrun %s; ", mne), mne, 0x01)
      cdef("user_set",       sprintf("atto_setpos    %s; ", mne), mne, 0x01)
      cdef("cleanup_always", sprintf("atto_cleanup   %s; ", mne), mne, 0x01)
    }
  }
}'

def attooff '{
  local unit i mne

  for (unit = 0; unit < ATTO["nrunit"]; unit++) {
    if (ATTO[unit]["init"] != 1)
      continue

    for (i = 0; i < ATTO[unit]["nrmot"]; i++) {
      mne = ATTO[unit][i]
      if (atto_mne_cmd(mne, "setm %d gnd") == -1)
        print "Warning, could not set to GND axis", mne

      ATTO[mne]["on"] = 0
      cdef("user_getangles", "", mne, "delete")
      cdef("user_checkall",  "", mne, "delete")
      cdef("user_motorsrun", "", mne, "delete")
      cdef("user_set",       "", mne, "delete")
      cdef("cleanup_always", "", mne, "delete")
    }
  }
}'


def atto_low_comm(unit, cmd, eos) '{
  local serline ret srsunit str

  serline = ATTO[unit]["serline"]
  if (substr(serline, 1, 7) != "SRSSIM:") {
    if (ser_put(serline, sprintf("%s\r", cmd)) == -1)
      return -1
    sleep(0.100)
    return ser_get(serline, eos)
  }

  srsunit = substr(serline, 7)
  ret = srssim_module_cmd("atto", cmd, 1, 1)
  if (ret == -1)
    printf("Error sending command to ATTO unit %d at SRS-SIM unit %d\n", \
           unit, srsunit)

  str = ret
  while (index(str, eos) == 0) {
    ret = srssim_module_cmd("atto", "", 1, 1)
    if (ret == -1) {
      printf("Error reading response from ATTO unit %d at SRS-SIM unit %d", \
             unit, srsunit)
      return -1
    }
    str = sprintf("%s%s", str, ret)
  }
  return str
}'


def atto_low_get(unit, eos) '{
  local serline str ret srsunit

  serline = ATTO[unit]["serline"]
  if (substr(serline, 1, 7) != "SRSSIM:")
    return ser_get(serline, eos)

  srsunit = substr(serline, 7)
  str = ""
  while (index(str, eos) == 0) {
    ret = srssim_module_cmd("atto", "", 1, 1)
    if (ret == -1) {
      printf("Error reading response from ATTO unit %d at SRS-SIM unit %d", \
             unit, srsunit)
      return -1
    }
    str = sprintf("%s%s", str, ret)
  }
  return str
}'

def atto_cmd(unit, cmd) '{
  local lines[] serline nlines resp

  if (ATTO[unit]["init"] != 1) {
    print "Error: ATTO unit", unit, "not configured"
    return -1
  }
  serline = ATTO[unit]["serline"]
  resp = atto_low_comm(unit, cmd, "> ")
  nlines = split(resp, lines, "\r\n")
  if (lines[0] != cmd) {
    print "Error in ATTO command: did not get echo"
    return -1
  } else if (lines[2] == "ERROR") {
    print "Error in ATTO command:", lines[1]
    return -1
  } else if (lines[nlines - 2] != "OK") {
    print "Error in ATTO command: did not get OK"
    return -1
  } else if (lines[nlines - 1] != "> ") {
    print "Error in ATTO command: did not get the prompt"
    return -1
  }
  return (substr(cmd, 1, 3) == "get") ? lines[1] : 0
}'

def atto_mne_cmd(mne, cmd, val) '{
  global ATTO_SWITCH[]
  local axis ret
  axis = ATTO[mne]["axis"]
  if (axis < 4 ) {
    if (ATTO_SWITCH[unit] != "off") {
      ret = atto_cmd(ATTO[mne]["unit"],"setm 1 gnd")
      ret = atto_cmd(ATTO[mne]["unit"],"setm 2 gnd")
      ret = atto_cmd(ATTO[mne]["unit"],"setm 3 gnd")
      ret = atto_cmd(ATTO[mne]["unit"],"rbctl off")
      ret = atto_cmd(ATTO[mne]["unit"],"setm 1 stp")
      ret = atto_cmd(ATTO[mne]["unit"],"setm 2 stp")
      ret = atto_cmd(ATTO[mne]["unit"],"setm 3 stp")
      ATTO_SWITCH[unit] = "off"
    }
  } else  if(axis >3){
       if (ATTO_SWITCH[unit]!="on"){
        ret = atto_cmd(ATTO[mne]["unit"],"setm 1 gnd")
        ret = atto_cmd(ATTO[mne]["unit"],"setm 2 gnd")
        ret = atto_cmd(ATTO[mne]["unit"],"setm 3 gnd")
        ret = atto_cmd(ATTO[mne]["unit"],"rbctl on")
        ret = atto_cmd(ATTO[mne]["unit"],"setm 1 stp")
        ret = atto_cmd(ATTO[mne]["unit"],"setm 2 stp")
        ret = atto_cmd(ATTO[mne]["unit"],"setm 3 stp")
        ATTO_SWITCH[unit] = "on"
     }
    axis -=3
  }
  return atto_cmd(ATTO[mne]["unit"], sprintf(cmd, axis, val))
}'

def atto_move(mne, steps, freq) '{
  local cmd 

  if ((freq != 0) && (freq != ATTO[mne]["freq"])) {
    if (atto_mne_cmd(mne, "setf %d %d", freq) == -1) {
      print "Error setting frequency on axis", axis, "to", freq
      return -1
    }
    ATTO[mne]["freq"] = freq
  }

  cmd = (steps > 0) ? "stepu" : "stepd"
  if (atto_mne_cmd(mne, sprintf("%s %%d %d", cmd, fabs(steps))) == -1) {
    cmd = (steps > 0) ? "up" : "down"
    printf("Error sending %d steps %s on axis %d\n", steps, cmd, axis)
    return -1
  }

  return 0
}'


def atto_stop(mne) '{
  if (atto_mne_cmd(mne, "stop %d") == -1) {
    print "Error stopping axis", ATTO[mne]["axis"]
    return -1
  }

  return 0
}'


def atto_getangles '{
  local mne mnum tend tnow t0 speed

  mne = "$1"
  if (ATTO[mne]["on"]) {
    mnum = ATTO[mne]["mnum"]
    if (ATTO[mne]["moving"]) {
      tnow = time()
      t0 = ATTO[mne]["t0"]
      tend = ATTO[mne]["tend"]
      speed = ATTO[mne]["freq"] / motor_par(mnum, "step_size") * \
	      ATTO[mne]["dir"]
      if (tnow < tend)
        tend = ATTO[mne]["t0"] = tnow
      else
        ATTO[mne]["moving"] = 0
        
      ATTO[mne]["pos"] += speed * (tend - t0)
    }

    A[$1] = ATTO[mne]["pos"]
  }
}'


def atto_checkall '{
  local mne mnum steps freq

  mne = "$1"
  mnum = ATTO[mne]["mnum"]
  if (ATTO[mne]["on"] && (A[mnum] != ATTO[mne]["pos"])) {
    steps = (A[mnum] - ATTO[mne]["pos"]) * motor_par(mnum, "step_size")
    freq = motor_par(mnum, "slew_rate")
    if (atto_move(mne, steps, freq) < 0)
      exit
    ATTO[mne]["tend"] = ATTO[mne]["t0"] = time()
    ATTO[mne]["tend"] += fabs(steps) / freq
    ATTO[mne]["dir"] = (steps > 0) ? 1 : -1
    ATTO[mne]["moving"] = 1
  }
}'


def atto_motorsrun '{
  if (ATTO["$1"]["moving"] && (time() < ATTO["$1"]["tend"]))
    return 1
}'


def atto_cleanup '{
  if (ATTO["$1"]["moving"]) {
    atto_stop("$1")
    ATTO["$1"]["tend"] = time()
    atto_getangles $1
  }
}'

def atto_setpos '{
  if (motor2set == $1)
    ATTO["$1"]["pos"] = position2set
}'

def attointeractive '{
  srssiminteractive atto
}'

def attovolt '{
  if ($# < 1) {
    print "Usage: attovolt motmne [volt]"
    exit
  }

  if ($# == 1) 
    print atto_mne_cmd("$1", "getv %d")
  else 
    print atto_mne_cmd("$1", "setv %d %d", $2)
}'