#
# Filename: $RCSfile: hexapod.mac,v $
#
# $Source: /cvs/G/SPEC/local/X12SA/macros/hexapod.mac,v $
# $Revision: 1.2 $  $Date: 2008/05/06 16:21:15 $
# $Author: bunk $
# $Tag: $
#
# Description:
# spec macros for communictaion via EPICS with a C-887 PI hexapod controller.
#
# Note:

# Installed location: 
# $SLSBASE/sls/spec/local/X04SA/ES3/
#
# Macro definitions which are changed with respect to standard.mac:
# - none
#
# Chained macro definitions:
# - none
#
# history: 


constant _hex_baseplate_thickness 20.0


# ----------------------------------------------------------------------
def hex_show_pivot_point '{
  local x,y,z

  x = epics_get("X04SA-ES3-HXP:RBPIVOT_X")
  y = epics_get("X04SA-ES3-HXP:RBPIVOT_Y")
  z = epics_get("X04SA-ES3-HXP:RBPIVOT_Z")

  z =  z - _hex_baseplate_thickness

  printf("pivot point xyz =  (%.4f, %.4f, %.4f) mm relative to center of Hexapod top plate surface\n",\
         x,y,z)
}'


# ----------------------------------------------------------------------
def hex_set_pivot_point '{
  if (($# != 3) && !(($# == 1) && ("$1" == "_show"))) {
    printf("Usage: $0 <x> <y> <z>\n")
    printf("The (0,0,0) point is in the center of the top surface of the Hexapod\n")
    printf("top plate.\n")
    if ($# != 0) {
      exit
    }
  }

  if ($# == 3) {
    local x,y,z

    # check if the dial values of the rotations are zero
    if ((dial(hrox,A[hrox]) != 0.0) || \
        (dial(hroy,A[hroy]) != 0.0) || \
        (dial(hroz,A[hroz]) != 0.0)) {
      printf("The dial positions of the Hexapod rotations hrox, hroy and hroz have\n")
      printf("to be at their zero position to set the pivot point.\n")
      printf("If it is save to do so execute\n")
      printf("  umv hrox %.6f  hroy %.6f  hroz %.6f\n",\
              user(hrox,0),user(hroy,0),user(hroz,0))
      printf("please.\n")
      exit
    }
    _arg_num 1 "$0" $1
    x = _num
    _arg_num 1 "$0" $2
    y = _num
    _arg_num 1 "$0" $3
    z = _num + _hex_baseplate_thickness
    # perform spec -> Hexapod coordinate transform
    epics_put("X04SA-ES3-HXP:PIVOT_X", x)
    epics_put("X04SA-ES3-HXP:PIVOT_Y", y)
    epics_put("X04SA-ES3-HXP:PIVOT_Z", z)
  }

  hex_show_pivot_point  
}'

def hex_ini '{
  printf("Initialiaze Hexapod C-887 through EPICS\n")
  home hx
  waitmove
}'

# ----------------------------------------------------------------------
def hex_show_all '{
  printf("PI C-887 Hexapod:\n")
  hex_show_pivot_point
  printf("\n")
  wm hx hy hz  hrox hroy hroz
}'

#---------------------------------------------------------------------
# Oveloading of function
def hexaShow '{
  hex_show_all
}'