/*	**************************************************************  */
/*
 *	Module ESP7000
 *	This module contains I/O functions for the Newport model
 *	ESP7000 controller.                 
 *
 *	Zachary Wolf
 *	5/13/05
 */ 

/*	**************************************************************  */
/*	INCLUDES  */
#include "repository.h"
#include DELAY //adds most commonly used functions

#include <utility.h>
#include <gpib.h>
#include <formatio.h>
#include <ansi_c.h>
#include "esp7000.h"

/*	**************************************************************  */

/*  PRIVATE FUNCTIONS  */
int esp7000_open_dev(int gpib_board_addr, int gpib_dev_addr);
void esp7000_close_dev(int dev_ID);
int esp7000_out(int dev_ID, char *buf);
int esp7000_in(int dev_ID, char* buf);
int esp7000_check_dev_open(int dev_ID);
int esp7000_check_axis_active(int dev_ID, int axis);
int esp7000_check_errors(int dev_ID);
void esp7000_hold_until_move_complete(int dev_ID, int axis);
void esp7000_message(char* msg);
void esp7000_error(char* msg);

/*	**************************************************************  */

/*  PRIVATE DEVICE TABLE  */
/*
 *	This table allows several devices of the same type to be
 *	used in the system.
 *	dev_addr, contains the GPIB addresses of opened devices
 *  dev_descr, contains the device descriptors of opened devices
 *  dev_count, contains the number of devices open of this type
 */
static int dev_addr[ESP7000_MAX_NUM_DEV + 1];
static int dev_descr[ESP7000_MAX_NUM_DEV + 1];
static int dev_count;

/*	**************************************************************  */

/*  PRIVATE GLOBAL VARIABLES  */
/*
 *  cmd, buffer for GPIB I/O strings
 *  msg, buffer for message strings to Standard I/O
 */
static char cmd[ESP7000_MAX_CMD + 1];
static char msg[ESP7000_MAX_CMD + 1];

/*	**************************************************************  */

/*	PUBLIC FUNCTIONS  */

/*	**************************************************************  */
/*
 *  esp7000_init
 *  This function opens the device, queries for ID, and
 *  initializes the device to a known state.
 *
 *	Input:
 *	gpib_board_addr, address of the GPIB board the device is connected to (0, 1, ...)
 *	gpib_dev_addr, GPIB address of the device (1 to 30, 0 is reserved)
 *
 *	Output:
 *	ID, identifier for future references to the device
 *
 *	Zachary Wolf
 *	5/16/05
 */

void esp7000_init(int gpib_board_addr, int gpib_dev_addr, int* ID)
{
    /* Declare variables */
    int dev_ID;
    int err;
    int axis, status;
    
    /* Message */
    esp7000_message("");
    esp7000_message("Initializing the ESP7000...");

    /* Check input parameters */
    if (gpib_board_addr < 0 || gpib_board_addr > 10)
    {
    	Fmt(msg, "%s<gpib_board_addr = %i, out of range", gpib_board_addr);
    	esp7000_error(cmd);
    	return;
    }
    if (gpib_dev_addr < 1 || gpib_dev_addr > 30)
    {
    	Fmt(msg, "%s<gpib_dev_addr = %i, out of range", gpib_dev_addr);
    	esp7000_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	*ID = 1;
    	return;
    #endif
    
    /* Open the ESP7000 */
    dev_ID = esp7000_open_dev(gpib_board_addr, gpib_dev_addr);
    if (dev_ID <= 0)
    {
        esp7000_error("Device was not opened");
        return;
    }
    
    /* Talk to the ESP7000 */
    esp7000_out(dev_ID, "VE?");  /* Get firmware version */
    err = esp7000_in(dev_ID, cmd);
    if (err != 0)
    {
        esp7000_error("Problem reading dev_ID");
        esp7000_close_dev(dev_ID);
        return;
    }
    esp7000_message(cmd);
    
    /* Print the active axes */
    printf("Active axis status: ");
    for (axis = 1; axis <= ESP7000_MAX_NUM_AXES; axis++)
    {
		status = esp7000_check_axis_active(dev_ID, axis);
		printf("%2i", status);
	}
	printf("\n");
    
    /* Set the controller units to millimeters for the active axes */
    /* All functions still use MKS units to pass parameters */
    for (axis = 1; axis <= ESP7000_MAX_NUM_AXES; axis++)
    {
		status = esp7000_check_axis_active(dev_ID, axis);
		if (status == 1)
		{
			Fmt(cmd, "%s<%iSN2", axis);
		    esp7000_out(dev_ID, cmd);
		}
	}
    
    /* Turn on power to the active axes */
    for (axis = 1; axis <= ESP7000_MAX_NUM_AXES; axis++)
    {
		status = esp7000_check_axis_active(dev_ID, axis);
		if (status == 1)
		{
			Fmt(cmd, "%s<%iMO", axis);
		    esp7000_out(dev_ID, cmd);
		}
	}
  	
	/* Move all active axes to the home position */
    //for (axis = 1; axis <= ESP7000_MAX_NUM_AXES; axis++)
    //{
	//	esp7000_move_home(dev_ID, axis);
	//}
    
    /* Check for errors */
	esp7000_check_errors(dev_ID);

	/* Return the ID number */
    *ID = dev_ID;
    
    /* Done */
    return;
}
								 
/*	**************************************************************  */
/*
 *	esp7000_move_home
 *  This function moves the specified axis to its home position.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, axis to move (1, 2, ...)
 *
 *	Zachary Wolf
 *	5/16/05
 */

void esp7000_move_home(int dev_ID, int axis)
{
	/* Declare variables */
	int dev_open;
	int axis_active;
	double pos;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    if (axis < 1 || axis > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis = %i, out of range (1, ..., %i)", axis, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = esp7000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	esp7000_error("Device not open");
    	return;
    }
    
    /* Check to make sure the axis is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis);
	if (axis_active != 1) return;
	
	/* Set the axis to trapezoidal mode */
	Fmt(cmd, "%s<%iTJ01", axis);
    esp7000_out(dev_ID, cmd);
	
	/* Make the home position the zero position */
	Fmt(cmd, "%s<%iSH0", axis);
    esp7000_out(dev_ID, cmd);
    
    /* Set the speed for the move home */
	Fmt(cmd, "%s<%iOH%f", axis, ESP7000_MAX_VEL*1000);  /* Move at the maximum speed, but convert to mm/sec */
    esp7000_out(dev_ID, cmd);
    
    /* Message */
    printf("Moving axis %i to its home position...\n", axis);
    
    /* Move to the home position */
	Fmt(cmd, "%s<%iOR4", axis);  /* Use the negative limit to define the home position. */
    esp7000_out(dev_ID, cmd);
   
    /* Wait unil the move is complete */
    esp7000_hold_until_move_complete(dev_ID, axis);
    
    /* Check that the position is zero */
	Fmt(cmd, "%s<%iTP", axis);
    esp7000_out(dev_ID, cmd);
    esp7000_in(dev_ID, msg);
    Scan(msg, "%s>%f", &pos);
    if (abs(pos) > 0.001) esp7000_error("Position wrong after home.");
    
    /* Check for errors */
	esp7000_check_errors(dev_ID);
    
    /* Done */
    return;
}
								 
/*	**************************************************************  */
/*
 *	esp7000_rel_stage_move
 *  This function moves a stage a given distance relative to
 *	its present position.  The acceleration and velocity are specified.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, axis to move (1, 2, ...)
 *	acc, acceleration (m/sec/sec)
 *	vel, velocity (m/sec)
 *	dis, distance (m), can be + or -
 *
 *	Zachary Wolf
 *	5/27/05
 */

void esp7000_rel_stage_move(int dev_ID, int axis, double acc, double vel, double dis)
{
	/* Declare variables */
	int dev_open;
	int axis_active;
	double ini_pos, fin_pos;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    if (axis < 1 || axis > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis = %i, out of range (1, ..., %i)", axis, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    if (acc <= 0.)
    {
    	Fmt(msg, "%s<acc = %f, out of range, must be positive, fixing...", acc);
    	acc = 0.000001;  /* m/s/s */
    }
    if (acc > ESP7000_MAX_ACC)
    {
    	Fmt(msg, "%s<acc = %f, out of range, too large, fixing...", acc);
    	acc = ESP7000_MAX_ACC;
    }
    if (vel <= 0.)
    {
    	Fmt(msg, "%s<vel = %f, out of range, must be positive, fixing...", vel);
    	vel = 0.000001;  /* m/s */
    }
    if (vel > ESP7000_MAX_VEL)
    {
    	Fmt(msg, "%s<vel = %f, out of range, too large, fixing...", vel);
    	vel = ESP7000_MAX_VEL;
    }
    if (dis < -ESP7000_MAX_DIS || dis > ESP7000_MAX_DIS)
    {
    	Fmt(msg, "%s<dis = %f, out of range", dis);
    	esp7000_error(msg);
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = esp7000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	esp7000_error("Device not open");
    	return;
    }
    
    /* Check to make sure the axis is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis);
	if (axis_active != 1) return;
	
	/* Set the axis to trapezoidal mode */
	Fmt(cmd, "%s<%iTJ01", axis);
    esp7000_out(dev_ID, cmd);
    
    /* Get the initial motor position before the move */
	esp7000_get_pos(dev_ID, axis, &ini_pos);
    
    /* Set the acceleration, required units mm/sec/sec */
	Fmt(cmd, "%s<%iAC%f", axis, acc*1000);
    esp7000_out(dev_ID, cmd);
    
    /* Set the deceleration, required units mm/sec/sec */
	Fmt(cmd, "%s<%iAG%f", axis, acc*1000);
    esp7000_out(dev_ID, cmd);
    
    /* Set the velocity, required units mm/sec */
	Fmt(cmd, "%s<%iVA%f", axis, vel*1000);
    esp7000_out(dev_ID, cmd);
    
    /* Move the stage, required units mm */
    Fmt(cmd, "%s<%iPR%f", axis, dis*1000);
    esp7000_out(dev_ID, cmd);
   
    /* Wait unil the move is complete */
    Delay(fabs(dis)/vel + 2*vel/acc);
    esp7000_hold_until_move_complete(dev_ID, axis);
    
    /* Check for errors */
	esp7000_check_errors(dev_ID);
    
    Delay(1.); 
    
    /* Get the motor position after the move */
	esp7000_get_pos(dev_ID, axis, &fin_pos);
	
	/* Check the move */
	if (fabs(fin_pos - ini_pos - dis) > 1.5e-6)
		esp7000_error("esp7000_rel_stage_move: the stage did not move the proper amount");
    
    /* Done */
    return;
}
								 
/*	**************************************************************  */
/*
 *	esp7000_rel_2_stage_move
 *  This function moves two stages a given distance relative to
 *	their present position.  The acceleration and velocity are specified.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis1, axis 1 (1, 2, ...)
 *	axis2, axis 2 (1, 2, ...)
 *	acc, acceleration (m/sec/sec)
 *	vel, velocity (m/sec)
 *	dis, distance (m), can be + or -
 *
 *	Zachary Wolf
 *	5/27/05
 */

void esp7000_rel_2_stage_move(int dev_ID, int axis1, int axis2, double acc, double vel, double dis)
{
	/* Declare variables */
	int dev_open;
	int axis_active;
	double ini_pos1, fin_pos1;
	double ini_pos2, fin_pos2;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    if (axis1 < 1 || axis1 > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis1 = %i, out of range (1, ..., %i)", axis1, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    if (axis2 < 1 || axis2 > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis2 = %i, out of range (1, ..., %i)", axis2, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    if (acc <= 0.)
    {
    	Fmt(msg, "%s<acc = %f, out of range, must be positive, fixing...", acc);
    	acc = 0.000001;  /* m/s/s */
    }
    if (acc > ESP7000_MAX_ACC)
    {
    	Fmt(msg, "%s<acc = %f, out of range, too large, fixing...", acc);
    	acc = ESP7000_MAX_ACC;
    }
    if (vel <= 0.)
    {
    	Fmt(msg, "%s<vel = %f, out of range, must be positive, fixing...", vel);
    	vel = 0.000001;  /* m/s */
    }
    if (vel > ESP7000_MAX_VEL)
    {
    	Fmt(msg, "%s<vel = %f, out of range, too large, fixing...", vel);
    	vel = ESP7000_MAX_VEL;
    }
    if (dis < -ESP7000_MAX_DIS || dis > ESP7000_MAX_DIS)
    {
    	Fmt(msg, "%s<dis = %f, out of range", dis);
    	esp7000_error(msg);
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = esp7000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	esp7000_error("Device not open");
    	return;
    }
    
    /* Check to make sure axis1 is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis1);
	if (axis_active != 1) return;
    
    /* Check to make sure axis2 is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis2);
	if (axis_active != 1) return;
	
	/* Setup the axes so axis1 is the master and axis2 is the slave */
	Fmt(cmd, "%s<%iSS%i", axis2, axis1);
    esp7000_out(dev_ID, cmd);
	
	/* Set axis1 to trapezoidal mode */
	Fmt(cmd, "%s<%iTJ01", axis1);
    esp7000_out(dev_ID, cmd);
	
	/* Set axis2 to slave the position of axis1 */
	Fmt(cmd, "%s<%iTJ05", axis2);
    esp7000_out(dev_ID, cmd);
    
    /* Get the initial motor position before the move */
	esp7000_get_pos(dev_ID, axis1, &ini_pos1);
	esp7000_get_pos(dev_ID, axis2, &ini_pos2);
    
    /* Set the acceleration, required units mm/sec/sec */
	Fmt(cmd, "%s<%iAC%f", axis1, acc*1000);
    esp7000_out(dev_ID, cmd);
    
    /* Set the deceleration, required units mm/sec/sec */
	Fmt(cmd, "%s<%iAG%f", axis1, acc*1000);
    esp7000_out(dev_ID, cmd);
    
    /* Set the velocity, required units mm/sec */
	Fmt(cmd, "%s<%iVA%f", axis1, vel*1000);
    esp7000_out(dev_ID, cmd);
    
    /* Move the stages, required units mm */
    Fmt(cmd, "%s<%iPR%f", axis1, dis*1000);
    esp7000_out(dev_ID, cmd);
   
    /* Wait unil the move is complete */
    Delay(fabs(dis)/vel + 2*vel/acc);
    esp7000_hold_until_move_complete(dev_ID, axis1);
    esp7000_hold_until_move_complete(dev_ID, axis2);
	
	/* Set axis2 back to trapezoidal mode, ie. un-slave it */
	Fmt(cmd, "%s<%iTJ01", axis2);
    esp7000_out(dev_ID, cmd);
    
    /* Check for errors */
	esp7000_check_errors(dev_ID);
    
    Delay(1.); 
    
    /* Get the motor position after the move */
	esp7000_get_pos(dev_ID, axis1, &fin_pos1);
	esp7000_get_pos(dev_ID, axis2, &fin_pos2);
	
	/* Check the move */
	if (fabs(fin_pos1 - ini_pos1 - dis) > 1.5e-6)
		esp7000_error("esp7000_rel_stage_move: the stage did not move the proper amount");
	if (fabs(fin_pos2 - ini_pos2 - dis) > 1.5e-6)
		esp7000_error("esp7000_rel_stage_move: the stage did not move the proper amount");
    
    /* Done */
    return;
}
								 
/*	**************************************************************  */
/*
 *	esp7000_abs_stage_move
 *  This function moves a stage to a specified position.
 *	The acceleration and velocity are specified.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, axis to move (1, 2, ...)
 *	acc, acceleration (m/sec/sec)
 *	vel, velocity (m/sec)
 *	pos, position (m), can be + or -
 *
 *	Zachary Wolf
 *	5/16/05
 */

void esp7000_abs_stage_move(int dev_ID, int axis, double acc, double vel, double pos)
{
	/* Declare variables */
	int dev_open;
	int axis_active;
	double fin_pos;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    if (axis < 1 || axis > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis = %i, out of range (1, ..., %i)", axis, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    if (acc <= 0.)
    {
    	Fmt(msg, "%s<acc = %f, out of range, must be positive, fixing...", acc);
    	acc = 0.000001;  /* m/s/s */
    }
    if (acc > ESP7000_MAX_ACC)
    {
    	Fmt(msg, "%s<acc = %f, out of range, too large, fixing...", acc);
    	acc = ESP7000_MAX_ACC;
    }
    if (vel <= 0.)
    {
    	Fmt(msg, "%s<vel = %f, out of range, must be positive, fixing...", vel);
    	vel = 0.000001;  /* m/s */
    }
    if (vel > ESP7000_MAX_VEL)
    {
    	Fmt(msg, "%s<vel = %f, out of range, too large, fixing...", vel);
    	vel = ESP7000_MAX_VEL;
    }
    if (pos < -ESP7000_MAX_DIS || pos > ESP7000_MAX_DIS)
    {
    	Fmt(msg, "%s<pos = %f, out of range", pos);
    	esp7000_error(msg);
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = esp7000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	esp7000_error("Device not open");
    	return;
    }
    
    /* Check to make sure the axis is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis);
	if (axis_active != 1) return;
	
	/* Set the axis to trapezoidal mode */
	Fmt(cmd, "%s<%iTJ01", axis);
    esp7000_out(dev_ID, cmd);
    
    /* Set the acceleration, required units m/sec/sec */
	Fmt(cmd, "%s<%iAC%f", axis, acc*1000);
    esp7000_out(dev_ID, cmd);
    
    /* Set the deceleration, required units m/sec/sec */
	Fmt(cmd, "%s<%iAG%f", axis, acc*1000);
    esp7000_out(dev_ID, cmd);
    
    /* Set the velocity, required units m/sec */
	Fmt(cmd, "%s<%iVA%f", axis, vel*1000);
    esp7000_out(dev_ID, cmd);
    
    /* Move the stage, required units m */
    Fmt(cmd, "%s<%iPA%f", axis, pos*1000);
    esp7000_out(dev_ID, cmd);
   
    /* Wait unil the move is complete */
    esp7000_hold_until_move_complete(dev_ID, axis);
    
    /* Check for errors */
	esp7000_check_errors(dev_ID);
	
	Delay(1.);
    
    /* Get the motor position after the move */
	
	esp7000_get_pos(dev_ID, axis, &fin_pos);
	
	/* Check the move */
	if (fabs(fin_pos - pos) > 1.5e-6)
		esp7000_error("esp7000_abs_stage_move: the stage did not move the proper amount");
    
    /* Done */
    return;
}
								 
/*	**************************************************************  */
/*
 *	esp7000_abs_2_stage_move
 *  This function moves two stages to a specified position.
 *	The acceleration and velocity are specified.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis1, axis 1 (1, 2, ...)
 *	axis2, axis 2 (1, 2, ...)
 *	acc, acceleration (m/sec/sec)
 *	vel, velocity (m/sec)
 *	pos, position (m), can be + or -	 
 *
 *	Zachary Wolf
 *	5/27/05
 */

void esp7000_abs_2_stage_move(int dev_ID, int axis1, int axis2, double acc, double vel, double pos)
{
	/* Declare variables */
	int dev_open;
	int axis_active;
	double fin_pos1, fin_pos2;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    if (axis1 < 1 || axis1 > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis1 = %i, out of range (1, ..., %i)", axis1, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    if (axis2 < 1 || axis2 > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis2 = %i, out of range (1, ..., %i)", axis2, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    if (acc <= 0.)
    {
    	Fmt(msg, "%s<acc = %f, out of range, must be positive, fixing...", acc);
    	acc = 0.000001;  /* m/s/s */
    }
    if (acc > ESP7000_MAX_ACC)
    {
    	Fmt(msg, "%s<acc = %f, out of range, too large, fixing...", acc);
    	acc = ESP7000_MAX_ACC;
    }
    if (vel <= 0.)
    {
    	Fmt(msg, "%s<vel = %f, out of range, must be positive, fixing...", vel);
    	vel = 0.000001;  /* m/s */
    }
    if (vel > ESP7000_MAX_VEL)
    {
    	Fmt(msg, "%s<vel = %f, out of range, too large, fixing...", vel);
    	vel = ESP7000_MAX_VEL;
    }
    if (pos < -ESP7000_MAX_DIS || pos > ESP7000_MAX_DIS)
    {
    	Fmt(msg, "%s<pos = %f, out of range", pos);
    	esp7000_error(msg);
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = esp7000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	esp7000_error("Device not open");
    	return;
    }
    
    /* Check to make sure axis1 is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis1);
	if (axis_active != 1) return;
    
    /* Check to make sure axis2 is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis2);
	if (axis_active != 1) return;
	
	/* Set the axes to trapezoidal mode */
	Fmt(cmd, "%s<%iTJ01", axis1);
    esp7000_out(dev_ID, cmd);
	Fmt(cmd, "%s<%iTJ01", axis2);
    esp7000_out(dev_ID, cmd);
    
    /* Set the acceleration, required units mm/sec/sec */
	Fmt(cmd, "%s<%iAC%f", axis1, acc*1000);
    esp7000_out(dev_ID, cmd);
	Fmt(cmd, "%s<%iAC%f", axis2, acc*1000);
    esp7000_out(dev_ID, cmd);
    
    /* Set the deceleration, required units mm/sec/sec */
	Fmt(cmd, "%s<%iAG%f", axis1, acc*1000);
    esp7000_out(dev_ID, cmd);
	Fmt(cmd, "%s<%iAG%f", axis2, acc*1000);
    esp7000_out(dev_ID, cmd);
    
    /* Set the velocity, required units mm/sec */
	Fmt(cmd, "%s<%iVA%f", axis1, vel*1000);
    esp7000_out(dev_ID, cmd);
	Fmt(cmd, "%s<%iVA%f", axis2, vel*1000);
    esp7000_out(dev_ID, cmd);
    
    /* Move the stages, required units mm */
    Fmt(cmd, "%s<%iPA%f;%iPA%f", axis1, pos*1000, axis2, pos*1000);
    esp7000_out(dev_ID, cmd);
   
    /* Wait unil the move is complete */
    esp7000_hold_until_move_complete(dev_ID, axis1);
    esp7000_hold_until_move_complete(dev_ID, axis2);
    
    /* Check for errors */
	esp7000_check_errors(dev_ID);
    
    Delay(1.); 
    
    /* Get the motor position after the move */
	esp7000_get_pos(dev_ID, axis1, &fin_pos1);
	esp7000_get_pos(dev_ID, axis2, &fin_pos2);
	
	/* Check the move */
	if (fabs(fin_pos1 - pos) > 1.5e-6)
		esp7000_error("esp7000_abs_2_stage_move: the stage did not move the proper amount");
	if (fabs(fin_pos2 - pos) > 1.5e-6)
		esp7000_error("esp7000_abs_2_stage_move: the stage did not move the proper amount");
    
    /* Done */
    return;
}
								 
/*	**************************************************************  */
/*
 *	esp7000_zero
 *  This function zeros the specified axis.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, axis to zero (1, 2, ...)
 *
 *	Zachary Wolf
 *	5/16/05
 */

void esp7000_zero(int dev_ID, int axis)
{
	/* Declare variables */
	int dev_open;
	int axis_active;
	double fin_pos;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    if (axis < 1 || axis > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis = %i, out of range (1, ..., %i)", axis, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = esp7000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	esp7000_error("Device not open");
    	return;
    }
    
    /* Check to make sure the axis is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis);
	if (axis_active != 1) return;
    
    /* Zero the stage */
	Fmt(cmd, "%s<%iDH0.0", axis);
    esp7000_out(dev_ID, cmd);
    
    /* Check for errors */
	esp7000_check_errors(dev_ID);
    
    /* Get the stage position after the move */
	esp7000_get_pos(dev_ID, axis, &fin_pos);
	
	/* Check the position */
	if (fabs(fin_pos) > 1.5e-6)
		esp7000_error("esp7000_zero: the stage did not zero properly");
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	esp7000_activate_axis
 *  This function turns on the power to the axis
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, system axis (1, 2, ...)
 *
 *
 *	Michael Levashov
 *	8/22/07
 */
void esp7000_activate_axis(int dev_ID, int axis)
{
	/* Declare variables */
	int dev_open;
	int axis_active;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    if (axis < 1 || axis > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis = %i, out of range (1, ..., %i)", axis, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = esp7000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	esp7000_error("Device not open");
    	return;
    }
    
	/* Check to make sure the axis is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis);
	if (axis_active != 1) return;
	
    /* Turn on the axis */
	Fmt(cmd, "%s<%iMO", axis);
    esp7000_out(dev_ID, cmd);
    
    /* Check for errors */
	esp7000_check_errors(dev_ID);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	esp7000_deactivate_axis
 *  This function turns off the power to the axis
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, system axis (1, 2, ...)
 *
 *
 *	Michael Levashov
 *	8/22/07
 */

void esp7000_deactivate_axis(int dev_ID, int axis)
{
	/* Declare variables */
	int dev_open;
	int axis_active;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    if (axis < 1 || axis > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis = %i, out of range (1, ..., %i)", axis, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = esp7000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	esp7000_error("Device not open");
    	return;
    }
    
	/* Check to make sure the axis is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis);
	if (axis_active != 1) return;
	
    /* Turn on the axis */
	Fmt(cmd, "%s<%iMF", axis);
    esp7000_out(dev_ID, cmd);
    
    /* Check for errors */
	esp7000_check_errors(dev_ID);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	esp7000_get_acc
 *  This function gets the acceleration of the specified axis.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, system axis (1, 2, ...)
 *
 *	Output:
 *	acc, stage acceleration (m/sec/sec)
 *
 *	Zachary Wolf
 *	5/16/05
 */

void esp7000_get_acc(int dev_ID, int axis, double* acc)
{
	/* Declare variables */
	int dev_open;
	int axis_active;
	int num_scan;
	double acc_mm_sec_sec;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    if (axis < 1 || axis > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis = %i, out of range (1, ..., %i)", axis, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	*acc = 0.;
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = esp7000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	esp7000_error("Device not open");
    	return;
    }
    
    /* Check to make sure the axis is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis);
	if (axis_active != 1)
	{
		*acc = 0;
		return;
	}
    
    /* Get the acceleration information */
	Fmt(cmd, "%s<%iAC?", axis);
    esp7000_out(dev_ID, cmd);
    esp7000_in(dev_ID, msg);
    
    /* Extract the acceleration */
    num_scan = Scan(msg, "%s>%f", &acc_mm_sec_sec);
    if (num_scan != 1) esp7000_error("Problem reading acceleration.");
    *acc = acc_mm_sec_sec/1000;
    
    /* Check for errors */
	esp7000_check_errors(dev_ID);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	esp7000_get_dcl
 *  This function gets the deceleration of the specified axis.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, system axis (1, 2, ...)
 *
 *	Output:
 *	dcl, stage deceleration (m/sec/sec)
 *
 *	Zachary Wolf
 *	5/16/05
 */

void esp7000_get_dcl(int dev_ID, int axis, double* dcl)
{
	/* Declare variables */
	int dev_open;
	int axis_active;
	int num_scan;
	double dcl_mm_sec_sec;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    if (axis < 1 || axis > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis = %i, out of range (1, ..., %i)", axis, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	*dcl = 0.;
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = esp7000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	esp7000_error("Device not open");
    	return;
    }
    
    /* Check to make sure the axis is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis);
	if (axis_active != 1)
	{
		*dcl = 0;
		return;
	}
    
    /* Get the deceleration information */
	Fmt(cmd, "%s<%iAG?", axis);
    esp7000_out(dev_ID, cmd);
    esp7000_in(dev_ID, msg);
    
    /* Extract the deceleration */
    num_scan = Scan(msg, "%s>%f", &dcl_mm_sec_sec);
    if (num_scan != 1) esp7000_error("Problem reading deceleration.");
    *dcl = dcl_mm_sec_sec/1000;
    
    /* Check for errors */
	esp7000_check_errors(dev_ID);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	esp7000_get_vel
 *  This function gets the velocity of the specified axis.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, system axis (1, 2, ...)
 *
 *	Output:
 *	vel, stage velocity (m/sec)
 *
 *	Zachary Wolf
 *	5/16/05
 */

void esp7000_get_vel(int dev_ID, int axis, double* vel)
{
	/* Declare variables */
	int dev_open;
	int axis_active;
	int num_scan;
	double vel_mm_sec;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    if (axis < 1 || axis > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis = %i, out of range (1, ..., %i)", axis, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	*vel = 0.;
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = esp7000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	esp7000_error("Device not open");
    	return;
    }
    
    /* Check to make sure the axis is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis);
	if (axis_active != 1)
	{
		*vel = 0;
		return;
	}
    
    /* Get the velocity information */
	Fmt(cmd, "%s<%iVA?", axis);
    esp7000_out(dev_ID, cmd);
    esp7000_in(dev_ID, msg);
    
    /* Extract the velocity */
    num_scan = Scan(msg, "%s>%f", &vel_mm_sec);
    if (num_scan != 1) esp7000_error("Problem reading velocity.");
    *vel = vel_mm_sec/1000;
    
    /* Check for errors */
	esp7000_check_errors(dev_ID);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	esp7000_get_pos
 *  This function gets the position of the specified axis.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, axis number (1, 2, ...)
 *
 *	Output:
 *	pos, stage position (m)
 *
 *	Zachary Wolf
 *	5/16/05
 */

void esp7000_get_pos(int dev_ID, int axis, double* pos)
{
	/* Declare variables */
	int dev_open;
	int axis_active;
	int num_scan;
	double pos_mm;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    if (axis < 1 || axis > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis = %i, out of range (1, ..., %i)", axis, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	*pos = 0.;
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = esp7000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	esp7000_error("Device not open");
    	return;
    }
    
    /* Check to make sure the axis is active */
	axis_active = esp7000_check_axis_active(dev_ID, axis);
	if (axis_active != 1)
	{
		*pos = 0.;
		return;
	}
    
    /* Get the position information */
	Fmt(cmd, "%s<%iTP", axis);
    esp7000_out(dev_ID, cmd);
    esp7000_in(dev_ID, msg);
    
    /* Extract the position */
    num_scan = Scan(msg, "%s>%f", &pos_mm);
    if (num_scan != 1) esp7000_error("Problem reading position.");
    *pos = pos_mm/1000;
    
    /* Check for errors */
	esp7000_check_errors(dev_ID);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	esp7000_exit
 *  This function prepares the ESP7000 for shutdown and closes
 *	the device.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Zachary Wolf
 *	8/3/99
 */

void esp7000_exit(int dev_ID)
{
	/* Declare variables */
	int err;
	int dev_open;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = esp7000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	return;		 /* OK, already closed */
    }
    
    /* Leave the ESP7000 in the desired state */
    /* Nothing needs to be done */

	/* Close the device */
    esp7000_close_dev(dev_ID);
    
    /* Done */
    return;
}

/*	**************************************************************  */

/*	PRIVATE FUNCTIONS  */

/*	**************************************************************  */
/*
 *	esp7000_open_dev
 *	This function opens the device and returns an identifier for
 *	future references to the device.
 *
 *	Input:
 *	gpib_board_addr, address of the GPIB board the device is connected to
 *	gpib_dev_addr, GPIB address of the device (1 to 30, 0 is reserved)
 *
 *	Output:
 *	dev_ID, identifier for future references to the	device (-1 if there is a problem)
 *
 *	Zachary Wolf
 *	8/23/98
 */

int esp7000_open_dev(int gpib_board_addr, int gpib_dev_addr)
{
	/* Declare variables */
    int i;
    int dev_ID;
    int descr;
    int err;

    /* Check input parameters */
    if (gpib_board_addr < 0 || gpib_board_addr > 10)
    {
    	Fmt(msg, "%s<gpib_board_addr = %i, out of range", gpib_board_addr);
    	esp7000_error(cmd);
    	return -1;
    }
    if (gpib_dev_addr < 1 || gpib_dev_addr > 30)
    {
    	Fmt(msg, "%s<gpib_dev_addr = %i, out of range", gpib_dev_addr);
    	esp7000_error(msg);
    	return -1;
    }
    
    /* Initialization */
    dev_ID = 0;

    /* Check to see if the device is already in the device table */
    for (i = 1; i <= ESP7000_MAX_NUM_DEV; i++)
    {
        if (dev_addr[i] == gpib_dev_addr)
        {
            dev_ID = i;
            return dev_ID;
        }
    }
    
    /* Find an ID for the device */
	for (i = 1; i <= ESP7000_MAX_NUM_DEV; i++)
	{
		if (dev_addr[i] == 0)
		{
			dev_ID = i;
			break;
		}
	}
     
    /* If an entry could not be opened in the device table, return an error */
    if (dev_ID <= 0)
    {
    	esp7000_error("The device could not be opened");
        return  -1;
    }
    
	/* Open the device */
    /* Terminate write without EOI, CR is added in the send routine */
    /* Terminate read on LF, remove CR LF in read routine */
	descr = ibdev(gpib_board_addr, gpib_dev_addr, 0, T10s, 0, 0x140A);
	if (descr <= 0)
	{
		esp7000_error("The device could not be opened");
		return  -1;
	}
    
    /* Open successful, store results in the device table */
    dev_addr[dev_ID] = gpib_dev_addr;
    dev_descr[dev_ID] = descr;
    dev_count++;
    
    /* Clear the device */
    //err = ibclr(dev_descr[dev_ID]);
    //Delay(.5);        /* Wait for the ESP7000 to clear */
    //if (err & 0x8000)
    //{
    //	esp7000_error("Problem clearing device");
    //}
    
    /* Return the device ID */
    return dev_ID;
}

/*	**************************************************************  */
/*
 *	esp7000_close_dev
 *  This function closes the device by removing it from the GPIB
 *	device table.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Zachary Wolf
 *	8/3/98
 */

void esp7000_close_dev(int dev_ID)
{
	/* Close the device */
    if (dev_descr[dev_ID] != 0)
    {
        ibonl(dev_descr[dev_ID], 0);
        dev_descr[dev_ID] = 0;
        dev_addr[dev_ID] = 0;
        dev_count--;
    }
    
    /* Done */
    return;
}


/*	**************************************************************  */
/*
 *	esp7000_out
 *  This function writes a buffer of data to the device.
 *
 *	Input:
 *	dev_ID, device identifier
 *	buf, null terminated string, the contents up to \0 are sent
 *
 *	Output:
 *	err, 0 if ok, -1 otherwise
 *
 *	Zachary Wolf
 *	8/23/98
 */

int esp7000_out(int dev_ID, char* buf)
{
	/* Declare variables */
	char out_buf[ESP7000_MAX_CMD];
	int nbytes;
	int err;
	
	/* Add CR to the command */
	Fmt(out_buf, "%s<%s%c", buf, 13);
	
	/* Send the command */
	nbytes = StringLength(out_buf);
	err = ibwrt(dev_descr[dev_ID], out_buf, (long)nbytes);
	
	/* Check for errors */
	if (err & 0x8000)
	{
		Fmt(msg, "%s<ibsta = %x, problem sending GPIB command", err);
		esp7000_error(msg);
		return -1;
	}
	delay(0.1);
	
	/* Done */
    return 0;
}

/*	**************************************************************  */
/*
 *	esp7000_in
 *  This function reads a buffer of data from the device.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Output:
 *	buf, null terminated string, the contents from the device
 *	err, 0 if ok, -1 otherwise
 *
 *	Zachary Wolf
 *	8/23/98
 */

int esp7000_in(int dev_ID, char* buf)
{
	/* Declare variables */
	int err;
	char in_buf[ESP7000_MAX_CMD];
	int spoll;
	
	delay(.1); 
	/* Read the message */
	err = ibrd(dev_descr[dev_ID], in_buf, ESP7000_MAX_CMD);
	
	/* Check for errors */
	if (err & 0x8000)
	{
		Fmt(msg, "%s<ibsta = %x, problem receiving GPIB input", err);
		esp7000_error(msg);
		return -1;
	}
	
	/* Remove the CR LF terminator (stop on CR) */
	Scan(in_buf, "%s[t13]>%s[t-]", buf);
	
	/* Done */
    return 0;
}
/*	**************************************************************  */
/*
 *	esp7000_check_dev_open
 *  This function checks to see if the specified device is open.
 *	If the device has been opened, a 1 is returned, 0 otherwise.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Output:
 *	status, 1 if device is open, 0 otherwise
 *
 *	Zachary Wolf
 *	7/27/98
 */

int esp7000_check_dev_open(int dev_ID)
{
	/* See if the device descriptor has a positive value */
    if (dev_descr[dev_ID] > 0)
    {
        return 1;		/* Open */
    }
    else
    {
	    return 0;		/* Not open */
	}
}

/*	**************************************************************  */
/*
 *	esp7000_check_axis_active
 *  This function checks whether the specified axis is active,
 *	ie. whether a positioner is connected.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, axis number
 *
 *	Output:
 *	active, 1 if active, 0 otherwise
 *
 *	Zachary Wolf
 *	5/16/05
 */

int esp7000_check_axis_active(int dev_ID, int axis)
{
	/* Declare variables */
	char system_config[ESP7000_MAX_CMD];
	long int config_val;
	int num_scan;
	int axis_bit_num;
	int axis_bit_val;
	int status;

	/* Get the system configuration */
    esp7000_out(dev_ID, "ZU");	 
    esp7000_in(dev_ID, system_config);
    num_scan = Scan(system_config, "%s[t72]>%i[r16]", &config_val);
    if (num_scan != 1) esp7000_error("Problem reading system configuration.");
    
    /* Determine the axis bit number */
    /* See manual p. 3-150 */
    /* Range of axis assumed to have been previously checked */
    axis_bit_num = 7 + axis;
    
    /* Determine the axis bit value */
    axis_bit_val = 1 << (axis_bit_num);  /* Bit number starts at 0 */
    
    /* See if the axis is active */
    if ((config_val & axis_bit_val) == axis_bit_val)
    	status = 1;
    else
    	status = 0;
    	
    /* Debug */
    //printf("\nesp7000_check_axis_active\n");
    //printf("system_config = %s\n", system_config);
    //printf("config_val = %i\n", config_val);
    //printf("axis = %i\n", axis);
    //printf("axis_bit_num = %i\n", axis_bit_num);
    //printf("axis_bit_val = %i\n", axis_bit_val);
    //printf("status = %i\n", status);
    	
    /* Done */
    return status;
}

/*	**************************************************************  */
/*
 *	esp7000_check_errors
 *  This function checks for device errors.
 *	The operator is alerted if there is an error.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Output:
 *	err, 0 if no errors, -1 otherwise
 *
 *	Zachary Wolf
 *	5/16/05
 */

int esp7000_check_errors(int dev_ID)
{
	/* Declare variables */
	char err_reg[ESP7000_MAX_CMD];
	int err_code;
	long int time_stamp;
	char err_msg[ESP7000_MAX_CMD];
	int num_scan;
	int err;
	
	/* Initialize */
	err = 0;
	err_code = -1;

	/* Read the error messages */
	while (err_code != 0)
	{
    	esp7000_out(dev_ID, "TB?");	 
    	esp7000_in(dev_ID, err_reg);
    	num_scan = Scan(err_reg, "%s>%i,%i,%s", &err_code, &time_stamp, err_msg);
    	if (num_scan != 3) esp7000_error("Problem reading errors.");
    	if (err_code != 0)
    	{
    		printf("ESP7000 Error: %s\n", err_reg);
    		err = -1;
    	}
    }
    
    /* See if the operator wants to continue if there are errors */
    if (err != 0) esp7000_error("esp7000_check_errors found errors");
    	
    /* Done */
    return err;
}

/*	**************************************************************  */
/*
 *	esp7000_hold_until_move_complete
 *  This function loops until the specified axis has stopped moving.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, axis to check (1, 2, ...)
 *
 *	Zachary Wolf
 *	5/16/05
 */

void esp7000_hold_until_move_complete(int dev_ID, int axis)
{
	/* Declare variables */
	char status_buf[ESP7000_MAX_CMD];
	int motion_done_status;
    enum  {MOTION_FALSE, MOTION_TRUE} motion_done;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > ESP7000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	esp7000_error(msg);
    	return;
    }
    if (axis < 1 || axis > ESP7000_MAX_NUM_AXES)
    {
    	Fmt(msg, "%s<axis = %i, out of range (1, ..., %i)", axis, ESP7000_MAX_NUM_AXES);
    	esp7000_error(msg);
    }
    
    /* Loop until the motion is done */
    motion_done = MOTION_FALSE;
    while (motion_done == MOTION_FALSE)
    {
		Fmt(cmd, "%s<%iMD?", axis);
    	esp7000_out(dev_ID, cmd);
    	esp7000_in(dev_ID, status_buf);
    	Scan(status_buf, "%s>%i", &motion_done_status);
    	//printf("motion done status: %i\n", motion_done_status);
    	if (motion_done_status == 1)
    	{
    		motion_done = MOTION_TRUE;			/* Move complete */
    	}
    	else
    	{
    		motion_done = MOTION_FALSE;		/* Still moving */
    		Delay(0.1);
    	}
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	esp7000_message
 *  This function handles messages about the ESP7000.
 *
 *	Input:
 *	message, string to display in standard I/O
 *
 *	Zachary Wolf
 *	8/3/98
 */

void esp7000_message(char* message)
{
	/* Print the message */
	printf("%s\n", message);
	
	/* Done */
	return;
}

/*	**************************************************************  */
/*
 *	esp7000_error
 *  This function handles error messages for the ESP7000.
 *
 *	Input:
 *	message, string to display in standard I/O
 *
 *	Zachary Wolf
 *	8/3/98
 */

void esp7000_error(char* message)
{
	/* Declare variables */
	char buf[80];
	
	/* Notify the operator of the error */
	printf("\nESP7000 ERROR: %s\n", message);
	Beep();
	Delay(.5);
	Beep();
	
	/* Terminate the program if the operator desires */
	printf("Press ENTER to continue.\nPress any key then ENTER to terminate program.\n");
	fgets(buf, 80, stdin);
	if (buf[0] == '\n') return;
	else exit(0);
}