#%NAME% WAGOAIR.MAC
#  Control of airpad (wago controlled) for instrumentation support.
#%DESCRIPTION%
#  wagoair.mac provides air check using wago for specified motors
#
#  Air-lock can be restricted to one motion direction or none.

#%UU% <wago_channel_out> <wago_channel_in> <motor1> <direction1> <delay1> [<motor2> <direction2> <delay2>...<motorN> <directionN> <delayN>]
#%MDESC%
#%BR% <wago_channel_out> :  wago channel to activate air
#%BR% <wago_channel_in> : wago channel to read back air status, "none" if there is no input channel to check air is on
#%BR% <motor1> ... <motorN> : motors which needs air on to be moved
#%BR% <direction1> ... <directionN> : set air lock to one direction only(1/positive, -1/negative) or to any movement (0)
#%BR% <delay1>...<delayN>: delay time before and after motion
#
def airchecksetup '{
	global AIR_PAR[] AIR_AUTO AIR_FLAG
    local narg iarg dir motmne
    
    list_test AIR_PAR
    
	narg= split("$*", args)

	if ($#<5 || (!narg%2)) {
		print "Usage: $0 <wago_channel_out> <wago_channel_in|none> <motor1> <direction1> <delay1> [<motor2> <direction2> <delay2>... <motorN> <directionN> <delayN>]"
		exit
	}

	for (iarg=2; iarg<narg; iarg+=3) {
		motmne= args[iarg]
        if (motor_num(motmne) == -1) {
           print "airchecksetup: invalid motor mnemonic "motmne
           exit
        }
        if (wago__log2hard(args[0])==-1){
          print "airchecksetup: invalid wago channel "args[0]
          exit
        }
        if (args[1]!="none" && wago__log2hard(args[1])==-1){
          print "airchecksetup: invalid wago channel "args[1]
          exit
        }
        dir = args[iarg+1]
        delay = args[iarg+2]
        list_add(AIR_PAR, motmne)
		list_setpar(AIR_PAR,motmne,"wago_out",args[0])
        list_setpar(AIR_PAR, motmne, "wago_in", args[1])
        list_setpar(AIR_PAR, motmne, "direction", dir)
        list_setpar(AIR_PAR, motmne, "delay", delay)
		cdef("user_checkall", sprintf("_air_checkall %s\n", motmne), motmne, 0x21)
		cdef("user_getpangles", sprintf("_air_getangles %s\n", motmne), motmne, 0x01)
   	    cdef("user_finished1", sprintf("_air_finished %s\n", motmne), motmne)
	}

	setup_tail("aircheck", motmne)
}'

#%UU% <motor mne> <[<delay_in_sec>]
#%MDESC%
# Set or Print out air delay
#
def airdelay '{
	if ($#!=0) {
	    if (list_check(AIR_PAR,"$1") == 0) {
          print "airdelay: motor $1 has not been set for air-checking, use airchecksetup first !"
          exit
        }
        if ($# == 2) {
         list_setpar(AIR_PAR,"$1", "delay",1. * $2)
        }
        print "AIR delay for motor $1 after air on command is", AIR_PAR["$1"]["delay"], "sec."
    }else print "Usage: $0 <motor mne> [<delay>]"
}'
#%IU% <motormne>
def aircheckunsetup '{
    cdef("", "", "$1", "delete")
	list_remove(AIR_PAR, "$1")
	print "AIR CHECK no longer activated for motor $1"
    if (list_n(AIR_PAR) == 0) {
      unglobal AIR_PAR AIR_AUTO AIR_FLAG
    }

}'

#%IU%
def _air_checkall '{
	local state _need_air _dir _sign motnum str
    motnum = motor_num("$1")
    
	if (AIR_PAR["$1"]["oldpos"]!= A[motnum]) {
      _dir = AIR_PAR["$1"]["direction"]
      _sign = (A[motnum]-AIR_PAR["$1"]["oldpos"])>0?1:-1
      #check if direction restriction is set then move if same direction
      if (_dir==0) _need_air = 1
      else {
        if  (_dir == _sign) _need_air = 1
        else _need_air = 0
      }
      str=(_dir!=0?(_dir==1? " in positive direction": " in negative direction"):"")

      AIR_PAR["$1"]["back"]=0
      if (_need_air) {
        if (AIR_AUTO) {
          if(!_air_setcheck("$1",1)) {
            print "Cannot set air ON to move <$1>, motion ABORTED"
            exit
          }
          AIR_PAR["$1"]["back"]=1
        }else {
          if(!_air_get("$1")) {
            print "Air should be ON to move <$1>" str ", motion ABORTED"
            print "Set automatic aircheck with \"airautoon\" or manually set air with \"airon\" "             
            exit
          }
        }        
      } else {
        if(AIR_AUTO) { 
          # always check air if off in reverse direction restriction, in case air was set manually !!
          if(! _air_setcheck("$1",0)) {
            print "Cannot set air OFF, to move <$1>, motion ABORTED"
            exit          
          }
        } else {
          if(_air_get("$1")) {
            print "Air should be OFF to move <$1>" str ", motion ABORTED"
            print "Set automatic aircheck with \"airautoon\" or manually set air with \"airon\" "             
            exit
          }
        }
      }
    }
}'

#%IU% <motmne>
def _air_getangles '{
	AIR_PAR["$1"]["oldpos"]= A[motor_num("$1")]
}'

#%IU% <motmne>
def _air_finished '{
	if (AIR_PAR["$1"]["back"]) {
		_air_setcheck("$1",0)
    }
}'

#%IU% (motor, value)
def _air_set(motor,value) '{
	wago_writech(AIR_PAR[motor]["wago_out"], value)
}'

#%IU% (motor)
def _air_get(motor) '{
	if (AIR_PAR[motor]["wago_in"] != "none") {
      return wago_readch(AIR_PAR[motor]["wago_in"])
    }else {
      # if there is no input channel for pressiostat, just return the command channel state
      return wago_readch(AIR_PAR[motor]["wago_out"])
    }
}'

#%IU% (motor, value)
def _air_setcheck(motor,value) '{
	local retval delay

    retval= _air_get(motor)
    if (retval != value) {
	  _air_set(motor,value)
      delay = AIR_PAR[motor]["delay"]
      tty_cntl("md");printf("\t\t<aircheck active>");tty_cntl("me")
      printf(", waittime %.2f sec. ...\r",delay)
	  sleep(delay)
	
      retval= _air_get(motor)
	  if (retval != value) {
		  printf("AIR ERROR >> Cannot set air %s\f", value ? "ON" : "OFF")
		  return (0)
	  }
	  return (1)
    }
    return (1)
}'

#%UU% <motmne>
#%MDESC% Auto mode set air on if motors are moving, and released air after motion
def airauto '{
	print "Airpad auto mode is ", (AIR_AUTO?"ON":"OFF")
}'

#%UU%
#%MDESC% Set auto mode ON
def airautoon '{
	AIR_AUTO= 1
}'

#%UU%
#%MDESC% Set auto mode OFF
def airautooff '{
	AIR_AUTO= 0
}'

#%UU% <motmne>
#%MDESC% Switch ON air
def airon '{
  if ($#!=1) {
    print "Usage: $0 <motmne>"
  }else {
    _air_setcheck("$1",1) 
  }
}'

#%UU% <motmne>
#%MDESC% Switch OFF air
def airoff '{
    if ($#!=1) {
    print "Usage: $0 <motmne>"
  }else {
    _air_setcheck("$1",0)
  }
}'

#%UU% <motmne>
#%MDESC% diagnosis of air control
def aircheck '{
	local error
	error= 0

  if ($#!=1) {
    print "Usage: $0 <motmne>"
  }else {
	print "--> switching air on:"
	if (!_air_setcheck("$1",1))
		error++
    else
		print "... OK."

	sleep(5 * AIR_PAR[motor]["delay"])

	print "--> switching air off:"
	if (!_air_setcheck("$1",0))
		error++
	else
		print "... OK."

	if (error) {
		print "air control not functionnal >> fix problems !!"
	}
  }
}'

#%UU%
#%MDESC% list the aircheck configured motors
def airshow '{
  local num _i motmne wout win dir delay stat
  
  num=list_n(AIR_PAR)
  
  printf("\n%s Motor(s) with wago aircheck declared :\n",num? num: "No")
  print "-----------------------------------------------------------------"
  tty_cntl("md")
  printf("%-8.8s %-12.12s %-12.12s %-12.12s %-8.8s %-8.8s\n","motor", "channel_out", "channel_in", "direction", "delay", "\"air is\"")
  tty_cntl("me")
  for (_i=1; _i<=num;_i++) {
     motmne = AIR_PAR[_i]
     wout = AIR_PAR[motmne]["wago_out"]
     win = AIR_PAR[motmne]["wago_in"]
     dir = AIR_PAR[motmne]["direction"]
     delay = AIR_PAR[motmne]["delay"]
     stat = _air_get(motmne)              
     printf("%-8.8s %-12.12s %-12.12s %-12.12s %-8.8s %-8.8s\n", motmne, wout, win, dir==1?"pos":(dir==-1?"neg":"both"), delay,stat?"ON":"OFF")
  }


}'
#%UU% 
#%MDESC% to be used with warningsetup [warning1] [warning2] ...
def airwarning '
	if (AIR_FLAG) {
    	tty_cntl("md")
		printf("\n- AIR IS ON %s", AIR_AUTO?"(auto mode)":"")
		tty_cntl("me")
	}
'

#%MACROS%
#%IMACROS%
#%AUTHOR% E. PAPILLON & L. CLAUSTRE / BCU / ESRF ( 2012 )
#%TOC%
#$Revision: 1.1 $ $Date: 2012/09/06 13:50:35 $%BR%
#%END%
#%LOG%
#$Log: wagoair.mac,v $
#Revision 1.1  2012/09/06 13:50:35  claustre
#Initial revision
#
#