#%TITLE% QG2.MAC
#%NAME%
#  Macros to control the Queensgate piezos through a serial line.
#
#%DESCRIPTION%
# These macros allow to define move and read the position of several  
#  Queensgate piezos, and to configure them as pseudomotors
#%BR%  These piezos are assumed to be controlled through
# a serial line
#%ATTENTION% 
#%UL%
#%LI%  Always put the baud rate to 2400 baud. The controller can not keep up
# with the high speed of 9600 baud for some commands. Pierre Pinel did some
# extensive tests in the lab to find that.

#%LI%  The offset knob on the controller adds to the voltage applied via the
# RS232 controlled unit. There are possitions of the knob were the computer can
# not reach the full range.

#%LI%  Reading back the encoder position can only be done with 2 bits less 
# precision and is therefore useless. The current macro set do not read back
# the position.

#%XUL%

#%UU% <motor_mnemo> <QG_list_index>
#%MDESC%
# Moves the associated Piezo with the A[] value
#%BR% motor_mnemo   :  pseudo motor mnemo name
#%BR% QG_list_index :  index in QG_DEV list

def qg_mv '{
  if (QG_OLD[$1] != A[$1]) {
    val = int(-dial($1,A[$1])*2/(QG_LONG[$2]*QG_SCALE[$2])*8192)
    if (val > 8192) 
      val = 8192
    else if (val < -8191)
      val = -8191
    qg_put(QG_DEV[$2],sprintf("N1\rM%d\r",QG_MOD[$2]))
    qg_put(QG_DEV[$2],sprintf("I%04X\r",val&0x3fff))
    qg_put(QG_DEV[$2],"N3\rN1\r")
    QG_OLD[$1] = A[$1]
  }
}'

def qg_read '{
  local str biterror compval ival
  QG_OLD[$1] = A[$1]
  if (QG_DOREAD || ($3 == 3615)) {
    qg_put(QG_DEV[$2],sprintf("M%d\rN0\rN4\r?\r",QG_MOD[$2]))
    str=qg_get(QG_DEV[$2])
    if ((str != "-1") && (str != "")) { 
      sscanf(str,"%4x",compval)
      biterror = compval & QG_MOD[$2]
      sscanf(str,"%3x",ival)
      A[$1]=user($1,(ival-(2*(ival&2048)))/2048 * QG_LONG[$2] *QG_SCALE[$2]/2)
      QG_OLD[$1] = A[$1]
    }  
  }
}'


def qg_init '{
  if (QG_MOD[$1] == 3) 
     QG_MOD[$1] = 4
  qg_put(QG_DEV[$1],sprintf("#%d\r\!QT\rM%d\r",QG_UNIT,QG_MOD[$1]))
}'

def qg_initall '{
for (iq=0;iq<QG_NO;iq++) {
  if ((  midx = motor_num(QG_MNE[iq])) != -1) {
    qg_init iq
    # 3615 is a magic number to force the read routine to actually read
    # disable the next 3 lines if you have problems reading the value
    # back from the qg controller. Whenever you type config the value
    # is read here. 
    # waitmove ; getangles
    # qg_read midx iq 3615
    # move_em; waitmove # Move qq to the pos. we just read (DANGER!!!!)
    # And when the above does not work :
    A[midx] = QG_OLD[midx]
    chg_dial(midx,dial(midx,QG_OLD[midx]))
  }
}
}'

def qg_put (devname, str) '{
  ESRF_ERR = -1
  esrf_io(devname, "DevSerWriteString",str)
  if (DEBUG&64) 
    printf("  QG %sWrite %10s: <%s>\n",\
	ESRF_ERR?"ERROR ":"",devname,no_cntl(str))
}'

def qg_get (devname) '{
  local str
  ESRF_ERR = -1
  str = esrf_io(devname, "DevSerReadString",2)
  if (DEBUG&64) 
    printf("  QG Read  %10s: <%s>\n",devname,no_cntl(str))
  if (ESRF_ERR)
    return ("-1")
  return (str)
}'

def no_cntl (str) '{
  local ch cstr
  cstr=""
  if ( (str == "-1") || (str == "") )
    return ("<Empty>")
  for (i=1; i<=length(str); i++) {
    if (substr(str,i,1) == "\r") 
      cstr=cstr"\\r"
    else if (substr(str,i,1) == "\n") 
      cstr=cstr"\\n"
    else if (asc(ch=substr(str,i,1)) < 32) 
      cstr=sprintf("%s\\%03d", cstr, asc(ch))
    else 
      cstr=sprintf("%s%s", cstr, ch)
  }
  return (cstr)
}'

#%UU% <nb_of_qg> <dev_name> <piezo_mne> <module_nb> <length> <scale>...
#%MDESC%  creates the queensgate pseudomotors.
#%UL%
#%LI% nb_of_qg  : number of devices
#  %BR% and for each device:
#%DL%
#%DT% dev_name %DD% device server name of the serial line device
#%DT% piezo_mne %DD%  piezo mnemonic
#%DT% module number %DD% 1,2,3 for M1,M2,M3
#%DT% length %DD% complete length of the piezo, 
#%DT% scale %DD% a scale factor. length is multiplied by this factor
#     This allows the length to be the real length of the piezo, and
#     the user to work in his own units.
#%XDL%
#%XUL%
def qgsetup '
{
global QG_NO QG_DEV QG_MNE QG_MAXNO QG_UNIT QG_MOD QG_LONG QG_OLD QG_SCALE
global QG_DEBUG QG_DOREAD
local _pmne _pdev _pmod _plong _pdelta

QG_DEBUG=0
QG_UNIT=7
QG_MAXNO=4
QG_DOREAD=0

_pdev[0]="$2"; _pmne[0]="$3"; _pmod[0]="$4"; _plong[0]="$5"; _pdelta[0]="$6";
_pdev[1]="$7"; _pmne[1]="$8"; _pmod[1]="$9"; _plong[1]="$10";_pdelta[1]="$11";
_pdev[2]="$12";_pmne[2]="$13";_pmod[2]="$14";_plong[2]="$15";_pdelta[2]="$16";
_pdev[3]="$17";_pmne[3]="$18";_pmod[3]="$19";_plong[3]="$20";_pdelta[3]="$21";

if ($# > QG_MAXNO*5+1) {
  print "Usage: $0 [no of piezos] [dev name] [mne] [mod] [length] [delta] .."
  exit
}

nomne = int (($#-1)/2)

if ($#) {
  QG_NO = $1
  for (i=0; i< QG_NO; i++) {
    QG_DEV[i]=_pdev[i]
    QG_MNE[i]=_pmne[i]
    QG_MOD[i]=_pmod[i]
    QG_LONG[i]=_plong[i]
    QG_SCALE[i]=_pdelta[i]
  }
} else {
  QG_NO = getval("How many queensgate piezos do you want to use ",QG_NO)
  for (i=0; i< QG_NO; i++) {
    QG_DEV[i]=getval("How is the serial line device called ",QG_DEV[i])
    QG_MNE[i] = getval(sprintf("Mnemonic of piezo %d",i+1),QG_MNE[i])
    QG_MOD[i] = getval("What is its module number (1,2,4) ",QG_MOD[i])
    QG_LONG[i] = getval("Which is its complete length ? ",QG_LONG[i])
    QG_SCALE[i] = getval("Additional scaling factor? ",QG_SCALE[i])
  }
}

for (i=0; i< QG_NO; i++) {
  if (QG_SCALE[i] == 0) 
    QG_SCALE[i]=1
}

for (i=0;i<QG_NO; i++) {
  local ppstring
  ppstring=sprintf ("%s qg_mv none qg_read none none none none %d",QG_MNE[i],i)
  pseudosdef ppstring  
} 

qg_initall
if (QG_NO) 
  cdef("config_mac","qg_initall;","queens")
else
  cdef("","","queens","delete")

}'

#%MACROS%
#%IMACROS%

#%AUTHOR%  J.KLORA - MCD 22/1/96