/*	**************************************************************  */
/*
 *	Module CM4000
 *	This module contains I/O functions for the Compumotor model
 *	4000 indexer.                 
 *
 *	Zachary Wolf
 *	9/4/02
 */ 

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

/*	INCLUDES  */
#include <utility.h>
#include <gpib.h>
#include <formatio.h>
#include <ansi_c.h>
#include "cm4000.h"

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

/*  PRIVATE FUNCTIONS  */
int cm4000_open_dev(int gpib_board_addr, int gpib_dev_addr);
void cm4000_close_dev(int dev_ID);
int cm4000_out(int dev_ID, char *buf);
int cm4000_in(int dev_ID, char* buf);
int cm4000_check_dev_open(int dev_ID);
int cm4000_read_error_status(int dev_ID);
void cm4000_hold_until_ready_to_reply(int dev_ID);
int cm4000_serial_poll(int dev_ID, int* spoll);
void cm4000_get_num_mot_steps_per_rev(int axis, long int* num_steps_per_rev);
void cm4000_get_num_enc_steps_per_rev(int axis, long int* num_steps_per_rev);
void cm4000_get_motor_resolution_code(int axis, int* mr);
void cm4000_message(char* msg);
void cm4000_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[CM4000_MAX_NUM_DEV + 1];
static int dev_descr[CM4000_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[CM4000_MAX_CMD + 1];
static char msg[CM4000_MAX_CMD + 1];

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

/*	PUBLIC FUNCTIONS  */

/*	**************************************************************  */
/*
 *  cm4000_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
 *	9/4/02
 */

void cm4000_init(int gpib_board_addr, int gpib_dev_addr, int* ID)
{
    /* Declare variables */
    int dev_ID;
    int err;
    int mr;
    int axis;
    
    /* Message */
    cm4000_message("");
    cm4000_message("Initializing the CM4000...");

    /* 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);
    	cm4000_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);
    	cm4000_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	*ID = 1;
    	return;
    #endif
    
    /* Open the CM4000 */
    dev_ID = cm4000_open_dev(gpib_board_addr, gpib_dev_addr);
    if (dev_ID <= 0)
    {
        cm4000_error("Device was not opened");
        return;
    }
    
    /* Enable the communications interface */
    cm4000_out(dev_ID, "/");
    
    /* Set the motor resolutions */
    for (axis = 1; axis <= 4; axis++)
    {
		cm4000_get_motor_resolution_code(axis, &mr);
    	Fmt(cmd, "%s<%i[w1]MR%i[w2p0]", axis, mr);
    	cm4000_out(dev_ID, cmd);
    }
    
    /* Energize the drives */
    for (axis = 1; axis <= 4; axis++)
    {
    	Fmt(cmd, "%s<%i[w1]ST0", axis);
    	cm4000_out(dev_ID, cmd);
    }
    
    /* Set the present position to zero */
    for (axis = 1; axis <= 4; axis++)
    {
    	Fmt(cmd, "%s<%i[w1]PZ", axis);
    	cm4000_out(dev_ID, cmd);
    }
    
    /* Get a response from the CM4000 */
    for (axis = 1; axis <= 4; axis++)
    {
    	Fmt(cmd, "%s<%i[w1]R", axis);
    	cm4000_out(dev_ID, cmd);
    	cm4000_hold_until_ready_to_reply(dev_ID);
    	err = cm4000_in(dev_ID, cmd);
    	if (err != 0)
    	{
    	    cm4000_error("Problem reading dev_ID");
    	    cm4000_close_dev(dev_ID);
    	    return;
    	}
    	printf("%s  ", cmd);
    }
    printf("\n");

	/* Return the ID number */
    *ID = dev_ID;
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm4000_rel_move
 *  This function moves the motor the specified number of revolutions
 *	with the specified acceleration and velocity.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, number of the axis to move (1 to 4)
 *	acc, acceleration (rev/sec/sec)
 *	vel, velocity (rev/sec)
 *	dis, distance (rev), can be + or -
 *
 *	Zachary Wolf
 *	9/4/02
 */

void cm4000_rel_move(int dev_ID, int axis, double acc, double vel, double dis)
{
	/* Declare variables */
	long int num_steps_per_rev;
	int dev_open;
	long int dis_steps;
	long int steps_taken;
	int reply_axis;
	
	/* Convert distance in revolutions to distance in steps */
	cm4000_get_num_mot_steps_per_rev(axis, &num_steps_per_rev);
	dis_steps = dis * num_steps_per_rev;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM4000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm4000_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm4000_error(msg);
    	return;
    }
    if (acc < 0.001 || acc > 999.999)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	cm4000_error(msg);
    	if (acc < 0.001) acc = 0.001; else acc = 999.999;
    }
    if (vel < 0.1 || vel > 99.)
    {
    	Fmt(msg, "%s<vel = %f, out of range, will be adjusted", vel);
    	cm4000_error(msg);
    	if (vel < 0.1) vel = 0.1; else vel = 99.;
    }
    if (labs(dis_steps) > 99999999)
    {
    	Fmt(msg, "%s<dis_steps = %i, out of range", dis_steps);
    	cm4000_error(msg);
    	return;
    }
    if (labs(dis_steps) == 0) return;
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm4000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm4000_error("Device not open");
    	return;
    }
    
    /* Set normal mode to move to a given position */
    Fmt(cmd, "%s<%i[w1]MN", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Set incremental mode */
    Fmt(cmd, "%s<%i[w1]FSA0", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Set the acceleration */
    Fmt(cmd, "%s<%i[w1]A%f[p2]", axis, acc);
    cm4000_out(dev_ID, cmd);
    
    /* Set the velocity */
    Fmt(cmd, "%s<%i[w1]V%f[p3]", axis, vel);
    cm4000_out(dev_ID, cmd);
    
    /* Set the distance */
    Fmt(cmd, "%s<%i[w1]D%i", axis, dis_steps);
    cm4000_out(dev_ID, cmd);
    
    /* Move */
    Fmt(cmd, "%s<%i[w1]G", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Wait until the move is complete */
    /* (Also, the following P command will not execute until the move is complete) */
    Delay(vel / acc + dis / vel + vel / acc);
    
    /* Ask how many steps were taken */
    Fmt(cmd, "%s<%i[w1]P", axis);
    cm4000_out(dev_ID, cmd);
    cm4000_hold_until_ready_to_reply(dev_ID); 
    cm4000_in(dev_ID, cmd);
    Scan(cmd, "%s>%i:%i", &reply_axis, &steps_taken);
    
    /* Debug */
    //printf("cmd = %s\n", cmd);
    //printf("steps_requested = %i, steps_taken = %i\n", dis_steps, steps_taken);
    
    /* Make sure the proper number of steps were taken */
    if (reply_axis != axis)
    {
    	cm4000_error("The wrong axis number was returned");
    	return;
    }
    if (steps_taken != dis_steps)
    {
    	cm4000_error("The wrong number of steps were taken");
    	return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm4000_start_rel_move
 *  This function starts a move of the motor.  It moves the
 *	specified number of revolutions with the specified acceleration
 *	and velocity.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, axis number to move (1 to 4)
 *	acc, acceleration (rev/sec/sec)
 *	vel, velocity (rev/sec)
 *	dis, distance (rev), can be + or -
 *
 *	Zachary Wolf
 *	9/9/02
 */

void cm4000_start_rel_move(int dev_ID, int axis, double acc, double vel, double dis)
{
	/* Declare variables */
	long int num_steps_per_rev;
	int dev_open;
	long int dis_steps;
	
	/* Convert distance in revolutions to distance in steps */
	cm4000_get_num_mot_steps_per_rev(axis, &num_steps_per_rev);
	dis_steps = dis * num_steps_per_rev;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM4000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm4000_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm4000_error(msg);
    	return;
    }
    if (acc < 0.001 || acc > 999.999)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	cm4000_error(msg);
    	if (acc < 0.001) acc = 0.001; else acc = 999.999;
    }
    if (vel < 0.1 || vel > 99.)
    {
    	Fmt(msg, "%s<vel = %f, out of range, will be adjusted", vel);
    	cm4000_error(msg);
    	if (vel < 0.1) vel = 0.1; else vel = 99.;
    }
    if (labs(dis_steps) > 99999999)
    {
    	Fmt(msg, "%s<dis_steps = %i, out of range", dis_steps);
    	cm4000_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm4000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm4000_error("Device not open");
    	return;
    }
    
    /* Set normal mode to move to a given position */
    Fmt(cmd, "%s<%i[w1]MN", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Set incremental mode */
    Fmt(cmd, "%s<%i[w1]FSA0", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Set the acceleration */
    Fmt(cmd, "%s<%i[w1]A%f[p2]", axis, acc);
    cm4000_out(dev_ID, cmd);
    
    /* Set the velocity */
    Fmt(cmd, "%s<%i[w1]V%f[p3]", axis, vel);
    cm4000_out(dev_ID, cmd);
    
    /* Set the distance */
    Fmt(cmd, "%s<%i[w1]D%i", axis, dis_steps);
    cm4000_out(dev_ID, cmd);
    
    /* Move */
    Fmt(cmd, "%s<%i[w1]G", axis);
    cm4000_out(dev_ID, cmd);	/* Do not hold while moving */
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm4000_end_rel_move
 *  This function checks that a move started by cm4000_start_rel_move
 *	has finished and performed properly.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, axis number (1 to 4)
 *	dis, distance (rev), can be + or -
 *
 *	Zachary Wolf
 *	9/9/02
 */

void cm4000_end_rel_move(int dev_ID, int axis, double dis)
{
	/* Declare variables */
	long int num_steps_per_rev;
	int dev_open;
	long int dis_steps;
	long int steps_taken;
	int reply_axis;
	
	/* Convert distance in revolutions to distance in steps */
	cm4000_get_num_mot_steps_per_rev(axis, &num_steps_per_rev);
	dis_steps = dis * num_steps_per_rev;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM4000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm4000_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm4000_error(msg);
    	return;
    }
    if (labs(dis_steps) > 99999999)
    {
    	Fmt(msg, "%s<dis_steps = %i, out of range", dis_steps);
    	cm4000_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm4000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm4000_error("Device not open");
    	return;
    }
   
    /* Ask how many steps were taken */
    /* The P command will not execute until the move is complete) */
    Fmt(cmd, "%s<%i[w1]P", axis);
    cm4000_out(dev_ID, cmd);
    cm4000_hold_until_ready_to_reply(dev_ID); 
    cm4000_in(dev_ID, cmd);
    Scan(cmd, "%s>%i:%i", &reply_axis, &steps_taken);
    
    /* Debug */
    //printf("cmd = %s\n", cmd);
    //printf("steps_requested = %i, steps_taken = %i\n", dis_steps, steps_taken);
    
    /* Make sure the proper number of steps were taken */
    if (reply_axis != axis)
    {
    	cm4000_error("The wrong axis number was returned");
    	return;
    }
    if (steps_taken != dis_steps)
    {
    	cm4000_error("The wrong number of steps were taken");
    	return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm4000_abs_move
 *  This function moves the motor to the specified location
 *	with the specified acceleration and velocity.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, number of the axis to move (1 to 4)
 *	acc, acceleration (rev/sec/sec)
 *	vel, velocity (rev/sec)
 *	pos, position (rev), can be + or -
 *
 *	Zachary Wolf
 *	9/4/02
 */

void cm4000_abs_move(int dev_ID, int axis, double acc, double vel, double pos)
{
	/* Declare variables */
	long int num_steps_per_rev;
	int dev_open;
	long int pos_steps;
	long int fin_pos;
	int reply_axis;
	
	/* Convert position in revolutions to position in steps */
	cm4000_get_num_mot_steps_per_rev(axis, &num_steps_per_rev);
	pos_steps = pos * num_steps_per_rev;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM4000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm4000_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm4000_error(msg);
    	return;
    }
    if (acc < 0.001 || acc > 999.999)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	cm4000_error(msg);
    	if (acc < 0.001) acc = 0.001; else acc = 999.999;
    }
    if (vel < 0.1 || vel > 99.)
    {
    	Fmt(msg, "%s<vel = %f, out of range, will be adjusted", vel);
    	cm4000_error(msg);
    	if (vel < 0.1) vel = 0.1; else vel = 99.;
    }
    if (labs(pos_steps) > 99999999)
    {
    	Fmt(msg, "%s<pos_steps = %i, out of range", pos_steps);
    	cm4000_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm4000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm4000_error("Device not open");
    	return;
    }
    
    /* Set normal mode to move to a given position */
    Fmt(cmd, "%s<%i[w1]MN", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Set absolute mode */
    Fmt(cmd, "%s<%i[w1]FSA1", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Set the acceleration */
    Fmt(cmd, "%s<%i[w1]A%f[p2]", axis, acc);
    cm4000_out(dev_ID, cmd);
    
    /* Set the velocity */
    Fmt(cmd, "%s<%i[w1]V%f[p3]", axis, vel);
    cm4000_out(dev_ID, cmd);
    
    /* Set the position */
    Fmt(cmd, "%s<%i[w1]D%i", axis, pos_steps);
    cm4000_out(dev_ID, cmd);
    
    /* Move */
    Fmt(cmd, "%s<%i[w1]G", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Wait until the move is complete */
    /* The following P command will not execute until the move is complete */
    
    /* Ask for the final position */
    Fmt(cmd, "%s<%i[w1]PR", axis);
    cm4000_out(dev_ID, cmd);
    cm4000_hold_until_ready_to_reply(dev_ID); 
    cm4000_in(dev_ID, cmd);
    Scan(cmd, "%s>%i:%i", &reply_axis, &fin_pos);
    
    /* Debug */
    //printf("cmd = %s\n", cmd);
    //printf("position_requested = %i, final_position = %i\n", pos_steps, fin_pos);
    
    /* Make sure the proper position was achieved */
    if (reply_axis != axis)
    {
    	cm4000_error("The wrong axis number was returned");
    	return;
    }
    if (fin_pos != pos_steps)
    {
    	cm4000_error("The wrong number of steps were taken");
    	return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm4000_go_home
 *  This function moves the motor home position.  The velocity
 *	and acceleration are specified.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, number of the axis to move (1 to 4)
 *	acc, acceleration (rev/sec/sec)
 *	vel, velocity (rev/sec)
 *
 *	Zachary Wolf
 *	9/4/02
 */

void cm4000_go_home(int dev_ID, int axis, double acc, double vel)
{
	/* Declare variables */
	int dev_open;
	int reply_axis;
	long int fin_pos;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM4000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm4000_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm4000_error(msg);
    	return;
    }
    if (acc < 0.001 || acc > 999.999)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	cm4000_error(msg);
    	if (acc < 0.001) acc = 0.001; else acc = 999.999;
    }
    if (fabs(vel) < 0.001 || fabs(vel) > 20.)
    {
    	Fmt(msg, "%s<vel = %f, out of range, will be adjusted", vel);
    	cm4000_error(msg);
    	if (fabs(vel) < 0.001) vel = 0.001; else vel = 20. * vel / fabs(vel);
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm4000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm4000_error("Device not open");
    	return;
    }
    
    /* Set normal mode to move to a given position */
    Fmt(cmd, "%s<%i[w1]MN", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Use only the home switch to define the home position */
    Fmt(cmd, "%s<%i[w1]OSD0", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Define the active state of the home switch as a low signal */
    Fmt(cmd, "%s<%i[w1]OSC0", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Use the CCW side of the home switch */
    Fmt(cmd, "%s<%i[w1]OSH1", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Set the final homing direction to CCW */
    Fmt(cmd, "%s<%i[w1]OSG1", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Back up to the home switch */
    Fmt(cmd, "%s<%i[w1]OSB1", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Set the acceleration */
    Fmt(cmd, "%s<%i[w1]GA%f[p2]", axis, acc);
    cm4000_out(dev_ID, cmd);
    
    /* Move with the specified velocity */
    Fmt(cmd, "%s<%i[w1]GH%f[p3]", axis, vel);
    cm4000_out(dev_ID, cmd);
    
    /* Wait until the move is complete */
    /* The following P command will not execute until the move is complete */
    
    /* Ask for the position */
    Fmt(cmd, "%s<%i[w1]PR", axis);
    cm4000_out(dev_ID, cmd);
    cm4000_hold_until_ready_to_reply(dev_ID); 
    cm4000_in(dev_ID, cmd);
    Scan(cmd, "%s>%i:%i", &reply_axis, &fin_pos);
    
    /* Debug */
    //printf("cmd = %s\n", cmd);
    //printf("position_requested = %i, final_position = %i\n", pos_steps, fin_pos);
    
    /* Check */
    if (reply_axis != axis)
    {
    	cm4000_error("The wrong axis number was returned");
    	return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm4000_get_mot_position
 *  This function gets the number of revolutions from zero that
 *	the motor is currently at.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, axis number (1 to 4)
 *
 *	Output:
 *	pos, position (rev), can be + or -
 *
 *	Zachary Wolf
 *	9/9/02
 */

void cm4000_get_mot_position(int dev_ID, int axis, double* pos)
{
	/* Declare variables */
	int dev_open;
	long int pos_steps;
	long int num_steps_per_rev;
	int reply_axis;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM4000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm4000_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm4000_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm4000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm4000_error("Device not open");
    	return;
    }
   
    /* Ask for the current position */
    Fmt(cmd, "%s<%i[w1]PR", axis);
    cm4000_out(dev_ID, cmd);
    cm4000_hold_until_ready_to_reply(dev_ID); 
    cm4000_in(dev_ID, cmd);
    Scan(cmd, "%s>%i:%i", &reply_axis, &pos_steps);
    
    /* Check */
    if (reply_axis != axis)
    {
    	cm4000_error("The wrong axis number was returned");
    	return;
    }
	
	/* Convert the position in steps to position in revolutions */
	cm4000_get_num_mot_steps_per_rev(axis, &num_steps_per_rev);
	*pos = (double) pos_steps / (double) num_steps_per_rev;
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm4000_get_enc_position
 *  This function gets the number of revolutions from zero that
 *	the encoder is currently at.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, axis number (1 to 4)
 *
 *	Output:
 *	pos, position (rev), can be + or -
 *
 *	Zachary Wolf
 *	9/9/02
 */

void cm4000_get_enc_position(int dev_ID, int axis, double* pos)
{
	/* Declare variables */
	int dev_open;
	long int pos_steps;
	long int num_steps_per_rev;
	int reply_axis;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM4000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm4000_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm4000_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm4000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm4000_error("Device not open");
    	return;
    }
   
    /* Ask for the current position */
    Fmt(cmd, "%s<%i[w1]PX", axis);
    cm4000_out(dev_ID, cmd);
    cm4000_hold_until_ready_to_reply(dev_ID); 
    cm4000_in(dev_ID, cmd);
    Scan(cmd, "%s>%i:%i", &reply_axis, &pos_steps);
    
    /* Check */
    if (reply_axis != axis)
    {
    	cm4000_error("The wrong axis number was returned");
    	return;
    }
	
	/* Convert the position in steps to position in revolutions */
	/* Four encoder transitions make one step */
	cm4000_get_num_enc_steps_per_rev(axis, &num_steps_per_rev);
	*pos = (double) pos_steps / (double)4 / (double) num_steps_per_rev;
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm4000_zero
 *  This function sets the indexer position counter to zero.
 *
 *	Input:
 *	dev_ID, device identifier
 *	axis, axis number (1 to 4)
 *
 *	Zachary Wolf
 *	8/25/98
 */

void cm4000_zero(int dev_ID, int axis)
{
	/* Declare variables */
	int err;
	int dev_open;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM4000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm4000_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm4000_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm4000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm4000_error("Device not open");
    	return;
    }

    /* Zero the position counter */
    Fmt(cmd, "%s<%i[w1]PZ", axis);
    cm4000_out(dev_ID, cmd);
    
    /* Check for errors */							
    err = cm4000_read_error_status(dev_ID);
    if (err != 0)
    {
        cm4000_error("The device has an error");
        cm4000_close_dev(dev_ID);
        return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm4000_exit
 *  This function prepares the CM4000 for shutdown and closes
 *	the device.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Zachary Wolf
 *	8/23/98
 */

void cm4000_exit(int dev_ID)
{
	/* Declare variables */
	int dev_open;
	int axis;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM4000_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm4000_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm4000_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	return;		 /* OK, already closed */
    }
    
    /* Leave the CM4000 in the desired state */
    
    /* De-energize the drives */
    for (axis = 1; axis <= 4; axis++)
    {
    	Fmt(cmd, "%s<%i[w1]ST1", axis);
    	cm4000_out(dev_ID, cmd);
    }

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

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

/*	PRIVATE FUNCTIONS  */

/*	**************************************************************  */
/*
 *	cm4000_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 cm4000_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);
    	cm4000_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);
    	cm4000_error(msg);
    	return -1;
    }
    
    /* Initialization */
    dev_ID = 0;

    /* Check to see if the device is already in the device table */
    for (i = 1; i <= CM4000_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 <= CM4000_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)
    {
    	cm4000_error("The device could not be opened");
        return  -1;
    }
    
	/* Open the device */
    /* Terminate write with EOI, CR is added in the send routine */
    /* Terminate read on CR, remove CR in read routine */
	descr = ibdev(gpib_board_addr, gpib_dev_addr, 0, T10s, 1, 0x140D);
	if (descr <= 0)
	{
		cm4000_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 CM4000 to clear */
    if (err & 0x8000)
    {
    	cm4000_error("Problem clearing device");
    }
    
    /* Return the device ID */
    return dev_ID;
}

/*	**************************************************************  */
/*
 *	cm4000_close_dev
 *  This function closes the device by removing it from the GPIB
 *	device table.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Zachary Wolf
 *	9/9/02
 */

void cm4000_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;
}


/*	**************************************************************  */
/*
 *	cm4000_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 cm4000_out(int dev_ID, char* buf)
{
	/* Declare variables */
	char out_buf[CM4000_MAX_CMD];
	int nbytes;
	int err;
	
	/* Add a carriage return 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);
		cm4000_error(msg);
		return -1;
	}
	
	/* Done */
    return 0;
}

/*	**************************************************************  */
/*
 *	cm4000_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 cm4000_in(int dev_ID, char* buf)
{
	/* Declare variables */
	int err;
	char in_buf[CM4000_MAX_CMD];
	
	/* Read the message */
	err = ibrd(dev_descr[dev_ID], in_buf, CM4000_MAX_CMD);
	
	/* Check for errors */
	if (err & 0x8000)
	{
		Fmt(msg, "%s<ibsta = %x, problem receiving GPIB input", err);
		cm4000_error(msg);
		return -1;
	}
	
	/* Remove the CR terminator */
	Scan(in_buf, "%s[t13]>%s[t-]", buf);
	
	/* Done */
    return 0;
}

/*	**************************************************************  */
/*
 *	cm4000_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 cm4000_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 */
	}
}

/*	**************************************************************  */
/*
 *	cm4000_read_error_status
 *  This function obtains 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
 *	9/9/02
 */

int cm4000_read_error_status(int dev_ID) 
{
	/* Declare variables */
	int axis;
	int reply_axis;
	char status[CM4000_MAX_CMD];
	
    /* Request the controller status */
    for (axis = 1; axis <= 4; axis++)
    {
    	Fmt(cmd, "%s<%i[w1]R", axis);
    	cm4000_out(dev_ID, cmd);
    	cm4000_hold_until_ready_to_reply(dev_ID);
    	cm4000_in(dev_ID, cmd);
	    Scan(cmd, "%s>%i:%s", &reply_axis, status);
    	if (strcmp(status, "*S") == 0 || strcmp(status, "*C") == 0)
    	{
    		cm4000_error("Controller error");
    		return -1;
    	}
    }
    
    /* Done */
    return 0;
}

/*	**************************************************************  */
/*
 *	cm4000_serial_poll
 *  This function performs a serial poll of the device.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Output:
 *	spoll, contents of the serial poll register
 *	err, 0 if ok, -1 otherwise
 *
 *	Zachary Wolf
 *	9/9/02
 */

int cm4000_serial_poll(int dev_ID, int* spoll)
{
	/* Declare variables */
	int err;
	char spoll_char;
	
	/* Perform the serial poll */
	err = ibrsp(dev_descr[dev_ID], &spoll_char);
	
	/* Check for errors */
	if (err & 0x8000)
	{
		Fmt(msg, "%s<ibsta = %x, problem performing GPIB serial poll", err);
		cm4000_error(msg);
		return -1;
	}
	
	/* Return the result as an integer */
	Scan(&spoll_char, "%c>%i", spoll);

	/* Debug */
	//printf("%i\n", *spoll);
	
	/* Done */
    return 0;
}

/*	**************************************************************  */
/*
 *	cm4000_hold_until_ready_to_reply
 *  This function serial polls the device in a loop until the
 *	ready to reply bit is set.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Zachary Wolf
 *	9/9/02
 */

void cm4000_hold_until_ready_to_reply(int dev_ID)
{
	/* Declare variables */
    int spoll;
    enum {TRUE, FALSE} busy;
    
    /* Loop while busy */
    busy = TRUE;
    while (busy == TRUE)
    {
    	cm4000_serial_poll(dev_ID, &spoll);
    	if ((spoll & 32) == 32)    
    	{
    		busy = FALSE;		   /* Ready to reply */
    	}
    	else
    	{
    		busy = TRUE;           /* Not ready to reply */
    		Delay(.1);
    	}
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm4000_get_num_mot_steps_per_rev
 *  This function gets the motor's number of steps per revolution.
 *
 *	Input:
 *	axis, axis number (1 to 4)
 *
 *	Output:
 *	num_steps_per_rev, number of steps per revolution
 *
 *	Zachary Wolf
 *	9/9/02
 */

void cm4000_get_num_mot_steps_per_rev(int axis, long int* num_steps_per_rev)
{
	/* Determine the number of steps per revolutuon */
	if      (axis == 1) *num_steps_per_rev = CM4000_NUM_MOT_STEPS_PER_REV_AXIS1;
	else if (axis == 2) *num_steps_per_rev = CM4000_NUM_MOT_STEPS_PER_REV_AXIS1;
	else if (axis == 3) *num_steps_per_rev = CM4000_NUM_MOT_STEPS_PER_REV_AXIS3;
	else if (axis == 4) *num_steps_per_rev = CM4000_NUM_MOT_STEPS_PER_REV_AXIS4;
	else cm4000_error("Unknown axis number");
	
	/* Done */
	return;
}

/*	**************************************************************  */
/*
 *	cm4000_get_num_enc_steps_per_rev
 *  This function gets the encoder's number of steps per revolution.
 *
 *	Input:
 *	axis, axis number (1 to 4)
 *
 *	Output:
 *	num_steps_per_rev, number of steps per revolution
 *
 *	Zachary Wolf
 *	9/9/02
 */

void cm4000_get_num_enc_steps_per_rev(int axis, long int* num_steps_per_rev)
{
	/* Determine the number of steps per revolutuon */
	if      (axis == 1) *num_steps_per_rev = CM4000_NUM_ENC_STEPS_PER_REV_AXIS1;
	else if (axis == 2) *num_steps_per_rev = CM4000_NUM_ENC_STEPS_PER_REV_AXIS1;
	else if (axis == 3) *num_steps_per_rev = CM4000_NUM_ENC_STEPS_PER_REV_AXIS3;
	else if (axis == 4) *num_steps_per_rev = CM4000_NUM_ENC_STEPS_PER_REV_AXIS4;
	else cm4000_error("Unknown axis number");
	
	/* Done */
	return;
}

/*	**************************************************************  */
/*
 *	cm4000_get_motor_resolution_code
 *  This function gets the motor resolution code.
 *
 *	Input:
 *	axis, axis number (1 to 4)
 *
 *	Output:
 *	mr, motor resolution code
 *
 *	Zachary Wolf
 *	9/9/02
 */

void cm4000_get_motor_resolution_code(int axis, int* mr)
{
	/* Declare variables */
	long int num_steps_per_rev;
	
	/* Get the number of steps per revolution */
	cm4000_get_num_mot_steps_per_rev(axis, &num_steps_per_rev);
	
	/* Determine the motor resolution code */
	/* See p. 41 of the manual */
	if      (num_steps_per_rev == 200)   *mr = 0;
	else if (num_steps_per_rev == 400)   *mr = 1;
	else if (num_steps_per_rev == 800)   *mr = 2;
	else if (num_steps_per_rev == 1000)  *mr = 3;
	else if (num_steps_per_rev == 2000)  *mr = 4;
	else if (num_steps_per_rev == 4000)  *mr = 5;
	else if (num_steps_per_rev == 5000)  *mr = 6;
	else if (num_steps_per_rev == 500)   *mr = 7;
	else if (num_steps_per_rev == 10000) *mr = 8;
	else if (num_steps_per_rev == 21600) *mr = 9;
	else if (num_steps_per_rev == 25000) *mr = 10;
	else if (num_steps_per_rev == 25400) *mr = 11;
	else if (num_steps_per_rev == 36000) *mr = 12;
	else if (num_steps_per_rev == 50000) *mr = 13;
	else if (num_steps_per_rev == 50800) *mr = 14;
	else if (num_steps_per_rev == 4096)  *mr = 15;
	else if (num_steps_per_rev == 12800) *mr = 16;
	else if (num_steps_per_rev == 25600) *mr = 17;
	else if (num_steps_per_rev == 12500) *mr = 18;
	else if (num_steps_per_rev == 16384) *mr = 19;
	else if (num_steps_per_rev == 20000) *mr = 20;
	else cm4000_error("Unknown motor resolution");
	
	/* Done */
	return;
}

/*	**************************************************************  */
/*
 *	cm4000_message
 *  This function handles messages about the CM4000.
 *
 *	Input:
 *	message, string to display in standard I/O
 *
 *	Zachary Wolf
 *	9/9/02
 */

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

/*	**************************************************************  */
/*
 *	cm4000_error
 *  This function handles error messages for the CM4000.
 *
 *	Input:
 *	message, string to display in standard I/O
 *
 *	Zachary Wolf
 *	9/9/02
 */

void cm4000_error(char* message)
{
	/* Declare variables */
	char buf[80];
	
	/* Notify the operator of the error */
	printf("\nCM4000 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);
}