/*	**************************************************************  */
/*
 *	Module SXDRIVE
 *	This module contains I/O functions for the Compumotor SX model
 *	indexer/drive controlled using RS232.                 
 *
 *	Zachary Wolf
 *	1/22/02
 */ 

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

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

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

/*  PRIVATE FUNCTIONS  */
int sxdrive_open_dev(int rs232_dev_addr);
void sxdrive_close_dev(int dev_ID);
int sxdrive_out(int dev_ID, char *buf);
int sxdrive_in(int dev_ID, char* buf);
int sxdrive_check_dev_open(int dev_ID);
int sxdrive_read_error_status(int dev_ID);
void sxdrive_hold_until_move_complete(int dev_ID);
void sxdrive_message(char* msg);
void sxdrive_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 addresses of opened devices
 *  dev_count, contains the number of devices open of this type
 */
static int dev_addr[SXDRIVE_MAX_NUM_DEV + 1];
static int dev_count;

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

/*  PRIVATE GLOBAL VARIABLES  */
/*
 *	com_port, port number for I/O
 *  cmd, buffer for RS232 I/O strings
 *  msg, buffer for message strings to Standard I/O
 */
static int com_port;
static char cmd[SXDRIVE_MAX_CMD + 1];
static char msg[SXDRIVE_MAX_CMD + 1];

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

/*	PUBLIC FUNCTIONS  */

/*	**************************************************************  */
/*
 *  sxdrive_init
 *  This function opens the device, queries for ID, and
 *  initializes the device to a known state.
 *
 *	Input:
 *	rs232_com_port, RS232 com port the device is connected to (1 to 32)
 *	rs232_dev_addr, RS232 address of the device (00 to 99, 00 is reserved)
 *
 *	Output:
 *	ID, identifier for future references to the device
 *
 *	Zachary Wolf
 *	1/22/02
 */

void sxdrive_init(int rs232_com_port, int rs232_dev_addr, int* ID)
{
    /* Declare variables */
    int dev_ID;
    int err;
    
    /* Message */
    sxdrive_message("");
    sxdrive_message("Initializing the SXDRIVE...");

    /* Check input parameters */
    if (rs232_com_port < 1 || rs232_com_port > 32)
    {
    	Fmt(msg, "%s<rs232_com_port = %i, out of range", rs232_com_port);
    	sxdrive_error(cmd);
    	*ID = 0;
    	return;
    }
    if (rs232_dev_addr < 0 || rs232_dev_addr > 99)
    {
    	Fmt(msg, "%s<rs232_dev_addr = %i, out of range", rs232_dev_addr);
    	sxdrive_error(msg);
    	*ID = 0;
    	return;
    }
    
    /* Save the com port number */
    com_port = rs232_com_port;
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	*ID = 1;
    	return;
    #endif
    
    /* Open RS232 communications */
	err = OpenComConfig(com_port, "", 9600, 0, 8, 1, 512, 512);
	if (err != 0) printf("OpenCom err = %i\n", err);
	err = FlushInQ(com_port);
	if (err != 0) printf("FlushInQ err = %i\n", err);
	err = FlushOutQ(com_port);
	if (err != 0) printf("FlushOutQ err = %i\n", err);
    
    /* Open the SXDRIVE */
    dev_ID = sxdrive_open_dev(rs232_dev_addr);
    if (dev_ID <= 0)
    {
        sxdrive_error("Device was not opened");
    	*ID = 0;
        return;
    }
    
    /* Turn on the drive */
    sxdrive_out(dev_ID, "ON");
    
    /* Reset the drive */
    sxdrive_out(dev_ID, "Z");
    Delay(3.5);
    
    /* Disable interactive mode */
    sxdrive_out(dev_ID, "SSI1");
    
    /* Turn off command echoing */
    sxdrive_out(dev_ID, "SSA1");
    
    /* Set the mode to normal */
    sxdrive_out(dev_ID, "MN");
    
    /* Enable hardware limit switches */
    //sxdrive_out(dev_ID, "LD0");

    sxdrive_out(dev_ID, "LD3");
    printf("sxdrive: limit switches disabled!!!\n");
    
    /* Get a response from the SXDRIVE */
    sxdrive_out(dev_ID, "RVV");
    sxdrive_in(dev_ID, msg);
    sxdrive_message(msg);
    
    /* Check for errors */
    err = sxdrive_read_error_status(dev_ID);
    if (err != 0)
    {
        sxdrive_error("The device has an error");
        sxdrive_close_dev(dev_ID);
        *ID = 0;
        return;
    }

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

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

void sxdrive_rel_move(int dev_ID, double acc, double vel, double dis)
{
	/* Declare variables */
	int err;
	int dev_open;
	long int dis_steps;
	
	/* Convert distance in revolutions to distance in steps */
	dis_steps = dis * SXDRIVE_NUM_STEPS_PER_REV;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > SXDRIVE_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	sxdrive_error(msg);
    	return;
    }
    if (acc < 0.01 || acc > 9999.99)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	sxdrive_error(msg);
    	if (acc < 0.01) acc = 0.01; else acc = 9999.99;
    }
    if (vel < 0. || vel > 50.)
    {
    	Fmt(msg, "%s<vel = %f, out of range, will be adjusted", vel);
    	sxdrive_error(msg);
    	if (vel < 0.) vel = 0.00001; else vel = 50.;
    }
    if (labs(dis_steps) > 2147483647)
    {
    	Fmt(msg, "%s<dis_steps = %i, out of range", dis_steps);
    	sxdrive_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 = sxdrive_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	sxdrive_error("Device not open");
    	return;
    }
    
    /* Set the mode to normal */
    sxdrive_out(dev_ID, "MN");
    
    /* Use incremental positioning */
    sxdrive_out(dev_ID, "MPI");
    
    /* Set the acceleration */
    Fmt(cmd, "%s<A%f[p2]", acc);
    sxdrive_out(dev_ID, cmd);
    
    /* Set the deleration */
    Fmt(cmd, "%s<AD%f[p2]", acc);
    sxdrive_out(dev_ID, cmd);
    
    /* Set the velocity */
    Fmt(cmd, "%s<V%f[p5]", vel);
    sxdrive_out(dev_ID, cmd);
    
    /* Set the distance */
    Fmt(cmd, "%s<D%i", dis_steps);
    sxdrive_out(dev_ID, cmd);
    
    /* Move */
    sxdrive_out(dev_ID, "G");
    sxdrive_hold_until_move_complete(dev_ID); 
    
    /* Check for errors */
    err = sxdrive_read_error_status(dev_ID);
    if (err != 0)
    {
        sxdrive_error("The device has an error");
        sxdrive_close_dev(dev_ID);
        return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	sxdrive_abs_move
 *  This function moves the motor to the specified position
 *	with the specified acceleration and velocity.
 *
 *	Input:
 *	dev_ID, device identifier
 *	acc, acceleration (rev/sec/sec)
 *	vel, velocity (rev/sec)
 *	pos, position (rev), can be + or -
 *
 *	Zachary Wolf
 *	2/15/02
 */

void sxdrive_abs_move(int dev_ID, double acc, double vel, double pos)
{
	/* Declare variables */
	int err;
	int dev_open;
	long int pos_steps;
	
	/* Convert the position in revolutions to position in steps */
	pos_steps = pos * SXDRIVE_NUM_STEPS_PER_REV;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > SXDRIVE_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	sxdrive_error(msg);
    	return;
    }
    if (acc < 0.01 || acc > 9999.99)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	sxdrive_error(msg);
    	if (acc < 0.01) acc = 0.01; else acc = 9999.99;
    }
    if (vel < 0. || vel > 50.)
    {
    	Fmt(msg, "%s<vel = %f, out of range, will be adjusted", vel);
    	sxdrive_error(msg);
    	if (vel < 0.) vel = 0.00001; else vel = 50.;
    }
    if (labs(pos_steps) > 2147483647)
    {
    	Fmt(msg, "%s<pos_steps = %i, out of range", pos_steps);
    	sxdrive_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = sxdrive_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	sxdrive_error("Device not open");
    	return;
    }
    
    /* Set the mode to normal */
    sxdrive_out(dev_ID, "MN");
    
    /* Use absolute positioning */
    sxdrive_out(dev_ID, "MPA");
    
    /* Set the acceleration */
    Fmt(cmd, "%s<A%f[p2]", acc);
    sxdrive_out(dev_ID, cmd);
    
    /* Set the deleration */
    Fmt(cmd, "%s<AD%f[p2]", acc);
    sxdrive_out(dev_ID, cmd);
    
    /* Set the velocity */
    Fmt(cmd, "%s<V%f[p5]", vel);
    sxdrive_out(dev_ID, cmd);
    
    /* Set the position */
    Fmt(cmd, "%s<D%i", pos_steps);
    sxdrive_out(dev_ID, cmd);
    
    /* Move */
    sxdrive_out(dev_ID, "G");
    sxdrive_hold_until_move_complete(dev_ID); 
    
    /* Check for errors */
    err = sxdrive_read_error_status(dev_ID);
    if (err != 0)
    {
        sxdrive_error("The device has an error");
        sxdrive_close_dev(dev_ID);
        return;
    }
    
    /* Done */
    return;
}

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

void sxdrive_get_mot_position(int dev_ID, double* pos)
{
	/* Declare variables */
	int dev_open;
	long int pos_steps;
	int num_scan;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > SXDRIVE_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	sxdrive_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = sxdrive_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	sxdrive_error("Device not open");
    	return;
    }
   
    /* Ask for the current position */
    sxdrive_out(dev_ID, "PR");
    sxdrive_in(dev_ID, cmd);
    num_scan = Scan(cmd, "%s>*%i", &pos_steps);
    if (num_scan != 1) sxdrive_error("sxdrive_get_mot_position: Can't read the motor position.");
	
	/* Convert the position in steps to position in revolutions */
	*pos = (double) pos_steps / (double) SXDRIVE_NUM_STEPS_PER_REV;
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	sxdrive_get_enc_position
 *  This function gets the encoder position from the indexer. 
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Output:
 *	pos, position (encoder lines), can be + or -
 *
 *	Zachary Wolf
 *	2/15/02
 */

void sxdrive_get_enc_position(int dev_ID, double* pos)
{
	/* Declare variables */
	int dev_open;
	long int pos_steps;
	int num_scan;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > SXDRIVE_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	sxdrive_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = sxdrive_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	sxdrive_error("Device not open");
    	return;
    }
   
    /* Ask for the current position */
    sxdrive_out(dev_ID, "PX");
    sxdrive_in(dev_ID, cmd);
    num_scan = Scan(cmd, "%s>*%i", &pos_steps);
    if (num_scan != 1) sxdrive_error("sxdrive_get_enc_position: Can't read the encoder position.");
	
	/* Convert the position in steps to position in encoder lines */
	*pos = (double) pos_steps / 4.;
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	sxdrive_get_inputs
 *  This function gets the state of the limit switches and
 *	input lines from the indexer. 
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Output:
 *	inputs, status of the inputs from the indexer (see p. 35)
 *
 *	Zachary Wolf
 *	2/25/02
 */

void sxdrive_get_inputs(int dev_ID, char inputs[])
{
	/* Declare variables */
	int dev_open;
	int num_scan;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > SXDRIVE_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	sxdrive_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = sxdrive_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	sxdrive_error("Device not open");
    	return;
    }
   
    /* Ask for the input status report */
    sxdrive_out(dev_ID, "IS");
    sxdrive_in(dev_ID, cmd);
    num_scan = Scan(cmd, "%s>%s", inputs);
    if (num_scan != 1) sxdrive_error("sxdrive_get_inputs: Can't read the inputs.");
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	sxdrive_set_software_limits
 *  This function enables and sets software limits in the indexer. 
 *
 *	Input:
 *	dev_ID, device identifier
 *	cw_limit, CW software limit (rev)
 *	ccw_limit, CCW software limit (rev)
 *
 *	Zachary Wolf
 *	2/25/02
 */

void sxdrive_set_software_limits(int dev_ID, double cw_limit, double ccw_limit)
{
	/* Declare variables */
	int dev_open;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > SXDRIVE_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	sxdrive_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = sxdrive_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	sxdrive_error("Device not open");
    	return;
    }
   
    /* Enable software limits */
    sxdrive_out(dev_ID, "SLD0");
    
    /* Set the deceleration when a limit is reached */
    //sxdrive_out(dev_ID, "LAD100");
    
    /* Set the software limits */
    Fmt(cmd, "%s<SL%i,%i", (long int) ccw_limit * SXDRIVE_NUM_STEPS_PER_REV, (long int) cw_limit * SXDRIVE_NUM_STEPS_PER_REV);
	sxdrive_out(dev_ID, cmd);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	sxdrive_set_manual_jogs
 *  This function sets up the indexer to allow manual jogs.  Input 1
 *	performs CW jogs. Input 2 performs CCW jogs.
 *
 *	Input:
 *	dev_ID, device identifier
 *	acc, acceleration (rev/sec/sec)
 *	vel, velocity (rev/sec)
 *
 *	Zachary Wolf
 *	1/27/02
 */

void sxdrive_set_manual_jogs(int dev_ID, double acc, double vel)
{
	/* Declare variables */
	int err;
	int dev_open;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > SXDRIVE_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	sxdrive_error(msg);
    	return;
    }
    if (acc < 0.01 || acc > 999.99)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	sxdrive_error(msg);
    	if (acc < 0.01) acc = 0.01; else acc = 999.99;
    }
    if (vel < 0.0001 || vel > 50.)
    {
    	Fmt(msg, "%s<vel = %f, out of range, will be adjusted", vel);
    	sxdrive_error(msg);
    	if (vel < 0.0001) vel = 0.0001; else vel = 50.;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = sxdrive_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	sxdrive_error("Device not open");
    	return;
    }
    
    /* Enable jogs */
    sxdrive_out(dev_ID, "OSE1");
    
    /* Set the acceleration */
    Fmt(cmd, "%s<JA%f[p2]", acc);
    sxdrive_out(dev_ID, cmd);
    
    /* Set the deleration */
    Fmt(cmd, "%s<JAD%f[p2]", acc);
    sxdrive_out(dev_ID, cmd);
    
    /* Set the velocity */
    Fmt(cmd, "%s<JVL%f[p4]", vel);
    sxdrive_out(dev_ID, cmd);
    Fmt(cmd, "%s<JVH%f[p4]", vel);
    sxdrive_out(dev_ID, cmd);
    
    /* Use input 1 for CW jogs */
    sxdrive_out(dev_ID, "IN1J");
    
    /* Use input 2 for CCW jogs */
    sxdrive_out(dev_ID, "IN2K");
    
    /* Check for errors */
    err = sxdrive_read_error_status(dev_ID);
    if (err != 0)
    {
        sxdrive_error("The device has an error");
        sxdrive_close_dev(dev_ID);
        return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	sxdrive_go_home
 *  This function has the indexer run the motor until the home
 *	switch is activated.  The home switch must be at the far CCW
 *	range of the motor.
 *
 *	Input:
 *	dev_ID, device identifier
 *	acc, acceleration (rev/sec/sec)
 *	vel, velocity (rev/sec)
 *
 *	Zachary Wolf
 *	1/28/02
 */

void sxdrive_go_home(int dev_ID, double acc, double vel)
{
	/* Declare variables */
	int err;
	int dev_open;

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > SXDRIVE_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	sxdrive_error(msg);
    	return;
    }
    if (acc < 0.01 || acc > 9999.99)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	sxdrive_error(msg);
    	if (acc < 0.01) acc = 0.01; else acc = 9999.99;
    }
    if (vel < 0.00001 || vel > 50.)
    {
    	Fmt(msg, "%s<vel = %f, out of range, will be adjusted", vel);
    	sxdrive_error(msg);
    	if (vel < 0.00001) vel = 0.00001; else vel = 50.;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = sxdrive_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	sxdrive_error("Device not open");
    	return;
    }
    
    /* Back up to home */
    sxdrive_out(dev_ID, "OSB1");
    
    /* Final homing direction CCW */
    sxdrive_out(dev_ID, "OSG1");
    
    /* Stop at CW edge of switch */
    sxdrive_out(dev_ID, "OSH0");
    
    /* Set the acceleration */
    Fmt(cmd, "%s<GHA%f[p2]", acc);
    sxdrive_out(dev_ID, cmd);
    
    /* Set the deleration */
    Fmt(cmd, "%s<GHAD%f[p2]", acc);
    sxdrive_out(dev_ID, cmd);
    
    /* Set the velocity */
    Fmt(cmd, "%s<GHV-%f[p5]", vel);
    sxdrive_out(dev_ID, cmd);
    
    /* Set the final velocity as the switch is approached */
    Fmt(cmd, "%s<GHF%f[p5]", vel/10.);
    sxdrive_out(dev_ID, cmd);
    
    /* Go home */
    sxdrive_out(dev_ID, "GH");
    
    /* Check for errors */
    err = sxdrive_read_error_status(dev_ID);
    if (err != 0)
    {
        sxdrive_error("The device has an error");
        sxdrive_close_dev(dev_ID);
        return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	sxdrive_zero
 *  This function sets the motor position counter to zero and also
 *	the encoder position counter to zero.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Zachary Wolf
 *	2/15/02
 */

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

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

    /* Zero the position counters */
    sxdrive_out(dev_ID, "PZ");
    
    /* Check for errors */
    err = sxdrive_read_error_status(dev_ID);
    if (err != 0)
    {
        sxdrive_error("The device has an error");
        sxdrive_close_dev(dev_ID);
        return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	sxdrive_exit
 *  This function prepares the SXDRIVE for shutdown and closes
 *	the device.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Zachary Wolf
 *	2/23/02
 */

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

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > SXDRIVE_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	sxdrive_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = sxdrive_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	return;		 /* OK, already closed */
    }
    
    /* Leave the SXDRIVE in the desired state */
    /* De-energize the drive */
    sxdrive_out(dev_ID, "OFF");

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

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

/*	PRIVATE FUNCTIONS  */

/*	**************************************************************  */
/*
 *	sxdrive_open_dev
 *	This function opens the device and returns an identifier for
 *	future references to the device.
 *
 *	Input:
 *	rs232_dev_addr, RS232 address of the device (00 to 99, 00 is reserved)
 *
 *	Output:
 *	dev_ID, identifier for future references to the	device (-1 if there is a problem)
 *
 *	Zachary Wolf
 *	2/23/02
 */

int sxdrive_open_dev(int rs232_dev_addr)
{
	/* Declare variables */
    int i;
    int dev_ID;

    /* Check input parameters */
    if (rs232_dev_addr < 0 || rs232_dev_addr > 99)
    {
    	Fmt(msg, "%s<rs232_dev_addr = %i, out of range", rs232_dev_addr);
    	sxdrive_error(msg);
    	return -1;
    }
    
    /* Initialization */
    dev_ID = 0;

    /* Check to see if the device is already in the device table */
    for (i = 1; i <= SXDRIVE_MAX_NUM_DEV; i++)
    {
        if (dev_addr[i] == rs232_dev_addr)
        {
            dev_ID = i;
            return dev_ID;
        }
    }
    
    /* Find an ID for the device */
	for (i = 1; i <= SXDRIVE_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)
    {
    	sxdrive_error("The device could not be opened");
        return  -1;
    }
    
    /* Open successful, save results */
    dev_addr[dev_ID] = rs232_dev_addr;
    dev_count++;
    
    /* Return the device ID */
    return dev_ID;
}

/*	**************************************************************  */
/*
 *	sxdrive_close_dev
 *  This function closes the device by removing it from the RS232
 *	device table.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Zachary Wolf
 *	2/23/02
 */

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


/*	**************************************************************  */
/*
 *	sxdrive_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
 *	2/23/02
 */

int sxdrive_out(int dev_ID, char* buf)
{
	/* Declare variables */
	char out_buf[SXDRIVE_MAX_CMD];
	int nbytes;
	int err;
	
	/* Flush the input and output queues */
	err = FlushInQ(com_port);
	if (err != 0) printf("FlushInQ err = %i\n", err);
	err = FlushOutQ(com_port);
	if (err != 0) printf("FlushOutQ err = %i\n", err);
	
	/* Add the address and CR LF to the message */
	Fmt(out_buf, "%s<%i[w2p0]%s%c%c", dev_addr[dev_ID], buf, 13, 10);
	//printf("out_buf = %s\n", out_buf);
	
	/* Send the message */
	nbytes = StringLength(out_buf);
	err = ComWrt(com_port, out_buf, nbytes);
	
	/* Check for errors */
	if (err <= 0)
	{
		Fmt(msg, "%s<err = %i, problem sending RS232 command", err);
		sxdrive_error(msg);
		return -1;
	}
	
	/* Allow time for the command to be processed */
	Delay(.6);
	
	/* Done */
    return 0;
}

/*	**************************************************************  */
/*
 *	sxdrive_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
 *	2/23/02
 */

int sxdrive_in(int dev_ID, char* buf)
{
	/* Declare variables */
	int nbytes;
	char in_buf[SXDRIVE_MAX_CMD];
	
	/* Read the message, CR and LF are discarded */
	nbytes = ComRdTerm(com_port, in_buf, SXDRIVE_MAX_CMD, 13);
	//printf("in_buf = %s\n", in_buf);
	
	/* Check for errors */
	if (nbytes <= 0)
	{
		Fmt(msg, "%s<# bytes in = %i, problem receiving RS232 input", nbytes);
		sxdrive_error(msg);
		return -1;
	}
	
	/* Null terminate the string */
	in_buf[nbytes] = '\0';
	
	/* Remove the CR LF terminator */
	//Scan(in_buf, "%s[t13]>%s[t-]", buf);	/* Already done */
	
	/* Return the string in buf */
	strcpy(buf, in_buf);
	
	/* Done */
    return 0;
}

/*	**************************************************************  */
/*
 *	sxdrive_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
 *	2/23/02
 */

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

/*	**************************************************************  */
/*
 *	sxdrive_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
 *	2/23/02
 */

int sxdrive_read_error_status(int dev_ID)
{
	/* Have the indexer report any errors */
    sxdrive_out(dev_ID, "RSE");
    sxdrive_in(dev_ID, msg);
	
	/* Check for problems */
    if (strcmp(msg, "*NO_ERRORS") != 0 && strcmp(msg, "*ERROR___BATTERY_BACKUP_RAM_CORRUPTED") != 0)
    {
        sxdrive_error(msg);
        return -1;
    }
    
    /* Done, exit here if no errors */
    return 0;
}

/*	**************************************************************  */
/*
 *	sxdrive_hold_until_move_complete
 *  This function requests the status of the device in a loop until the
 *	move is complete.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Zachary Wolf
 *	1/30/02
 */

void sxdrive_hold_until_move_complete(int dev_ID)
{
	/* Declare variables */
	char status[SXDRIVE_MAX_CMD];
	
	/* Have the indexer report its status */
	check_again:
	sxdrive_out(dev_ID, "R");
	sxdrive_in(dev_ID, status);
	
    /* Check the status */
    if      (strcmp(status, "*R") == 0)		/* Ready */
    {
    	return;
    }
    else if (strcmp(status, "*S") == 0)		/* Attention needed */
    {
    	sxdrive_message("SX drive needs attention");
    	return;
    }
    else if (strcmp(status, "*B") == 0)		/* Busy */
    {
    	Delay(1.);
    	goto check_again;
    }
    else if (strcmp(status, "*C") == 0)		/* Busy, attention needed */
    {
    	Delay(1.);
    	goto check_again;
    }
    else if (strcmp(status, "*Z") == 0)		/* Busy, auto run mode */
    {
    	Delay(1.);
    	goto check_again;
    }
    else if (strcmp(status, "*K") == 0)		/* Kill input active */
    {
    	Delay(1.);
    	goto check_again;
    }
    else if (strcmp(status, "*W") == 0)		/* Waiting for SSC */
    {
    	Delay(1.);
    	goto check_again;
    }
    else
    {
    	sxdrive_error("Unknown SX status response.");
    	return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	sxdrive_message
 *  This function handles messages about the SXDRIVE.
 *
 *	Input:
 *	message, string to display in standard I/O
 *
 *	Zachary Wolf
 *	2/23/02
 */

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

/*	**************************************************************  */
/*
 *	sxdrive_error
 *  This function handles error messages for the SXDRIVE.
 *
 *	Input:
 *	message, string to display in standard I/O
 *
 *	Zachary Wolf
 *	2/23/02
 */

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