#%TITLE% KLEINDIEK.MAC 
#
#%NAME%
# Macros motor to control the KLEINDIEK 3 axes nano positioning stage 
# (two rotations and one translation). The controller is accessed
# through a serial line 
#
#%CATEGORY% Positioning
#
#%DESCRIPTION%
#
#%DL%
#%DT%Configuring the serial line%DD%
#
#  %DL%
#    %DT%DEVICE
#     What ever you have, for instance ID13/serial/com2
#    %DT%TYPE
#     Related to previous field, for instance ESRF
#    %DT%BAUD
#     19200
#    %DT%MODE
#     raw
#  %XDL%
#
#
#%DT%Configuring controller %DD%
#  %DL%
#    %DT%DEVICE
#    kd
#    %DT%ADDR
#    the serial entry in config list, for instance 0
#    %DT%NUM
#    3
#    %DT%TYPE
#    Macro Motors
#  %XDL%
#
#
#%DT%Configuring three macro motors %DD%
#  %DL%
#    %DT%CONTROLLER
#    MAC_MOT
#    %DT%UNIT/MODULE/CHANNEL
#    the controller entry in config list and the channel, for instance 0/0
#    to access the first axis (rotation, labelled "1" on the controller)
#  %XDL%
#%XDL%
#%END%
#

#%IU%
#%MDESC%
# MACRO MOTOR: 
# Called by spec after reading the config file
#
def kd_config(num,type,p1,p2,p3) '{
 global KD[]
 global KD_SL
 local  dev

 #
 if(type=="ctrl") {
  unglobal KD

  dev=kd_ADDR
  if(dev=="") {
   printf("KLEINDIEK ERROR: missing ADDR in config\n")
   return
  }
 }


 # p1==unit p2==module p3==channel
 if(type=="mot") {
  local mne
  local ch

  if((p3<0) || (p3>2)) {
   printf("KLEINDIEK ERROR: invalid channel %d\n",p3)
   return
  }

  mne=motor_mne(num)
  KD[mne]["sl"]=motor_par(num,"address")

  # not used for macro motors but only for debug
  KD_SL=motor_par(num,"address")

  
  ch=(p3==0?"A":(p3==1?"B":"C"))
  KD[mne]["ch"]=ch

 }

}'


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

 if(num == "..") { return }
 mne=motor_mne(num)

 #
 # return the current motor position in mm or deg
 #
 if (key == "position") {
  local c pos

  cmd=sprintf("COARSE %s ?\r",KD[mne]["ch"])
  ans=kd_com(mne,cmd)
  c=""
  pos=0.0
  if(sscanf(ans,"%c\t%f",c,pos) != 2) {
   printf("KLEINDIEK ERROR: unable to get pos")
   return(99999999999999)
  }
  return(pos)
 }



 #
 # start a motion (p1==abs pos, p2==rel pos, with pos in mm or deg)
 #
 if (key == "start_one") {
  local cmd
  local dev
  
  
  #MP Sep 11 2006: controller works with integers only, force the cast
  cmd=sprintf("COARSE %s %d\r",KD[mne]["ch"], int(p2))
  kd_com(mne, cmd)
 }



 #
 # return the current motor status
 #
 if (key == "get_status") {
 }


 #
 # stop a single motor
 #
 if (key == "abort_one") {
  #MP Sep 26 2006: coarse commands can be aborted during execution
  #by sending any new command, including a single <CR>
  kd_com(mne, "coarse ?\r")
 }


 #
 # set position (p1=abs_steps)
 #
 if (key == "set_position") {
 }


}'

#%IU%(motor_mne,command)
#%MDESC%
# Send the specified command to the controller and return its answer
# or empty string.
#
def kd_com(mne,cmd) '{
 local dev

 dev=KD[mne]["sl"]
 ser_par(dev,"timeout",2)
 ser_par(dev,"flush")
 ser_put(dev,cmd)
 ans=ser_get(dev,"\r")
 if(substr(ans,1,1) != "o") {
   printf("KLEINDIEK ERROR: executing \"%s\"\n",substr(cmd,1,length(cmd)-1))
   return ""
 }
 return ans

}'






#%IU%
#%MDESC%
# Interactive console, <Ctrl-C> to quit
#
def kd '{
 global KD_SL
 local line c
 local ncomm

 local cmd
 local ret ans
 local eoa

 ncomm = 1
 ser_par(KD_SL, "flush")
 ser_par(KD_SL, "timeout", 0.1)
 while(1) {
  line = ""
  printf("%d.KLEINDIEK console> ",ncomm);
  while(1) {
    c = input(-1)
    if (c == "\n") {
      printf("\n")
      tty_cntl("cd")
      break
    } else if ((c == "\b" || c == "\177") && line) {
      line = substr(line, 0, length(line)-1)
      printf("\b \b")
    } else if (asc(c) == 27) {
      while(input(-1));
    } else if (c >= " " && c <= "z") {
      line = line c
      printf(c)
    }
  }


  if(line) {
   line = line "\r"
   ser_put(KD_SL, line)
   ncomm++
   sleep(0.1)
  }

  eoa=0
  while(!eoa) {
   ans=ser_get(KD_SL,"\r")
   if(ans != "") {  
    printf("%s\n",ans)
   } else {
    eoa = 1
   }
  }

 printf("\n")

 }
}'