/*	**************************************************************  */
/*
 *	Module CM6K
 *	This module contains I/O functions for the Compumotor model
 *	6K indexer.                 
 *
 *	Zachary Wolf
 *	4/4/06
 */ 

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

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

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

/*  PRIVATE FUNCTIONS  */
int cm6k_open_dev(int rs232_dev_addr);
void cm6k_close_dev(int dev_ID);
int cm6k_out(int dev_ID, char *buf);
int cm6k_in(int dev_ID, char* buf);
int cm6k_check_dev_open(int dev_ID);
int cm6k_read_error_status(int dev_ID);
void cm6k_get_num_mot_steps_per_rev(int axis, long int* num_steps_per_rev);
void cm6k_get_num_enc_steps_per_rev(int axis, long int* num_steps_per_rev);
void cm6k_message(char* msg);
void cm6k_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[CM6K_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[CM6K_MAX_CMD + 1];
static char msg[CM6K_MAX_CMD + 1];

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

/*	PUBLIC FUNCTIONS  */

/*	**************************************************************  */
/*
 *  cm6k_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
 *	4/4/06
 */

void cm6k_init(int rs232_com_port, int rs232_dev_addr, int* ID)
{
    /* Declare variables */
    int dev_ID;
    int err;
    int axis;
    long int num_steps_per_rev;
    
    /* Message */
    cm6k_message("");
    cm6k_message("Initializing the CM6K...");

    /* 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);
    	cm6k_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);
    	cm6k_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 CM6K */
    dev_ID = cm6k_open_dev(rs232_dev_addr);
    if (dev_ID <= 0)
    {
        cm6k_error("Device was not opened");
    	*ID = 0;
        return;
    }
    
    /* Turn all drives off */
    cm6k_out(dev_ID, "@DRIVE0");
    
    /* Reset the controller */
    cm6k_out(dev_ID, "RESET");
    Delay(3.);
    
    /* Use stepping motors on all axes */
    cm6k_out(dev_ID, "@AXSDEF0");
    
    /* Turn off command echoing */
    cm6k_out(dev_ID, "ECHO0");
    
    /* Enable hardware limit switches */
    //cm6k_out(dev_ID, "LD0");

    cm6k_out(dev_ID, "LH0,0");
    cm6k_out(dev_ID, "LS0,0");
    printf("cm6k: limit switches disabled!!!\n");
    
    /* Turn off scaling */
    cm6k_out(dev_ID, "Scale0");
    
    /* Set the resolution of each axis */
    for (axis = 1; axis <= CM6K_NUM_AXES; axis++)
    {
		cm6k_get_num_mot_steps_per_rev(axis, &num_steps_per_rev);
    	Fmt(msg, "%s<%i[w1]DRES%i", axis, num_steps_per_rev);
    	cm6k_out(dev_ID, msg);
    }
    
    /* Get a response from the CM6K */
    cm6k_out(dev_ID, "TREV");
    cm6k_in(dev_ID, msg);
    cm6k_message(msg);
    
    /* Set the present position to zero */
    //for (axis = 1; axis <= 4; axis++)
    //{
    //	Fmt(cmd, "%s<%i[w1]PZ", axis);
    //	cm6k_out(dev_ID, cmd);
    //}
    
    /* Turn all drives on */
    cm6k_out(dev_ID, "@DRIVE1");

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

/*	**************************************************************  */
/*
 *	cm6k_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 8)
 *	acc, acceleration (rev/sec/sec)
 *	vel, velocity (rev/sec)
 *	dis, distance (rev), can be + or -
 *
 *	Zachary Wolf
 *	4/4/06
 */

void cm6k_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;
	char go_array[20];
	int i;
	
	/* Convert distance in revolutions to distance in steps */
	cm6k_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 > CM6K_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm6k_error(msg);
    	return;
    }
    if (axis < 1 || axis > CM6K_NUM_AXES)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm6k_error(msg);
    	return;
    }
    if (acc < 0.001 || acc > 999.999)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	cm6k_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);
    	cm6k_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);
    	cm6k_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 = cm6k_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm6k_error("Device not open");
    	return;
    }
    
    /* Set to preset mode to move to a given position */
    Fmt(cmd, "%s<%i[w1]MC0", axis);
    cm6k_out(dev_ID, cmd);
    
    /* Set incremental mode */
    Fmt(cmd, "%s<%i[w1]MA0", axis);
    cm6k_out(dev_ID, cmd);
    
    /* Set the acceleration */
    Fmt(cmd, "%s<%i[w1]A%f[p2]", axis, acc);
    cm6k_out(dev_ID, cmd);
    
    /* Set the deceleration */
    Fmt(cmd, "%s<%i[w1]AD%f[p2]", axis, acc);
    cm6k_out(dev_ID, cmd);
    
    /* Set the velocity */
    Fmt(cmd, "%s<%i[w1]V%f[p3]", axis, vel);
    cm6k_out(dev_ID, cmd);
    
    /* Set the distance */
    Fmt(cmd, "%s<%i[w1]D%i", axis, dis_steps);
    cm6k_out(dev_ID, cmd);
    
    /* Move */
    for (i = 1; i <= CM6K_NUM_AXES; i++) go_array[i - 1] = '0';
    go_array[CM6K_NUM_AXES] = '\0';
    go_array[axis - 1] = '1';
    Fmt(cmd, "%s<GO%s", go_array);
    cm6k_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]D", axis);
    cm6k_out(dev_ID, cmd);
    cm6k_in(dev_ID, cmd);
    Scan(cmd, "%s>*%iD%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)
    {
    	cm6k_error("The wrong axis number was returned");
    	return;
    }
    if (steps_taken != dis_steps)
    {
    	cm6k_error("The wrong number of steps were taken");
    	return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm6k_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
 *	4/4/06
 */

void cm6k_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 */
	cm6k_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 > CM6K_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm6k_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm6k_error(msg);
    	return;
    }
    if (acc < 0.001 || acc > 999.999)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	cm6k_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);
    	cm6k_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);
    	cm6k_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm6k_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm6k_error("Device not open");
    	return;
    }
    
    /* Set normal mode to move to a given position */
    Fmt(cmd, "%s<%i[w1]MN", axis);
    cm6k_out(dev_ID, cmd);
    
    /* Set incremental mode */
    Fmt(cmd, "%s<%i[w1]FSA0", axis);
    cm6k_out(dev_ID, cmd);
    
    /* Set the acceleration */
    Fmt(cmd, "%s<%i[w1]A%f[p2]", axis, acc);
    cm6k_out(dev_ID, cmd);
    
    /* Set the velocity */
    Fmt(cmd, "%s<%i[w1]V%f[p3]", axis, vel);
    cm6k_out(dev_ID, cmd);
    
    /* Set the distance */
    Fmt(cmd, "%s<%i[w1]D%i", axis, dis_steps);
    cm6k_out(dev_ID, cmd);
    
    /* Move */
    Fmt(cmd, "%s<%i[w1]G", axis);
    cm6k_out(dev_ID, cmd);	/* Do not hold while moving */
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm6k_end_rel_move
 *  This function checks that a move started by cm6k_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
 *	4/4/06
 */

void cm6k_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 */
	cm6k_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 > CM6K_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm6k_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm6k_error(msg);
    	return;
    }
    if (labs(dis_steps) > 99999999)
    {
    	Fmt(msg, "%s<dis_steps = %i, out of range", dis_steps);
    	cm6k_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm6k_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm6k_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);
    cm6k_out(dev_ID, cmd);
    //cm6k_hold_until_ready_to_reply(dev_ID); 
    cm6k_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)
    {
    	cm6k_error("The wrong axis number was returned");
    	return;
    }
    if (steps_taken != dis_steps)
    {
    	cm6k_error("The wrong number of steps were taken");
    	return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm6k_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
 *	4/4/06
 */

void cm6k_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 */
	cm6k_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 > CM6K_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm6k_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm6k_error(msg);
    	return;
    }
    if (acc < 0.001 || acc > 999.999)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	cm6k_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);
    	cm6k_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);
    	cm6k_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm6k_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm6k_error("Device not open");
    	return;
    }
    
    /* Set normal mode to move to a given position */
    Fmt(cmd, "%s<%i[w1]MN", axis);
    cm6k_out(dev_ID, cmd);
    
    /* Set absolute mode */
    Fmt(cmd, "%s<%i[w1]FSA1", axis);
    cm6k_out(dev_ID, cmd);
    
    /* Set the acceleration */
    Fmt(cmd, "%s<%i[w1]A%f[p2]", axis, acc);
    cm6k_out(dev_ID, cmd);
    
    /* Set the velocity */
    Fmt(cmd, "%s<%i[w1]V%f[p3]", axis, vel);
    cm6k_out(dev_ID, cmd);
    
    /* Set the position */
    Fmt(cmd, "%s<%i[w1]D%i", axis, pos_steps);
    cm6k_out(dev_ID, cmd);
    
    /* Move */
    Fmt(cmd, "%s<%i[w1]G", axis);
    cm6k_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);
    cm6k_out(dev_ID, cmd);
    //cm6k_hold_until_ready_to_reply(dev_ID); 
    cm6k_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)
    {
    	cm6k_error("The wrong axis number was returned");
    	return;
    }
    if (fin_pos != pos_steps)
    {
    	cm6k_error("The wrong number of steps were taken");
    	return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm6k_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
 *	4/4/06
 */

void cm6k_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 > CM6K_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm6k_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm6k_error(msg);
    	return;
    }
    if (acc < 0.001 || acc > 999.999)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	cm6k_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);
    	cm6k_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 = cm6k_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm6k_error("Device not open");
    	return;
    }
    
    /* Set normal mode to move to a given position */
    Fmt(cmd, "%s<%i[w1]MN", axis);
    cm6k_out(dev_ID, cmd);
    
    /* Use only the home switch to define the home position */
    Fmt(cmd, "%s<%i[w1]OSD0", axis);
    cm6k_out(dev_ID, cmd);
    
    /* Define the active state of the home switch as a low signal */
    Fmt(cmd, "%s<%i[w1]OSC0", axis);
    cm6k_out(dev_ID, cmd);
    
    /* Use the CCW side of the home switch */
    Fmt(cmd, "%s<%i[w1]OSH1", axis);
    cm6k_out(dev_ID, cmd);
    
    /* Set the final homing direction to CCW */
    Fmt(cmd, "%s<%i[w1]OSG1", axis);
    cm6k_out(dev_ID, cmd);
    
    /* Back up to the home switch */
    Fmt(cmd, "%s<%i[w1]OSB1", axis);
    cm6k_out(dev_ID, cmd);
    
    /* Set the acceleration */
    Fmt(cmd, "%s<%i[w1]GA%f[p2]", axis, acc);
    cm6k_out(dev_ID, cmd);
    
    /* Move with the specified velocity */
    Fmt(cmd, "%s<%i[w1]GH%f[p3]", axis, vel);
    cm6k_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);
    cm6k_out(dev_ID, cmd);
    //cm6k_hold_until_ready_to_reply(dev_ID); 
    cm6k_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)
    {
    	cm6k_error("The wrong axis number was returned");
    	return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm6k_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
 *	4/4/06
 */

void cm6k_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 > CM6K_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm6k_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm6k_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm6k_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm6k_error("Device not open");
    	return;
    }
   
    /* Ask for the current position */
    Fmt(cmd, "%s<%i[w1]PR", axis);
    cm6k_out(dev_ID, cmd);
    //cm6k_hold_until_ready_to_reply(dev_ID); 
    cm6k_in(dev_ID, cmd);
    Scan(cmd, "%s>%i:%i", &reply_axis, &pos_steps);
    
    /* Check */
    if (reply_axis != axis)
    {
    	cm6k_error("The wrong axis number was returned");
    	return;
    }
	
	/* Convert the position in steps to position in revolutions */
	cm6k_get_num_mot_steps_per_rev(axis, &num_steps_per_rev);
	*pos = (double) pos_steps / (double) num_steps_per_rev;
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm6k_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
 *	4/4/06
 */

void cm6k_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 > CM6K_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm6k_error(msg);
    	return;
    }
    if (axis < 1 || axis > 4)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm6k_error(msg);
    	return;
    }
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif
    
    /* Check to make sure the device is open */
    dev_open = cm6k_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm6k_error("Device not open");
    	return;
    }
   
    /* Ask for the current position */
    Fmt(cmd, "%s<%i[w1]PX", axis);
    cm6k_out(dev_ID, cmd);
    //cm6k_hold_until_ready_to_reply(dev_ID); 
    cm6k_in(dev_ID, cmd);
    Scan(cmd, "%s>%i:%i", &reply_axis, &pos_steps);
    
    /* Check */
    if (reply_axis != axis)
    {
    	cm6k_error("The wrong axis number was returned");
    	return;
    }
	
	/* Convert the position in steps to position in revolutions */
	/* Four encoder transitions make one step */
	cm6k_get_num_enc_steps_per_rev(axis, &num_steps_per_rev);
	*pos = (double) pos_steps / (double)4 / (double) num_steps_per_rev;
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm6k_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 cm6k_zero(int dev_ID, int axis)
{
	/* Declare variables */
	int err;
	int dev_open;

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

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

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

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

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

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

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

/*	PRIVATE FUNCTIONS  */

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

    /* Check to see if the device is already in the device table */
    for (i = 1; i <= CM6K_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 <= CM6K_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)
    {
    	cm6k_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;
}

/*	**************************************************************  */
/*
 *	cm6k_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 cm6k_close_dev(int dev_ID)
{
	/* Close the device */
    if (dev_addr[dev_ID] != 0)
    {
        dev_addr[dev_ID] = 0;
        dev_count--;
    }
    
    /* Done */
    return;
}


/*	**************************************************************  */
/*
 *	cm6k_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 cm6k_out(int dev_ID, char* buf)
{
	/* Declare variables */
	char out_buf[CM6K_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 to the message */
	Fmt(out_buf, "%s<%s%c", buf, 13);
	//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);
		cm6k_error(msg);
		return -1;
	}
	
	/* Allow time for the command to be processed */
	Delay(.5);
	
	/* Done */
    return 0;
}

/*	**************************************************************  */
/*
 *	cm6k_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 cm6k_in(int dev_ID, char* buf)
{
	/* Declare variables */
	int nbytes;
	char in_buf[CM6K_MAX_CMD];
	
	/* Read the message, CR is discarded */
	nbytes = ComRdTerm(com_port, in_buf, CM6K_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);
		cm6k_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;
}

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

/*	**************************************************************  */
/*
 *	cm6k_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
 *	4/4/06
 */

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

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

void cm6k_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 = CM6K_NUM_MOT_STEPS_PER_REV_AXIS1;
	else if (axis == 2) *num_steps_per_rev = CM6K_NUM_MOT_STEPS_PER_REV_AXIS1;
	else if (axis == 3) *num_steps_per_rev = CM6K_NUM_MOT_STEPS_PER_REV_AXIS3;
	else if (axis == 4) *num_steps_per_rev = CM6K_NUM_MOT_STEPS_PER_REV_AXIS4;
	else if (axis == 5) *num_steps_per_rev = CM6K_NUM_MOT_STEPS_PER_REV_AXIS5;
	else if (axis == 6) *num_steps_per_rev = CM6K_NUM_MOT_STEPS_PER_REV_AXIS6;
	else if (axis == 7) *num_steps_per_rev = CM6K_NUM_MOT_STEPS_PER_REV_AXIS7;
	else if (axis == 8) *num_steps_per_rev = CM6K_NUM_MOT_STEPS_PER_REV_AXIS8;
	else cm6k_error("Unknown axis number");
	
	/* Done */
	return;
}

/*	**************************************************************  */
/*
 *	cm6k_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
 *	4/4/06
 */

void cm6k_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 = CM6K_NUM_ENC_STEPS_PER_REV_AXIS1;
	else if (axis == 2) *num_steps_per_rev = CM6K_NUM_ENC_STEPS_PER_REV_AXIS1;
	else if (axis == 3) *num_steps_per_rev = CM6K_NUM_ENC_STEPS_PER_REV_AXIS3;
	else if (axis == 4) *num_steps_per_rev = CM6K_NUM_ENC_STEPS_PER_REV_AXIS4;
	else cm6k_error("Unknown axis number");
	
	/* Done */
	return;
}

/*	**************************************************************  */
/*
 *	cm6k_message
 *  This function handles messages about the CM6K.
 *
 *	Input:
 *	message, string to display in standard I/O
 *
 *	Zachary Wolf
 *	4/4/06
 */

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

/*	**************************************************************  */
/*
 *	cm6k_error
 *  This function handles error messages for the CM6K.
 *
 *	Input:
 *	message, string to display in standard I/O
 *
 *	Zachary Wolf
 *	4/4/06
 */

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