// some utility functions to go along with the OMS libraries
// it is a little scary that some of these functions weren't included
// Brian Leibowitz
// Created: July 2, 1998

#include "OMSUtil.h"




// amazingly enough, this function doesn't appear in their library
int SetOmsAllAccelerations(HANDLE omsh, long acc)
{
	SetOmsAxisAcceleration(omsh, OMS_X_AXIS, acc);
	SetOmsAxisAcceleration(omsh, OMS_Y_AXIS, acc);
	SetOmsAxisAcceleration(omsh, OMS_Z_AXIS, acc);
	SetOmsAxisAcceleration(omsh, OMS_T_AXIS, acc);
	SetOmsAxisAcceleration(omsh, OMS_U_AXIS, acc);
	return 
	SetOmsAxisAcceleration(omsh, OMS_V_AXIS, acc);
}


// amazingly enough, this function doesn't appear in their library
int SetOmsAllVelocities(HANDLE omsh, long vel)
{
	SetOmsAxisVelocity(omsh, OMS_X_AXIS, vel);
	SetOmsAxisVelocity(omsh, OMS_Y_AXIS, vel);
	SetOmsAxisVelocity(omsh, OMS_Z_AXIS, vel);
	SetOmsAxisVelocity(omsh, OMS_T_AXIS, vel);
	SetOmsAxisVelocity(omsh, OMS_U_AXIS, vel);
	return SetOmsAxisVelocity(omsh, OMS_V_AXIS, vel);
}




int MoveOmsAllAxesAbs( HANDLE omsHandle
              , float x
              , float y
              , float z
              , float t
              , float u
              , float v
              )
{
	int error;

	if( omsHandle == NULL ) 
		return -1;       


	error = MoveOmsAxisAbs(omsHandle, OMS_X_AXIS, (long int)x);
	if( error != SUCCESS ) return error;
	error = MoveOmsAxisAbs(omsHandle, OMS_Y_AXIS, (long int)y);
	if( error != SUCCESS ) return error;
	error = MoveOmsAxisAbs(omsHandle, OMS_Z_AXIS, (long int)z);
	if( error != SUCCESS ) return error;
	error = MoveOmsAxisAbs(omsHandle, OMS_T_AXIS, (long int)t);
	if( error != SUCCESS ) return error;
	error = MoveOmsAxisAbs(omsHandle, OMS_U_AXIS, (long int)u);
	if( error != SUCCESS ) return error;
	error = MoveOmsAxisAbs(omsHandle, OMS_V_AXIS, (long int)v);
	
	return error;

}


int HomeOmsAllAxes(HANDLE omsHandle) 
{
	int error;
	// use a very slow home velocity
	error = SetOmsAllVelocities(omsHandle, 100); 		if( error != SUCCESS ) return error;

	// do the home sequence for all six axes
	error = HomeOmsAxis( omsHandle, OMS_X_AXIS );		if( error != SUCCESS ) return error;
	error = HomeOmsAxis( omsHandle, OMS_Y_AXIS );		if( error != SUCCESS ) return error;
	error = HomeOmsAxis( omsHandle, OMS_Z_AXIS );		if( error != SUCCESS ) return error;
	error = HomeOmsAxis( omsHandle, OMS_T_AXIS );		if( error != SUCCESS ) return error;
	error = HomeOmsAxis( omsHandle, OMS_U_AXIS );		if( error != SUCCESS ) return error;
	error = HomeOmsAxis( omsHandle, OMS_V_AXIS );		if( error != SUCCESS ) return error;

	// return all axes to use the commanded velocity value
	//SetOmsAllVelocities(omsHandle, oldVelocity=??? );

	return SUCCESS;				// this should return error code
}

int HomeOmsAxis( HANDLE omsHandle, int axis)
{
	int error;
	// disable limit switches, since limit and home switch are the same
	error = SetOmsAxisOvertravelDetect(omsHandle, axis, MODE_OFF); 
	if( error != SUCCESS ) return error;                                        
	
	// go until we hit the switch and call that position -15 steps
	// this way when we return to position 0 we have a little buffer
	// before hitting the negative travel limit.  increase this value
	// if switch hysteresis leaves the limit switch activated after homing
	error = DefineOmsHomeAsSwitchClosed(omsHandle, axis);
	if( error != SUCCESS ) return error;                                        
	error = HomeOmsAxisRevUseSwitch(omsHandle, axis, -15);
	if( error != SUCCESS ) return error;                                        

	// move the specified axis to position 0 
	error = MoveOmsAxisAbs(omsHandle, axis, 0);	
	if( error != SUCCESS ) return error;                                        

	// turn limit switches back on
	error = SetOmsAxisOvertravelDetect(omsHandle, axis, MODE_ON);                                         
	if( error != SUCCESS ) return error;                                        

	return SUCCESS;				// this should return error code

}