#include <userint.h>

/*	**************************************************************  */
/*
 *	Module CM6K6
 *	This module contains I/O functions for the Compumotor model
 *	6K6 indexer.                 
 *
 *	Zachary Wolf
 *	9/4/02
 * Modified for CM6K6 module by Yurii Levashov
 */ 

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

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

/*  PRIVATE FUNCTIONS  */


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

/*  PRIVATE DEVICE TABLE  */
/*
 *	This table allows several devices of the same type to be
 *	used in the system.
 *	dev_addr, contains the rs232 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[CM6K6_MAX_NUM_DEV + 1];
static int dev_descr[CM6K6_MAX_NUM_DEV + 1];
static int dev_count;

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

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

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

/*	PUBLIC FUNCTIONS  */

/*	**************************************************************  */
/*
 *  cm6k6_init
 *  This function opens the device, queries for ID, and
 *  initializes the device to a known state.
 *
 *	Input:
 *	rs232_com_port (1-32)
 *	rs232_dev_addr  (00-99) (00 is reserved)
 *
 *	Output:
 *	ID, identifier for future references to the device
 *
 *	Zachary Wolf
 *	9/4/02
 *  Modified by Yurii Levashov 01/26/2005
 */

void cm6k6_init(int rs232_com_port, int rs232_dev_addr, int* ID)
{
    /* Declare variables */
    int dev_ID;
    int err;
    int mr;
    int er;
    int axis;
    long int mres[7];
    long int eres[7];
    long int scale[7];
    double acc;
    int limit;
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	*ID = 1;
    	return;
    #endif
    
    /* Check input parameters */
    if(rs232_com_port < 1 || rs232_com_port > 32)
        if(cm6k6_error_handling(rs232_com_port, "RS232_COM_PORT") == 1) return;
         
    if(rs232_dev_addr < 0 || rs232_dev_addr > 99)
    	if(cm6k6_error_handling(rs232_dev_addr, "RS232_DEV_ADDR") == 1) return;
    
    /* Save the com port number */
    com_port = rs232_com_port;
    
    /* Open RS232 communications */
	err = OpenComConfig(com_port, "", 9600, 0, 8, 1, 512, 512);
	if (err != 0)
	 if(cm6k6_error_handling(err, "OpenCom") == 1) return;
	
	err = FlushInQ(com_port);
	if (err != 0)
	 if(cm6k6_error_handling(err, "FlushInQ") == 1) return; 
	
	err = FlushOutQ(com_port);
	if (err != 0)
	 if(cm6k6_error_handling(err, "FlushOutQ") == 1) return;
    
    /* Open the CM6K6 */
    dev_ID = cm6k6_open_dev(rs232_dev_addr);
    if (dev_ID <= 0)
     if(cm6k6_error_handling(dev_ID, "RS232.Device was not opened") == 1) return;
    
    /* Reset */
    cm6k6_out(dev_ID, "!RESET");
    Delay(5);

    /* set COM1 for command transmission */
    cm6k6_out(dev_ID, "PORT1");
    
    /* Enable communication */
    cm6k6_out(dev_ID, "E1");
     
     /* Disable Echo */
    cm6k6_out(dev_ID, "ECHO0");
    
    /* set responce field without command itself and error */
 //   cm6k6_out(dev_ID, "ERRLVL0");
    
    /* Make sure start-up program doesn't execute */
    cm6k6_out(dev_ID, "STARTP CLR");
    
    /* Define stepper motor axis types */
//    cm6k6_out(dev_ID, "DRIVE000000");   // De-Energize the motor drives
    cm6k6_out(dev_ID, "AXSDEF000000");  // 0 - for stepper motors
    
    /* Define position limits settings */
    cm6k6_out(dev_ID, "LS0,0,0,0,0,0");   // Programmable limits disabled 
//	GetCtrlVal(panel_ID, PANEL_LIMITS_DISABLE, &limit);
//    if(limit == 1)
      cm6k6_out(dev_ID, "LH0,0,0,0,0,0");   // End-of-travel limits disabled
//       else
//         {
//		  err = MessagePopup ("WARNING",
//							  "Please make sure limit switches are connected");
//          cm6k6_out(dev_ID, "LH3,3,3,3,3,3");   // End-of-travel limits enabled
          cm6k6_out(dev_ID, "LIMLVL0");			// Set signal level to low
//          GetCtrlVal (PANEL, PANEL_NUMERIC_ACCELERATION, &acc);
          
          Fmt(cmd, "%s<@LHAD%f", ACC);  //Set deceleration value 10 times higher
    	  cm6k6_out(dev_ID, cmd);
//	     }
	     
    /* Disable drive fault checking */
    cm6k6_out(dev_ID, "DRFEN000000");
    
    /* Enable error detection */
//    cm6k6_out(dev_ID, "ERROR111111");
    
    /* Enable encoder ports */
    cm6k6_out(dev_ID, "ENCCNT111111");	  // Reference to the encoder position

    /* Set the motor resolutions and follower scaling in revs*/
    for (axis = 1; axis <= 6; axis++)
    {
		cm6k6_get_num_mot_steps_per_rev(axis, &mr);
		mres[axis] = mr;
    }
    
    /* Enable Scaling */
    cm6k6_out(dev_ID, "SCALE1");
    
    Fmt(cmd, "%s<DRES%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0]",mres[1], mres[2], mres[3], mres[4], mres[5], mres[6]);
    cm6k6_out(dev_ID, cmd);
    Fmt(cmd, "%s<SCLD%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0]",mres[1], mres[2], mres[3], mres[4], mres[5], mres[6]);
    cm6k6_out(dev_ID, cmd);
    Fmt(cmd, "%s<SCLV%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0]",mres[1], mres[2], mres[3], mres[4], mres[5], mres[6]);
    cm6k6_out(dev_ID, cmd);
    Fmt(cmd, "%s<SCLA%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0]",mres[1], mres[2], mres[3], mres[4], mres[5], mres[6]);
    cm6k6_out(dev_ID, cmd);

    /* Set the encoder resolution and master scaling in revs*/
    for (axis = 1; axis <= 6; axis++)
    {
		cm6k6_get_num_enc_cts_per_rev(axis, &er);
		eres[axis] = er;
    }
    Fmt(cmd, "%s<ERES%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0]",eres[1], eres[2], eres[3], eres[4], eres[5], eres[6]);
    cm6k6_out(dev_ID, cmd);
    Fmt(cmd, "%s<SCLMAS%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0]",eres[1], eres[2], eres[3], eres[4], eres[5], eres[6]);
    cm6k6_out(dev_ID, cmd);
   	 
    /* Set follower:master ratio 1:1 */
    cm6k6_out(dev_ID, "FOLRN1,1,1,1,1,1");
    cm6k6_out(dev_ID, "FOLRD1,1,1,1,1,1");
    
    /* Energize the drives */
    Fmt(cmd, "%s<DRIVE111111");
    cm6k6_out(dev_ID, cmd);
    
    /* Set the present follower position to zero*/
    Fmt(cmd, "%s<PSET000000");
    cm6k6_out(dev_ID, cmd);

    /* Allow commands during motion */
    cm6k6_out(dev_ID, "COMEXC1");
    
        // De - energize the drives */
    Fmt(cmd, "%s<DRIVE000000");
    cm6k6_out(dev_ID, cmd);
    
    /* Get a response from the CM6K6*/
    cm6k6_read_error_status(dev_ID);

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


/*	**************************************************************  */
/*
 *	cm6k6_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 6)
 *	acc, acceleration (rev/sec/sec)
 *	vel, velocity (rev/sec)
 *	dis, distance (rev), can be + or -
 *
 *	Zachary Wolf
 *	9/4/02
 *  Modified by Yurii Levashov 01/26/2005
 */

int cm6k6_rel_move(int dev_ID, int axis, double acc, double vel, double dis)
{
	/* Declare variables */
	int dev_open, err;
	char st[] = "NOT";
	double ini_pos;
	double fin_pos;
	double revs_taken;
	int ID;
	
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return 0;
    #endif

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM6K6_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
		 if(cm6k6_error_handling(dev_ID, msg) == 1) return -1; 
	}
    if (axis < 1 || axis > CM6K6_NUM_AXIS)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	 if(cm6k6_error_handling(axis, msg) == 1) return -1;
    }
    if (acc < 0.001 || acc > 9999.9)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	 if(cm6k6_error_handling(acc, msg) == 1) return -1;
    	if (acc < 0.001) acc = 0.001; else acc = 9999.9;
    }
    if (vel < 0.1 || vel > 99.)
    {
    	Fmt(msg, "%s<vel = %f, out of range, will be adjusted", vel);
    	 if(cm6k6_error_handling(vel, msg) == 1) return -1;
    	if (vel < 0.1) vel = 0.1; else vel = 99.;
    }
    
    /* Check to make sure the device is open */
    dev_open = cm6k6_check_dev_open(dev_ID);
    if (dev_open != 1)
     if(cm6k6_error_handling(dev_open, "Device access. Device is not Open") == 1) return -1;
	  
    /* Ask for the initial position */
    Fmt(cmd, "%s<%i[w1]TPC", axis);
    cm6k6_out(dev_ID, cmd);
    cm6k6_in(dev_ID, cmd);
    CopyString(cmd,0, cmd,5,20);
    Scan(cmd, "%s>%f", &ini_pos);
    
    /* Set incremental-preset mode */
    Fmt(cmd, "%s<%i[w1]MA0", axis);	   // Move wrt current position
    cm6k6_out(dev_ID, cmd);
    Fmt(cmd, "%s<%i[w1]MC0", axis);	   //to go a specific distance
    cm6k6_out(dev_ID, cmd);
    
    /* Set the acceleration */
    Fmt(cmd, "%s<%i[w1]A%f", axis, acc);
    cm6k6_out(dev_ID, cmd);
    
    /* Set the velocity */
    Fmt(cmd, "%s<%i[w1]V%f", axis, vel);
    cm6k6_out(dev_ID, cmd);
    
    /* Set the distance */
    Fmt(cmd, "%s<%i[w1]D%f", axis, dis);
    cm6k6_out(dev_ID, cmd);
    
     /* Move*/
    if (axis == 1)
    {
    Fmt(cmd, "%s<GO100000");
    }
     if (axis == 2)
    {
    Fmt(cmd, "%s<GO010000");
    }
     if (axis == 3)
    {
    Fmt(cmd, "%s<GO001000");
    }
     if (axis == 4)
    {
    Fmt(cmd, "%s<GO000100");
    }
    if (axis == 5)
    {
    Fmt(cmd, "%s<GO000010");
    }
     if (axis == 6)
    {
    Fmt(cmd, "%s<GO000001");
    }
    cm6k6_out(dev_ID, cmd);

	/* Wait until the move is complete */
	err = cm6k6_ifmove_delay (dev_ID, axis);
	  if(err != 0) return err;
	  
   /* Ask for the final position */ 
    Fmt(cmd, "%s<%i[w1]TPC", axis);
    cm6k6_out(dev_ID, cmd);
    cm6k6_in(dev_ID, cmd);
    CopyString(cmd,0, cmd,5,20);
    Scan(cmd, "%s>%f", &fin_pos);
    
    /* Caclulate number of steps taken */
    revs_taken = fin_pos - ini_pos;
    
    /* Make sure the proper position was achieved */  
    err = (int) fabs(dis * 100000) - (int) fabs(revs_taken * 100000);
    if (err != 0)
     if(cm6k6_error_handling(err, "Positioning") == 1) return -1;

    /* Check for command errors */
    cm6k6_read_error_status(dev_ID);
    
    /* Done */
    return 0;
}

/*	**************************************************************  */
/*
 *	cm6k6_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
 *  Modified by Yurii Levashov 01/26/2005
 */

void cm6k6_abs_move(int dev_ID, int axis, double acc, double vel, double pos)
{
	/* Declare variables */
	int dev_open, err;
	double fin_pos;
	
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM6K6_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	err = cm6k6_error_handling(dev_ID, msg);
    	return;
    }
    if (axis < 1 || axis > CM6K6_NUM_AXIS)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	err = cm6k6_error_handling(axis, msg);
    	return;
    }
    if (acc < 0.001 || acc > 999.999)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	if(cm6k6_error_handling(acc, msg) == 1) return;
    	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);
    	if(cm6k6_error_handling(vel, msg) == 1) return;
    	if (vel < 0.1) vel = 0.1; else vel = 99.;
    }
    
    /* Check to make sure the device is open */
    dev_open = cm6k6_check_dev_open(dev_ID);
    if (dev_open != 1)
     if(cm6k6_error_handling(dev_open, "Open device. Device not open") == 1) return;
    
    /* Set absolute-preset mode */
    Fmt(cmd, "%s<%i[w1]MA1", axis);  // Move to absolute position
    cm6k6_out(dev_ID, cmd);
    Fmt(cmd, "%s<%i[w1]MC0", axis);   //move at specific distance
    cm6k6_out(dev_ID, cmd);
    
    /* Set the acceleration */
    Fmt(cmd, "%s<%i[w1]A%f", axis, acc);
    cm6k6_out(dev_ID, cmd);
    
    /* Set the velocity */
    Fmt(cmd, "%s<%i[w1]V%f", axis, vel);
    cm6k6_out(dev_ID, cmd);
    
    /* Set the position */
    Fmt(cmd, "%s<%i[w1]D%f", axis, pos);
    cm6k6_out(dev_ID, cmd);
    
   
    /* Move */
    if (axis ==1 )
    {
    Fmt(cmd, "%s<GO100000");
    }
     if (axis == 2)
    {
    Fmt(cmd, "%s<GO010000");
    }
     if (axis == 3)
    {
    Fmt(cmd, "%s<GO001000");
    }
     if (axis == 4)
    {
    Fmt(cmd, "%s<GO000100");
    }
    if (axis == 5)
    {
    Fmt(cmd, "%s<GO000010");
    }
     if (axis == 6)
    {
    Fmt(cmd, "%s<GO000001");
    }
    cm6k6_out(dev_ID, cmd);
    
   /* Wait until the move is complete */
	cm6k6_ifmove_delay (dev_ID, axis);
    
    /* Ask for the final position */
    Fmt(cmd, "%s<%i[w1]TPC", axis);
    cm6k6_out(dev_ID, cmd);
    cm6k6_in(dev_ID, cmd);
    CopyString(cmd,0, cmd,5,20);
    Scan(cmd, "%s>%f", &fin_pos);
    
    /* Make sure the proper position was achieved */
    err = (int) fin_pos - (int) pos;
    if (err != 0)
     if(cm6k6_error_handling(err, "Positioning") == 1) return;

    /* Check for command errors */
    cm6k6_read_error_status(dev_ID);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm6k6_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 6)
 *	acc, acceleration (rev/sec/sec)
 *	vel, velocity (rev/sec)
 *
 *	Zachary Wolf
 *	9/4/02
 *  Modified by Yurii Levashov 01/26/2005
 */

void cm6k6_go_home(int dev_ID, int axis, double acc, double vel)
{
	/* Declare variables */
	int dev_open, err;
	long int fin_pos;
	
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM6K6_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	err = cm6k6_error_handling(dev_ID, msg);
    	return;
    }
    if (axis < 1 || axis > CM6K6_NUM_AXIS)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	err = cm6k6_error_handling(axis, msg);
    	return;
    }
    if (acc < 0.001 || acc > 999.999)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	err = cm6k6_error_handling(acc, 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);
    	err = cm6k6_error_handling(vel, msg);
    	if (vel < 0.1) vel = 0.1; else vel = 99.;
    }
    
    /* Check to make sure the device is open */
    dev_open = cm6k6_check_dev_open(dev_ID);
    if (dev_open != 1)
     if(cm6k6_error_handling(dev_open, "Open device. Device not open") == 1) return;

/* Use only the home switch to define the home position */
/*    Fmt(cmd, "%s<%i[w1]HOMEBAC", axis);
    cm6k6_out(dev_ID, cmd);		 */
    
    /* Define the active state of the home switch as a low signal */
    Fmt(cmd, "%s<%i[w1]LIMLVL0", axis);
    cm6k6_out(dev_ID, cmd);
    
    /* Use the CCW side of the home switch */
    Fmt(cmd, "%s<%i[w1]HOMEDG0", axis);
    cm6k6_out(dev_ID, cmd);
    
    /* Set the final homing direction to CCW */
    Fmt(cmd, "%s<%i[w1]HOMDF0", axis);
    cm6k6_out(dev_ID, cmd);
    
    /* Back up to the home switch */
    Fmt(cmd, "%s<%i[w1]HOMEBAC", axis);
    cm6k6_out(dev_ID, cmd);																																																																																												                                                                                                       
    
    /* Set the acceleration */
    Fmt(cmd, "%s<%i[w1]HOMA%f[p2]", axis, acc);
    cm6k6_out(dev_ID, cmd);
    					                                                                                                                                                                                                                                                                                                                                                                                                                                                   
    /* Set the deceleration */
    Fmt(cmd, "%s<%i[w1]HOMAD%f[p2]", axis, acc);
    cm6k6_out(dev_ID, cmd);

    /* Move with the specified velocity */
    Fmt(cmd, "%s<%i[w1]HOMV%f[p3]", axis, vel);
    cm6k6_out(dev_ID, cmd);
    
    /* Set the final home velocity */
    Fmt(cmd, "%s<%i[w1]HOMVF%f[p3]", axis, vel/10.);
    cm6k6_out(dev_ID, cmd);
    
   /* Wait until the move is complete */
	err = cm6k6_ifmove_delay (dev_ID, axis);
    
    /* Ask for the position */
    Fmt(cmd, "%s<%i[w1]TPC", axis);
    cm6k6_out(dev_ID, cmd);
    cm6k6_in(dev_ID, cmd);
    CopyString(cmd,0, cmd,6,20);
    Scan(cmd, "%s>%i",&fin_pos);
    
    /* Check for command errors */
    cm6k6_read_error_status(dev_ID);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm6k6_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 (revs), can be + or -
 *
 *	Zachary Wolf
 *	9/9/02
 *  Modified by Yurii Levashov 01/26/2005
 */

void cm6k6_get_mot_position(int dev_ID, int axis, double* pos)
{
	/* Declare variables */
	int dev_open;
	int err;
	
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM6K6_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm6k6_error(msg);
    	return;
    }
    if (axis < 1 || axis > CM6K6_NUM_AXIS)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm6k6_error(msg);
    	return;
    }
    
    /* Check to make sure the device is open */
    dev_open = cm6k6_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm6k6_error("Device not open");
    	return;
    }
    
   /* Wait until the move is complete */
	err = cm6k6_ifmove_delay (dev_ID, axis);
    
    /* Get position commanded */
    Fmt(cmd, "%s<%i[w1]TPC", axis);
    cm6k6_out(dev_ID, cmd);
    cm6k6_in(dev_ID, cmd);
    CopyString(cmd,0, cmd,5,20);
    Scan(cmd, "%s>%f", pos);
    
    /* Check for command errors */
    cm6k6_read_error_status(dev_ID);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm6k6_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
 *  Modified by Yurii Levashov 01/26/2005
 */

void cm6k6_get_enc_position(int dev_ID, int axis, double* pos)
{
	/* Declare variables */
	int dev_open;
	double cts;
	long int cts_per_rev;
	double dum;
	int err;
	
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM6K6_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm6k6_error(msg);
    	return;
    }
    if (axis < 1 || axis > CM6K6_NUM_AXIS)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm6k6_error(msg);
    	return;
    }
    
    /* Check to make sure the device is open */
    dev_open = cm6k6_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm6k6_error("Device not open");
    	return;
    }
   
	/* get scaling factor */
	cm6k6_get_num_enc_cts_per_rev(axis, &cts_per_rev);
	
   /* Wait until the move is complete */
	err = cm6k6_ifmove_delay (dev_ID, axis);
	
    /* Ask for the current position in counts*/
    Fmt(cmd, "%s<%i[w1]TPE", axis);
    cm6k6_out(dev_ID, cmd);
    cm6k6_in(dev_ID, cmd);
    CopyString(cmd,0, cmd,5,20);
    Scan(cmd, "%s>%f",&cts);
    
    /* convert to position in revs */
    dum = cts / cts_per_rev;
    Fmt(cmd, "%s<%f", dum);
    Scan(cmd, "%s>%f", pos);
    
    /* Check for command errors */
    cm6k6_read_error_status(dev_ID);
    
    /* Done */
    return;
}


/*	**************************************************************  */
/*
 *	cm6k6_follow_move
 *  This function sets up the motor to follow the specified axis 
 *  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)
 *  follow, axis to follow (1 to 4)
 *	acc, acceleration (rev/sec/sec)
 *	vel, velocity (rev/sec)
 *	pos, position (rev), can be + or -
 *
 *	Kirsten Hacker
 *	9-14-04
 *  Modified by Yurii Levashov 01/26/2005
 */

void cm6k6_follow_move (int dev_ID, int master, int follow, double acc, double vel, double dis)
{
	/* Declare variables */
	int dev_open;
	int scale;
	long int fin_pos;
	long int steps_taken;
	
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM6K6_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm6k6_error(msg);
    	return;
    }
    if (master < 1 || master > CM6K6_NUM_AXIS)
    {
    	Fmt(msg, "%s<master = %i, out of range", master);
    	cm6k6_error(msg);
    	return;
    }
    if (acc < 0.001 || acc > 999.999)
    {
    	Fmt(msg, "%s<acc = %f, out of range, will be adjusted", acc);
    	cm6k6_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);
    	cm6k6_error(msg);
    	if (vel < 0.1) vel = 0.1; else vel = 99.;
    }
    if (master==follow)
    {
    	cm6k6_error("Master cannot also be the follower");
    }
    
    /* Check to make sure the device is open */
    dev_open = cm6k6_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm6k6_error("Device not open");
    	return;
    }

    /* Set which axis follows which master */
    Fmt(cmd, "%s<%i[w1]FOLMAS+%i[w1]1", follow, master);
    cm6k6_out(dev_ID, cmd);
    
    /* Enable follower mode */
    Fmt(cmd, "%s<%i[w1]FOLEN1", follow);
    cm6k6_out(dev_ID, cmd);
    
    /* Set incremental continuous mode for follower axis */
    Fmt(cmd, "%s<%i[w1]MA0", follow);
    cm6k6_out(dev_ID, cmd);
    Fmt(cmd, "%s<%i[w1]MC1", follow);
    cm6k6_out(dev_ID, cmd);
    
    /* Set incremental-preset mode for master axis */
    Fmt(cmd, "%s<%i[w1]MA0", master);
    cm6k6_out(dev_ID, cmd);
    Fmt(cmd, "%s<%i[w1]MC0", master);
    cm6k6_out(dev_ID, cmd);
    
    /* Set the acceleration for master axis */
    Fmt(cmd, "%s<%i[w1]A%f", master, acc);
    cm6k6_out(dev_ID, cmd);
    
    /* Set the velocity for master axis */
    Fmt(cmd, "%s<%i[w1]V%f", master, vel);
    cm6k6_out(dev_ID, cmd);
    
    /* Set the position for master axis */
    Fmt(cmd, "%s<%i[w1]D%f", master, dis);
    cm6k6_out(dev_ID, cmd);
    
    /* Move */
    if ((follow==1 && master==2) || (follow==2 && master==1))
    {
    	Fmt(cmd, "%s<GO110000");
    }
    else if ((follow==1 && master==3) || (follow==3 && master==1))
    {
    	Fmt(cmd, "%s<GO101000");
    }
    else if ((follow==1 && master==4) || (follow==4 && master==1))
    {
    	Fmt(cmd, "%s<GO100100");
    }
    else if ((follow==2 && master==3) || (follow==3 && master==2))
    {
    	Fmt(cmd, "%s<GO011000");
    }
    else if ((follow==3 && master==4) || (follow==4 && master==3))
    {
    	Fmt(cmd, "%s<GO001100");
    }
    else if ((follow==2 && master==4) || (follow==4 && master==2))
    {
    	Fmt(cmd, "%s<GO010100");
    }
    else
    {
    	cm6k6_error("This follower/master configuration has not been coded yet");
    }
    cm6k6_out(dev_ID, cmd);
    
   /* Wait until the move is complete */
    Delay(1 + fabs(dis / vel));
    
    /* Check for command errors */
    cm6k6_read_error_status(dev_ID);
    
    /* Disable follower mode */
    Fmt(cmd, "%s<!%i[w1]FOLEN0", follow);
    cm6k6_out(dev_ID, cmd);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm6k6_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
 *  Modified by Yurii Levashov 01/26/2005
 */

void cm6k6_zero(int dev_ID, int axis)
{
	/* Declare variables */
	int err;
	int dev_open;
	
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM6K6_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm6k6_error(msg);
    	return;
    }
    if (axis < 1 || axis > CM6K6_NUM_AXIS)
    {
    	Fmt(msg, "%s<axis = %i, out of range", axis);
    	cm6k6_error(msg);
    	return;
    }
    
    /* Check to make sure the device is open */
    dev_open = cm6k6_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	cm6k6_error("Device not open");
    	return;
    }

    /* Zero the position counter */
    if (axis==1)
    	{
    	Fmt(cmd, "%s<PSET0,,,,,");
    	}
    if (axis==2)
    	{
    	Fmt(cmd, "%s<PSET,0,,,,");
    	}
    if (axis==3)
    	{
    	Fmt(cmd, "%s<PSET,,0,,,");
    	}
    if (axis==4)
    	{
    	Fmt(cmd, "%s<PSET,,,0,,");
    	}
    if (axis==5)
    	{
    	Fmt(cmd, "%s<PSET,,,,0,");
    	}
    if (axis==6)
    	{
    	Fmt(cmd, "%s<PSET,,,,,0");
    	}	
    cm6k6_out(dev_ID, cmd);
    
    /* Check for errors */							
    err = cm6k6_read_error_status(dev_ID);
    if (err != 0)
    {
        cm6k6_error("The device has an error");
        cm6k6_close_dev(dev_ID);
        return;
    }
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	cm6k6_exit
 *  This function prepares the CM6K6 for shutdown and closes
 *	the device.
 *
 *	Input:
 *	dev_ID, device identifier
 *
 *	Zachary Wolf
 *	8/23/98
 *  Modified by Yurii Levashov 01/26/2005
 */

void cm6k6_exit(int dev_ID)
{
	/* Declare variables */
	int dev_open;
	int axis;
	
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
    	return;
    #endif

    /* Check input parameters */
    if (dev_ID < 1 || dev_ID > CM6K6_MAX_NUM_DEV)
    {
    	Fmt(msg, "%s<dev_ID = %i, out of range", dev_ID);
    	cm6k6_error(msg);
    	return;
    }
    
    
    /* Check to make sure the device is open */
    dev_open = cm6k6_check_dev_open(dev_ID);
    if (dev_open != 1)
    {
    	return;		 /* OK, already closed */
    }
    
    /* Leave the CM6K6 in the desired state */
    
    /* De-energize the drives */
    Fmt(cmd, "%s<DRIVE000000");
    cm6k6_out(dev_ID, cmd);

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

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

/*	PRIVATE FUNCTIONS  */

/*	**************************************************************  */
/*
 *	cm6k6_open_dev
 *	This function opens the device and returns an identifier for
 *	future references to the device.
 *
 *	Input:
 *	rs232_com_port, address of the COM port the device is connected to
 *	rs232_dev_addr, RS232 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
 *  Modified by Yurii Levashov 01/26/2005
 */

int cm6k6_open_dev(int rs232_dev_addr)
{
	/* Declare variables */
    int i;
    int dev_ID;
    int descr;
    int err;
    
    /* Initialization */
    dev_ID = 0;

    /* Check to see if the device is already in the device table */
    for (i = 1; i <= CM6K6_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 <= CM6K6_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)
    {
    	cm6k6_error("The device could not be opened");
        return  -1;
    }
    
    /* Get Description */
    descr=1;
    
    
	/* Open the device */
	if (descr <= 0)
	{
		cm6k6_error("The device could not be opened");
		return  -1;
	}
    
    /* Open successful, store results in the device table */
    dev_addr[dev_ID] = rs232_dev_addr;
    dev_descr[dev_ID] = descr;
    dev_count++;
    
    /* Return the device ID */
    return dev_ID;
}

/*	**************************************************************  */
/*
 *	cm6k6_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
 *  Modified by Yurii Levashov 01/26/2005
 */

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

/*
 *  cm6k6_check_port
 *  This function checks if a port for a device has closed, and
 *  opens the port if it was closed.
 *
 *	Input:
 *	rs232_com_port (1-32)
 *	rs232_dev_addr  (00-99) (00 is reserved)
 *
 *	Output:
 *	0 if the port was open, 1 if it was closed and fixed, -1 if an error occured
 *
 *	Michael Levashov
 */
int cm6k6_check_port(int rs232_com_port)
{
	int err;
	
	/* Check if the device is open */
	if((GetComConnectionState(rs232_com_port))<=0)
	{
		printf("RS232 port closed!  Attempting to re-open.\n");
		/* Open RS232 communications */
		err = OpenComConfig(com_port, "", 9600, 0, 8, 1, 512, 512);
		if (err != 0)
			if(cm6k6_error_handling(err, "OpenCom") == 1) return -1;
		printf("Port re-opened successfully.\n");
		
		return 1;
	}
	else
		return 0;
}
	 
/*	**************************************************************  */
/*
 *	cm6k6_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
 *  Modified by Yurii Levashov 01/26/2005
 */

int cm6k6_out(int dev_ID, char* buf)
{
	/* Declare variables */
	char out_buf[CM6K6_MAX_CMD];
	int nbytes;
	int err;
	
	/* Check that the port is still open */
	//err=cm6k6_check_port(com_port);
	//if (err == -1)
	//	if(cm6k6_error_handling(err, "Check Port") == 1) return -1;
	
	/* 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<%s%c%c", 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);
		cm6k6_error(msg);
		return -1;
	}
	
	/* Allow time for the command to be processed */
	Delay(.6);
	
	/* Done */
    return 0;
}

/*	**************************************************************  */
/*
 *	cm6k6_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
 *  Modified by Yurii Levashov 01/26/2005
 */

int cm6k6_in(int dev_ID, char* buf)
{
	/* Declare variables */
	int nbytes;
	char in_buf[CM6K6_MAX_CMD];
					
	/* Read the message, CR and LF are discarded */
	nbytes = ComRdTerm(com_port, in_buf, CM6K6_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);
		cm6k6_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;
}

/*	**************************************************************  */
/*
 *	cm6k6_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
 *  Modified by Yurii Levashov 01/26/2005
 */

int cm6k6_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 */
	}
}

/*	**************************************************************  */
/*
 *	cm6k6_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
 *  Modified by Yurii Levashov 01/26/2005
 */

int cm6k6_read_error_status(int dev_ID) 
{
	/* Declare variables */
	int axis;
	char status[CM6K6_MAX_CMD];
	
    /* Request the controller status */
    for (axis = 1; axis <= 4; axis++)
    {
    	Fmt(cmd, "%s<TCMDER");
    	cm6k6_out(dev_ID, cmd);
    	cm6k6_in(dev_ID, cmd);
    	if (strcmp(cmd, "*NO ERRORS") != 0)
    	{
    		cm6k6_message(cmd);
    		cm6k6_error("Error executing command");
    		return -1;
    	}
    }
    
    /* Done */
    return 0;
}

/*	**************************************************************  */
/*
 *	cm6k6_get_num_mot_steps_per_rev
 *  This function gets the motor's number of steps per revolution.
 *
 *	Input:
 *	axis, axis number (1 to 6)
 *
 *	Output:
 *	num_steps_per_rev, number of steps per revolution
 *
 *	Zachary Wolf
 *	9/9/02
 *  Modified by Yurii Levashov 01/26/2005
 */

void cm6k6_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 = CM6K6_NUM_MOT_STEPS_PER_REV_AXIS1;
	else if (axis == 2) *num_steps_per_rev = CM6K6_NUM_MOT_STEPS_PER_REV_AXIS2;
	else if (axis == 3) *num_steps_per_rev = CM6K6_NUM_MOT_STEPS_PER_REV_AXIS3;
	else if (axis == 4) *num_steps_per_rev = CM6K6_NUM_MOT_STEPS_PER_REV_AXIS4;
	else if (axis == 5) *num_steps_per_rev = CM6K6_NUM_MOT_STEPS_PER_REV_AXIS5;
	else if (axis == 6) *num_steps_per_rev = CM6K6_NUM_MOT_STEPS_PER_REV_AXIS6;
	else cm6k6_error("Unknown axis number");
	
	/* Done */
	return;
}

/*	**************************************************************  */
/*
 *	cm6k6_get_num_enc_cts_per_rev
 *  This function gets the encoder's number of steps per revolution.
 *
 *	Input:
 *	axis, axis number (1 to 6)
 *
 *	Output:
 *	num_steps_per_rev, number of steps per revolution
 *
 *	Zachary Wolf
 *	9/9/02
 *  Modified by Yurii Levashov 01/26/2005
 */

void cm6k6_get_num_enc_cts_per_rev(int axis, long int* num_cts_per_rev)
{
	/* Determine the number of steps per revolutuon */
	if      (axis == 1) *num_cts_per_rev = CM6K6_NUM_ENC_STEPS_PER_REV_AXIS1;
	else if (axis == 2) *num_cts_per_rev = CM6K6_NUM_ENC_STEPS_PER_REV_AXIS2;
	else if (axis == 3) *num_cts_per_rev = CM6K6_NUM_ENC_STEPS_PER_REV_AXIS3;
	else if (axis == 4) *num_cts_per_rev = CM6K6_NUM_ENC_STEPS_PER_REV_AXIS4;
	else if (axis == 5) *num_cts_per_rev = CM6K6_NUM_ENC_STEPS_PER_REV_AXIS5;
	else if (axis == 6) *num_cts_per_rev = CM6K6_NUM_ENC_STEPS_PER_REV_AXIS6;
	else cm6k6_error("Unknown axis number");
	
	/* Done */
	return;
}

/*	**************************************************************  */
/*
 *	cm6k6_message
 *  This function handles messages about the CM6K6.
 *
 *	Input:
 *	message, string to display in standard I/O
 *
 *	Zachary Wolf
 *	9/9/02
 *  Modified by Yurii Levashov 01/26/2005
 */

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

/*	**************************************************************  */
/*
 *	cm6k6_error
 *  This function handles error messages for the CM6K6.
 *
 *	Input:
 *	message, string to display in standard I/O
 *
 *	Zachary Wolf
 *	9/9/02
 *  Modified by Yurii Levashov 01/26/2005
 */

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

/*	**************************************************************  */
/*
 *	cm6k6_ifmove_delay
 *  This function waits while the move stops.
 *
 *  Input: Device ID
 *
 *
 *	Yurii Levashov
 *	10/09/2004
 *
 */
int cm6k6_ifmove_delay (int dev_ID, int axis)
{
	/* declare variables */
	char st[] = "NOT";
	int err;
	int ID;
  
	Fmt(cmd,"%s<WAIT(MOV=b000000)");  //wait, until the movement stops
	cm6k6_out(dev_ID, cmd);
	while(st[0] != 'Y')				 
	{
		Fmt(cmd,"%s<WRITE\"YES\"");		  // write YES into COM1 buffer
		cm6k6_out(dev_ID, cmd);
	    Delay(0.2);
	    err = ComRdTerm(com_port, cmd, CM6K6_MAX_CMD, 13);
	    CopyString(st,0,cmd,0,3);		 // Read first 3 bites
		err = FlushOutQ(com_port);		 // Flush COM1 buffer to avoid overfilling
		if (err != 0) printf("FlushOutQ err = %i\n", err);
	}
	
	return 0;
}
/*	**************************************************************  */  

/* 	cm6k6_error_handling
 *
 *  This function inform operator about error and error codes 
 *
 *	Yurii Levashov
 *	09/10/2004
*/
int cm6k6_error_handling(int err, char descr[])
{
	char msg[80];
   
	Fmt(msg, "%s<Error in %s. Check error code # %i. Continue?", descr, err); 
	err = ConfirmPopup ("ERROR", msg);
	if (err == 0)  return 1;     
	 
	return 0;
}

 /*	**************************************************************  
 *	cm6k6_stop_motor
 *  This function stops the motoin on a specified axis
 *
 *	Input:
 *	dev_ID, device identifier
 *  axis 
 *
 *	Yurii Levashov
 *	03/16/2005
 */

void cm6k6_stop_motor(int dev_ID, int axis) 
{
    // prepare axis list 
    	cm6k6_axis(axis, msg);
    
    // Send immediate STOP command 
    	Fmt(cmd, "%s<!K%s", msg);
    	cm6k6_out(dev_ID, cmd);
 
    // Done 
    return;
} 
  
 /*	**************************************************************  
 *	cm6k6_axis
 *  This function makes axis list for motion controller commands
 *
 *	Input:
 *  int axis 
 *
 *
 *  Output: axis list - msg
 *
 *	Yurii Levashov
 *	03/16/2005
 */

void cm6k6_axis(int axis, char list[]) 
{
   /* Zero the position counter */
    if (axis==1) strcpy (list, "1,0,0,0,0,0");
    
    if (axis==2) strcpy (list, "0,1,0,0,0,0");
    	 
    if (axis==3) strcpy (list, "0,0,1,0,0,0");
    	 
    if (axis==4) strcpy (list, "0,0,0,1,0,0");
    	
    if (axis==5) strcpy (list, "0,0,0,0,1,0");
    	 
    if (axis==6) strcpy (list, "0,0,0,0,0,1");
    
    /* Done */
 return;
} 

/*	**************************************************************  */
/*
 *  cm6k6_init
 *  This function opens the device, queries for ID, and
 *  initializes the device to a known state.
 *
 *	Input:
 *	rs232_com_port (1-32)
 *	rs232_dev_addr  (00-99) (00 is reserved)
 *
 *	Output:
 *	ID, identifier for future references to the device
 *
 *	Zachary Wolf
 *	9/4/02
 *  Modified by Yurii Levashov 01/26/2005
 *  Modified by Michael Levashov 07/02/2007
 */

void cm6k6_cam_init(int rs232_com_port, int rs232_dev_addr, 
											int* ID, struct move_param param)
{
	/* Declare variables */
	int dev_ID;
	int err;
	int mr;
	int er;
	int axis;
	long int mres[7];
	long int eres[7];
	long int scale[7];
	double acc;
	int limit;
    
	/* Exit here if testing without hardware */
	#ifdef DUMMY_DEVICES
		*ID = 1;
		return;
	#endif
    
	/* Check input parameters */
	if(rs232_com_port < 1 || rs232_com_port > 32)
		if(cm6k6_error_handling(rs232_com_port, "RS232_COM_PORT") == 1) return;
         
	if(rs232_dev_addr < 0 || rs232_dev_addr > 99)
		if(cm6k6_error_handling(rs232_dev_addr, "RS232_DEV_ADDR") == 1) return;
    
	/* Save the com port number */
	com_port = rs232_com_port;
    
	/* Open RS232 communications */
	err = OpenComConfig(com_port, "", 9600, 0, 8, 1, 512, 512);
	if (err != 0)
		if(cm6k6_error_handling(err, "OpenCom") == 1) return;
	 
	err = FlushInQ(com_port);
	if (err != 0)
		if(cm6k6_error_handling(err, "FlushInQ") == 1) return; 
	
	err = FlushOutQ(com_port);
	if (err != 0)
		if(cm6k6_error_handling(err, "FlushOutQ") == 1) return;
    
	/* Open the CM6K6 */
	dev_ID = cm6k6_open_dev(rs232_dev_addr);
	if (dev_ID <= 0)
		if(cm6k6_error_handling(dev_ID, "RS232.Device was not opened") == 1) return;
    
	/* Reset */
	cm6k6_out(dev_ID, "!RESET");
	Delay(5);

	/* set COM1 for command transmission */
	cm6k6_out(dev_ID, "PORT1");
    
	/* Enable communication */
	cm6k6_out(dev_ID, "E1");
     
	/* Disable Echo */
	cm6k6_out(dev_ID, "ECHO0");
    
	/* set responce field without command itself and error */
 //   cm6k6_out(dev_ID, "ERRLVL0");
    
	/* Make sure start-up program doesn't execute */
	cm6k6_out(dev_ID, "STARTP CLR");
    
	/* Define stepper motor axis types */
//    cm6k6_out(dev_ID, "DRIVE000000");   // De-Energize the motor drives
	cm6k6_out(dev_ID, "AXSDEF000000");  // 0 - for stepper motors
    
	/* Define position limits settings */
	cm6k6_out(dev_ID, "LS0,0,0,0,0,0");   // Programmable limits disabled 
//	GetCtrlVal(panel_ID, PANEL_LIMITS_DISABLE, &limit);
//    if(limit == 1)
	cm6k6_out(dev_ID, "LH0,0,0,0,0,0");   // End-of-travel limits disabled
//       else
//         {
//		  err = MessagePopup ("WARNING",
//							  "Please make sure limit switches are connected");
//          cm6k6_out(dev_ID, "LH3,3,3,3,3,3");   // End-of-travel limits enabled
	cm6k6_out(dev_ID, "LIMLVL0");			// Set signal level to low
//          GetCtrlVal (PANEL, PANEL_NUMERIC_ACCELERATION, &acc);
          
	Fmt(cmd, "%s<@LHAD%f", ACC * 10);  //Set hard limit deceleration value 10 times higher
	cm6k6_out(dev_ID, cmd);
//	     }
	     
    // Disable drive fault checking 
	cm6k6_out(dev_ID, "DRFEN000000");
    
    // Enable error detection 
//    cm6k6_out(dev_ID, "ERROR111111");
    
    // Enable encoder ports 
	cm6k6_out(dev_ID, "ENCCNT111111");	  // Reference to the encoder position

    // Set the motor resolutions and follower scaling in revs
	for (axis = 1; axis <= CM6K6_NUM_AXIS ; axis++)
	{
		cm6k6_get_num_mot_steps_per_rev(axis, &mr);
		mres[axis]=mr;
	}
    
	// Enable Scaling 
	cm6k6_out(dev_ID, "SCALE1");
    
	Fmt(cmd, "%s<DRES%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0]",mres[1], mres[2], mres[3], mres[4], mres[5], mres[6]);
	cm6k6_out(dev_ID, cmd);
	Fmt(cmd, "%s<SCLD%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0]",mres[1], mres[2], mres[3], mres[4], mres[5], mres[6]);
	cm6k6_out(dev_ID, cmd);
	Fmt(cmd, "%s<SCLV%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0]",mres[1], mres[2], mres[3], mres[4], mres[5], mres[6]);
	cm6k6_out(dev_ID, cmd);
	Fmt(cmd, "%s<SCLA%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0]",mres[1], mres[2], mres[3], mres[4], mres[5], mres[6]);
	cm6k6_out(dev_ID, cmd);

	// Set the encoder resolution and master scaling in revs
	for (axis = 1; axis <= CM6K6_NUM_AXIS ; axis++)
	{
		cm6k6_get_num_enc_cts_per_rev(axis, &er);
		eres[axis]=er;
	}
	Fmt(cmd, "%s<ERES%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0]",eres[1], eres[2], eres[3], eres[4], eres[5], eres[6]);
	cm6k6_out(dev_ID, cmd);
	Fmt(cmd, "%s<SCLMAS%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0],%i[w5p0]",eres[1], eres[2], eres[3], eres[4], eres[5], eres[6]);
	cm6k6_out(dev_ID, cmd);
   	 
    // Set follower:master ratio 1:1 
    cm6k6_out(dev_ID, "FOLRN1,1,1,1,1,1");
    cm6k6_out(dev_ID, "FOLRD1,1,1,1,1,1");
    
    // Energize the drives 
    Fmt(cmd, "%s<DRIVE111111");
    cm6k6_out(dev_ID, cmd);
    
    // Set the present follower position to zero
    Fmt(cmd, "%s<PSET000000");
    cm6k6_out(dev_ID, cmd);

    // Allow commands during motion
    cm6k6_out(dev_ID, "COMEXC1");
    
    // Set incremental-preset mode 
    Fmt(cmd, "%s<%i[w1]MA0", param.axis);	   // Move wrt current position
    cm6k6_out(dev_ID, cmd);
    Fmt(cmd, "%s<%i[w1]MC0", param.axis);	   //to go a specific distance
    cm6k6_out(dev_ID, cmd);
    
    // Set the acceleration 
    Fmt(cmd, "%s<%i[w1]A%f", param.axis, param.accel);
    cm6k6_out(dev_ID, cmd);
    
    // Set average acceleration 50% of acceleration to make S-curve 
    Fmt(cmd, "%s<%i[w1]AA%f", param.axis, param.accel / 2.);
    cm6k6_out(dev_ID, cmd);
    
    // Set max deceleration  
    Fmt(cmd, "%s<%i[w1]AD%f", param.axis, param.accel);
    cm6k6_out(dev_ID, cmd);
    
    // Set average deceleration 50% of acceleration to make S-curve 
    Fmt(cmd, "%s<%i[w1]ADA%f", param.axis, param.accel / 2.);
    cm6k6_out(dev_ID, cmd);
    
    // Set the velocity 
    Fmt(cmd, "%s<%i[w1]V%f", param.axis, param.vel);
    cm6k6_out(dev_ID, cmd);
    
    cm6k6_out(dev_ID, "DRIVE000000");   // De-Energize the motor drives 
    
    // Get a response from the CM6K6
    cm6k6_read_error_status(dev_ID);

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

/*	**************************************************************  */
/*
 *	cm6k6_cam_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 6)
 *	acc, acceleration (rev/sec/sec)
 *	vel, velocity (rev/sec)
 *	dis, distance (rev), can be + or -
 *
 *	Zachary Wolf
 *	9/4/02
 *  Modified by Yurii Levashov 01/26/2005
 */

int cm6k6_cam_move(int panel, int control, int dev_ID, int axis, double step_size, int led_flag)
{
	/* Declare variables */
	int dev_open, err = 0;
	char st[] = "NOT";
	double ini_pos;
	double fin_pos;
	double revs_taken;
	int ID;
	
    // Ask for the initial position 
    Fmt(cmd, "%s<%i[w1]TPC", axis);
    cm6k6_out(dev_ID, cmd);
    cm6k6_in(dev_ID, cmd);
    CopyString(cmd,0, cmd,5,20);
    Scan(cmd, "%s>%f", &ini_pos);
    
    // Format the go command
    if (axis==1)        Fmt(msg, "%s<100000");
     else if (axis==2)  Fmt(msg, "%s<010000");
     else if (axis==3)  Fmt(msg, "%s<001000");
     else if (axis==4)  Fmt(msg, "%s<000100");
     else if (axis==5)  Fmt(msg, "%s<000010");
     else if (axis==6)  Fmt(msg, "%s<000001");
     else  cm6k6_error_handling(err, "wrong axis number");
     
    // Energize motor driver
    Fmt(cmd, "%s<DRIVE%s", msg);
	cm6k6_out(dev_ID, cmd);
	Delay(2.0);
	
    // Set the distance 
    Fmt(cmd, "%s<%i[w1]D%f%cGO%s", axis, step_size, 13, msg); // positive step_size moves motor clockwise
    cm6k6_out(dev_ID, cmd);
    
    // Turn ON LED
    if(led_flag == 1)
    {
    	SetCtrlAttribute (panel, control, ATTR_CTRL_VAL, 1);
 		DisplayPanel (panel);
 	}
	// Wait until the move is complete 
	err = cm6k6_ifmove_delay (dev_ID, axis);
	
	// De-Energize motor drivers
	cm6k6_out(dev_ID, "DRIVE000000");
	Delay(1.0);
	
	// Turn OFF LED
	if(led_flag == 1)
	{
    	SetCtrlAttribute (panel, control, ATTR_CTRL_VAL, 0);
 		DisplayPanel (panel);
 	}
	  if(err != 0) return err;
	  
   // Ask for the final position  
    Fmt(cmd, "%s<%i[w1]TPC", axis);
    cm6k6_out(dev_ID, cmd);
    cm6k6_in(dev_ID, cmd);
    CopyString(cmd,0, cmd,5,20);
    Scan(cmd, "%s>%f", &fin_pos);
    
    /* Caclulate number of steps taken */
    revs_taken = (fin_pos - ini_pos);
    
    /* Make sure the proper position was achieved */  
    err = (int) (fabs(step_size) - fabs(revs_taken));
    if (err != 0)
     	if(cm6k6_error_handling(err, "Positioning") == 1) 
     	{
      		cm6k6_stop_motor(dev_ID, axis);
      
      		// De - Energize motor driver
      		cm6k6_out(dev_ID, "DRIVE000000");
      		return -1;
     	}

    /* Check for command errors */
//    cm6k6_read_error_status(dev_ID);
    
    /* Done */
    return 0;
}

/*	**************************************************************  */
/*
 *	cm6k6_cam_move_5
 *  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 6)
 *	acc, acceleration (rev/sec/sec)
 *	vel, velocity (rev/sec)
 *	dis, distance (rev), can be + or -
 *
 *	Zachary Wolf
 *	9/4/02
 *  Modified by Yurii Levashov 01/26/2005
 *  Modified by Michael Levashov 07/02/2007
 */

int cm6k6_cam_move_5(int panel, int control[], int dev_ID, double distance[], int led_flag)
{
	/* Declare variables */
	int dev_open, err = 0;
	char st[] = "NOT";
	double ini_pos[5];
	double fin_pos[5];
	double revs_taken;
	int ID;
	int i;
	
    for(i = 0; i < 5; i++)
    {
    	// Ask for the initial position 
    	Fmt(cmd, "%s<%i[w1]TPC", i + 1);
    	cm6k6_out(dev_ID, cmd);
    	cm6k6_in(dev_ID, cmd);
    	CopyString(cmd,0, cmd,5,20);
    	Scan(cmd, "%s>%f", &ini_pos[i]);
    
    	// Set the distance for each axis
    	Fmt(cmd, "%s<%i[w1]D%f[p10]", i + 1, distance[i]); // positive step_size moves motor clockwise
    	cm6k6_out(dev_ID, cmd);
    
    	//turn ON LED's
    	if(led_flag == 1)
    		SetCtrlAttribute (panel, control[i + 5], ATTR_CTRL_VAL, 1); 
    }
    if(led_flag == 1)
    	DisplayPanel (panel);
    
    // Energize motor drivers
	cm6k6_out(dev_ID, "DRIVE111110");
	Delay(2.0);
	
	// move 5 motors 
	cm6k6_out(dev_ID,"GO111110");
	
    // Wait until the move is complete 
	err = cm6k6_ifmove_delay (dev_ID, i + 1);
	
	Delay(1.0);
	// De-Energize motor drivers
	cm6k6_out(dev_ID, "DRIVE000000");;
	
 	for(i = 0; i < 5; i++)
 	{
		// Turn OFF LED
    	if(led_flag == 1)
    		SetCtrlAttribute (panel, control[i + 5], ATTR_CTRL_VAL, 0);
 	
 		Delay(1.0);
 		
   		// Ask for the final position  
    	Fmt(cmd, "%s<%i[w1]TPC", i + 1);
    	cm6k6_out(dev_ID, cmd);
    	cm6k6_in(dev_ID, cmd);
    	CopyString(cmd,0, cmd,5,20);
    	Scan(cmd, "%s>%f", &fin_pos[i]);
    
		// Caclulate number of steps taken
    	revs_taken = (fin_pos[i] - ini_pos[i]);
    
    	/* Make sure the proper position was achieved */  
    	err = (int) (fabs(distance[i]) - fabs(revs_taken));
    	if (err != 0)
     	 if(cm6k6_error_handling(err, "Positioning") == 1) 
     	 {
      		cm6k6_stop_motor(dev_ID, i + 1);
      
      		return -1;
     	 }
	}
	if(led_flag == 1)
		DisplayPanel (panel);
	
    /* Check for command errors */
//    cm6k6_read_error_status(dev_ID);
    
    /* Done */
    return 0;
}