// seek.c - seek frames using MAX-420 Stepper Motor Controller
// erco@3dsite.com 1.00 10/01/01
// erco@3dsite.com 1.10 ??/??/01 - mods for seek -l
// erco@3dsite.com 1.20 02/27/02 - check for ReadIntComm() errors
//

#include <iostream.h>
#include <string.h>
#include <windows.h>
#include <stdio.h>
#include <math.h>
#include "error.h"

#define VERSION "1.20"		// CHANGE THIS BY HAND!

#define SYSERR 		GetErrorMessage()
#define ISIT(s)		if ( strcmp(argv[t], s) == 0 )

// CHANNELS
#define A		0
#define B		1

// DEFAULTS
//    These are loaded from the defaults file,
//    but can also be overridden on the command line.
//
class Defs
{
public:
    char comdev[80];
    HANDLE com;

    int debug;
    int verbose;
    int motors;

    // MOTOR DEFAULTS
    float homespeed[2];		// F <homespeed> <homedir>
    float homedir[2];		// F <homespeed> <homedir>
    float speed[2];		// V <speed>
    float startstopspeed[2];	// I <speed>
    float ppr[2];		// used as multiplier
    float ramp[2];		// K <ramp> <ramp>
    int   dirinvert[2];		// used as multiplier (1 or -1)
    int   loadsteps[2];		// number of steps to move to load position

    Defs()
    {
        // INIT
        strcpy(comdev, "com1");
	com = INVALID_HANDLE_VALUE;

	debug = 0;
	verbose = 0;
	motors = 1;

	homespeed[A] = .25;
	homespeed[B] = .25;

	speed[A] = .25;
	speed[B] = .25;

	startstopspeed[A] = .25;
	startstopspeed[B] = .25;

	ppr[A] = 200;
	ppr[B] = 200;

	ramp[A] = 10;
	ramp[B] = 10;

	dirinvert[A] = 1;
	dirinvert[B] = 1;

	loadsteps[A] = 50;
	loadsteps[B] = 50;
    }
};

// GLOBALS
Defs G_defs;

// CONVERT A CHANNEL LETTER TO AN INDEX VALUE
//     Use the index value for arrays.
//
int Chan2Index(char c)
{
    switch ( c )
    {
        case 'a':
	case 'A': return(0);

        case 'b':
	case 'B': return(1);

	default:
	    cerr << "seek: invalid channel '" << c << "'" << endl;
	    exit(1);
    }
}

// CONVERT AN INDEX TO A CHANNEL LETTER
char Index2Chan(int i)
{
    if ( i == 0 ) return('A');
    if ( i == 1 ) return('B');
    cerr << "seek: invalid channel number '" << i << "'" << endl;
    exit(1);
}

// REMOVE CRLFS FROM STRING
void NoCRLF(char *s)
{
    char *ss;
    if ( ( ss = strchr(s, '\n') ) ) *ss = '\0';
    if ( ( ss = strchr(s, '\r') ) ) *ss = '\0';
    return;
}

// LOAD THE DEFAULTS FILE
void LoadDefs(const char* filename)
{
    FILE *fp = fopen(filename, "r");
    if ( fp == NULL )
        { cerr << "seek: " << filename << ": " << SYSERR; exit(1); }
    char s[1024];
    int line = 0;
    while ( fgets(s, sizeof(s)-1, fp) )
    {
        ++line;
        if ( s[0] == '\n' || s[0] == 0 || s[0] == '#' ) continue;

	/************************************
	char comdev[80];
	HANDLE com;
	int debug;
	int verbose;
	float homespeed[2];
	float homedir[2];
	float speed[2];
	float startstopspeed[2];
	float ppr[2];
	float ramp[2];
	int   dirinvert[2];
	int   loadsteps[2];
	*************************************/

	NoCRLF(s);
	char c;
	float fval;
	int ival;
	if ( sscanf(s, "comdev %79s", G_defs.comdev  ) == 1 ) { continue; }
	if ( sscanf(s, "debug %d",   &G_defs.debug   ) == 1 ) { continue; }
	if ( sscanf(s, "verbose %d", &G_defs.verbose ) == 1 ) { continue; }
	if ( sscanf(s, "motors %d",  &G_defs.motors  ) == 1 ) { continue; }
	if ( sscanf(s, "homespeed %c %f", &c, &fval) == 2 )
	    { int i = Chan2Index(c); G_defs.homespeed[i] = fval; continue; }
	if ( sscanf(s, "homedir %c %f", &c, &fval) == 2 )
	    { int i = Chan2Index(c); G_defs.homedir[i] = fval; continue; }
	if ( sscanf(s, "speed %c %f", &c, &fval) == 2 )
	    { int i = Chan2Index(c); G_defs.speed[i] = fval; continue; }
	if ( sscanf(s, "startstopspeed %c %f", &c, &fval) == 2 )
	    { int i = Chan2Index(c); G_defs.startstopspeed[i] = fval; continue; }
	if ( sscanf(s, "ppr %c %f", &c, &fval) == 2 )
	    { int i = Chan2Index(c); G_defs.ppr[i] = fval; continue; }
	if ( sscanf(s, "ramp %c %f", &c, &fval) == 2 )
	    { int i = Chan2Index(c); G_defs.ramp[i] = fval; continue; }
	if ( sscanf(s, "dirinvert %c %d", &c, &ival) == 2 )
	    { int i = Chan2Index(c); G_defs.dirinvert[i] = ival; continue; }
	if ( sscanf(s, "loadsteps %c %d", &c, &ival) == 2 )
	    { int i = Chan2Index(c); G_defs.loadsteps[i] = ival; continue; }

	cerr << "seek: " << filename 
	     << " (Line " << line << "): unknown command '" << s << "'" << endl;
	exit(1);
    }
    fclose(fp);

    if ( G_defs.debug )
    {
        cerr << "--- Loaded Defaults:\n"
	     << "            comdev=" << G_defs.comdev << "\n"
	     << "             debug=" << G_defs.debug << "\n"
	     << "           verbose=" << G_defs.verbose << "\n"
	     << "            motors=" << G_defs.motors << "\n"
	     << "      homespeed[A]=" << G_defs.homespeed[A] << "\n"
	     << "      homespeed[B]=" << G_defs.homespeed[B] << "\n"
	     << "        homedir[A]=" << G_defs.homedir[A] << "\n"
	     << "        homedir[B]=" << G_defs.homedir[B] << "\n"
	     << "          speed[A]=" << G_defs.speed[A] << "\n"
	     << "          speed[B]=" << G_defs.speed[B] << "\n"
	     << " startstopspeed[A]=" << G_defs.startstopspeed[A] << "\n"
	     << " startstopspeed[B]=" << G_defs.startstopspeed[B] << "\n"
	     << "            ppr[A]=" << G_defs.ppr[A] << "\n"
	     << "            ppr[B]=" << G_defs.ppr[B] << "\n"
	     << "           ramp[A]=" << G_defs.ramp[A] << "\n"
	     << "           ramp[B]=" << G_defs.ramp[B] << "\n"
	     << "      dirinvert[A]=" << G_defs.dirinvert[A] << "\n"
	     << "      dirinvert[B]=" << G_defs.dirinvert[B] << "\n"
	     << "      loadsteps[A]=" << G_defs.loadsteps[A] << "\n"
	     << "      loadsteps[B]=" << G_defs.loadsteps[B] << "\n"
	     << "---\n"
	     << endl;
    }
}

// INITIALIZE SERIAL COMMUNICATIONS
void SerialInit()
{
    // NO MOTORS? DONT EVEN BOTHER WITH COM PORT
    if ( G_defs.motors == 0 ) return;

    if ( G_defs.debug )
        cerr << "DEBUG: Opening " << G_defs.comdev << ".." << endl;
    G_defs.com = 
            CreateFile(G_defs.comdev,
		       GENERIC_READ|GENERIC_WRITE,
		       0,
		       0,
		       OPEN_EXISTING,
		       FILE_ATTRIBUTE_NORMAL,	// | FILE_FLAG_OVERLAPPED,
		       0);
    if ( G_defs.com == INVALID_HANDLE_VALUE )
	{ cerr << "seek: " << G_defs.comdev << ": " << SYSERR << endl; exit(1); }
    if ( G_defs.debug ) cerr << "DEBUG: OK" << endl;

    // SET TIMEOUTS
    static COMMTIMEOUTS timeouts = 
    {
        0x01, 		// read timeout in MS
	0, 		// read total timeout mult (in MS * #bytes to read)
	0, 		// read total timeout constant
	0, 		// write total timeout mult (in MS * #bytes to read)
	0 		// write total timeout constant
    };
    SetCommTimeouts(G_defs.com, &timeouts);

    // SET STATE
    //     See MSDN\TECHART\4918\SETTINGS.C
    //
    DCB dcb = { 0 };
    dcb.DCBlength = sizeof(dcb);
    if (!GetCommState(G_defs.com, &dcb))
        { cerr << "seek: " << G_defs.comdev << ": " << SYSERR << endl; exit(1); }
    dcb.BaudRate = CBR_9600;
    dcb.fBinary  = 1;
    dcb.fParity  = 0;
    dcb.ByteSize = 8;
    SetCommState(G_defs.com, &dcb);
}

// READ SINGLE CHAR FROM COM PORT
char ReadComm()
{
    if ( G_defs.motors == 0 ) return(0);

    DWORD nbytes = 0;
    char echochar;

    // READ BACK THE ECHO
    int err = ReadFile(G_defs.com,
		       &echochar,
		       1,
		       &nbytes,
		       NULL);

    // CHECK FOR READ ERRORS
    if ( err == 0 )
    {
	cerr << "seek: ReadFile(): " << SYSERR << endl;
	exit(1);
    }
    if ( nbytes != 1 )
    {
	cerr << "seek: ReadFile(): read " << nbytes 
	     << ", expected 1" << endl;
	exit(1);
    }
    if ( G_defs.debug ) 
	cerr << "DEBUG:     RECEIVED 0x" << hex << (int)(unsigned char)echochar 
	     << " ('" << echochar << "')" << dec << endl;

    return(echochar);
}

// READ COMM FOR CHARS, MATCH AGAINST STRING
// RETURNS:
//    0 - OK
//   -1 - Error occurred
//
int ExpectComm(const char *s)
{
    if ( G_defs.motors == 0 ) return(0);
    int err;

    if ( G_defs.debug ) 
        cerr << "DEBUG: EXPECTCOMM: '" << s << "'" << endl;

    for ( ; *s; s++ )
        if ( ReadComm() != *s )
	    return(-1);

    if ( G_defs.debug ) 
        cerr << "DEBUG: EXPECTCOMM: DONE" << endl;

    return(0);
}

// WRITE A STRING TO THE COM DEVICE, CHECK FOR ECHOED CHARACTERS
//    Return: always 0. On error we exit(1).
//
int SendComm(const char *s, int checkecho)
{
    if ( G_defs.motors == 0 ) return(0);

    int err;
    DWORD nbytes;

    if ( G_defs.verbose ) cerr << s;

    // SEND BYTE ONE AT A TIME, CHECK FOR ECHO
    for ( ; *s; s++ )
    {
        nbytes = 0;

	if ( G_defs.debug ) 
	    cerr << "DEBUG:      SENDING 0x" << hex << (int)*s 
	         << " ('" << *s << "')" << dec << endl;

	// WRITE CHARACTER
	err = WriteFile(G_defs.com,
		        s,
		        1,
		        &nbytes,
		        NULL);

	// CHECK FOR WRITE ERRORS
	if ( err == 0 )
	{
	    cerr << "seek: WriteFile(0x" << hex << (int)*s 
	         << " ('" << *s << "'): " << SYSERR << dec << endl;
	    exit(1);
	}
	if ( nbytes != 1 )
	{
	    cerr << "seek: WriteFile(0x" << hex << (int)*s << dec
	         << "): wrote " << nbytes << ", expected 1" << endl;
	    exit(1);
	}

	if ( checkecho )
	{
	    char echochar = ReadComm();

	    // CHECK THE CHAR RECEIVED IS THE CHAR WE SENT
	    if ( echochar != *s )
	    {
		cerr << "seek: expected 0x" << hex << (int)*s << " ('" << *s
		     << "'), got 0x" << hex << (int)echochar << " ('" 
		     << echochar << "')" << dec << endl;
		exit(1);
	    }
	}
    }
    return(0);
}

// RETURN A \n DELIMITED INTEGER, eg. " 0\n"
//     Returns -1 on error
//
int ReadIntComm()
{
    if ( G_defs.motors == 0 ) return(0);

    char s[80];
    s[0] = 0;

    for ( int t=0; t<79; t++ )
    {
        s[t] = ReadComm();
	if ( s[t] == '\n' || s[t] == '\r' )
	    { s[++t] = 0; break; }
    }
    s[79] = 0;

    int val;
    if ( sscanf(s, "%d", &val) == 1 )
         return(val);

    return(-1);
}

// WAIT UNTIL MOTOR STOPS RUNNING
void WaitForStop(char chan)
{
    if ( G_defs.motors == 0 ) { Sleep(100); return; } // simulate

    char cmd[40];
    sprintf(cmd, "%c^", chan);

    // DON'T ECHO POLLING INFO DURING VERBOSE
    int verbosesave = G_defs.verbose;
    G_defs.verbose = 0;

    while ( 1 )
    {
        // SEND "A^\n"
	// EXPECT: 
	//     " 0\n" -- motor stopped
	//     " 1\n" -- motor running
	//

	SendComm(cmd, 1);
	SendComm("\n", 0);		// \n is not echoed back, 
					// so don't check for it

	if ( G_defs.debug ) 
	    cerr << "--- READ BACK IF STOPPED (Expect 0\\n or 1\\n)\n";

	// SEE IF MOTOR STOPPED
	int val = ReadIntComm();
	if ( val == 0 ) break;

	// LITTLE DELAY
	Sleep(10);
    }

    G_defs.verbose = verbosesave;
}

// QUERY FOR FAULTS
int QueryFault()
{
    if ( G_defs.motors == 0 ) 
    { 
        cerr << "seek: can't do this operation if motors disabled" << endl;
	exit(1);
    }

    // INITIALIZE THE COMM PORT
    SerialInit();

    // SEND "Aw\n", "Bw\n"
    // EXPECT: 
    //     " 0\n" -- OK
    //     " 1\n" -- over temperature
    //     etc..
    //
    SendComm("Aw", 1);
    SendComm("\n", 0);		// \n is not echoed back, so don't check for it
    int val = ReadIntComm();
    if ( val == -1 )
    {
        cerr << "Drive A: communication problem -- couldn't get status.\n";
    }
    else
    {
	if ( val == 0 ) cerr << "DRIVE A: OK\n";
	if ( val &  1 ) cerr << "DRIVE A: Over temperature\n";
	if ( val &  2 ) cerr << "DRIVE A: Short circuit\n";
	if ( val &  4 ) cerr << "DRIVE A: Under voltage\n";
    }

    SendComm("Bw", 1);
    SendComm("\n", 0);		// \n is not echoed back, so don't check for it
    val = ReadIntComm();
    if ( val == -1 )
    {
        cerr << "Drive B: communication problem -- couldn't get status.\n";
    }
    else
    {
	if ( val == 0 ) cerr << "DRIVE B: OK\n";
	if ( val &  1 ) cerr << "DRIVE B: Over temperature\n";
	if ( val &  2 ) cerr << "DRIVE B: Short circuit\n";
	if ( val &  4 ) cerr << "DRIVE B: Under voltage\n";
    }

    return(0);
}

// RETURN PATHNAME TO seek.defs
//    This should live in the same directory as the executable.
//
void GetDefsPathname(char *defs, int size)
{
    if ( GetModuleFileName(NULL, defs, size) != 0 )
    {
	// BACKSLASHED
	char *ss;

	if ( ss = strrchr(defs, '\\') )
            { strcpy(ss, "\\seek.defs"); return; }

	// FRONTSLASHED
	if ( ss = strrchr(defs, '/') )
	    { strcpy(ss, "/seek.defs"); return; }
    }

    // FAILURE DEFAULT
    strcpy(defs, "C:\\seek.defs"); 
    return;
}

// HELP
void HelpAndExit()
{
    cerr << "seek -- seek motors on the AMS MAX-420 stepper motor controller\n"
         << "VERSION " << VERSION << "\n"
         << "\n"
	 << "USAGE\n"
	 << "    seek [options] [#Afrms [#Bfrms]]\n"
	 << "\n"
	 << "OPTIONS\n"
	 << "    -s A .25 - overrides the running speed for the A channel\n"
	 << "    -r A .50 - overrides the ramp up speed for the A channel\n"
	 << "    -p A 200 - overrides the pulses-per-revolution (ppr)\n"
	 << "               for the A channel\n"
	 << "    -l AB    - Load projectors A and B\n"
	 << "    -nw      - Don't wait for channels to stop.\n"
	 << "               (affects frame shooting, -H and -S)\n"
	 << "    -nm      - Disable motors (disables drive communication)\n"
	 << "    -v       - verbose communications\n"
	 << "    -d       - debug mode\n"
	 << "    -h       - help\n"
	 << "\n"
	 << "SPECIAL OPTIONS\n"
	 << "    -H A     - Home the 'A' channel\n"
	 << "    -R       - Reset driver\n"
	 << "    -S       - Stop all channels\n"
	 << "    -W       - Wait for channels to stop (if -nw was used)\n"
	 << "    -Q       - Query status of device for faults\n"
	 << "\n"
	 << "EXAMPLES\n"
	 << "    seek 5 5         # move A and B channels forward +5 frames\n"
	 << "    seek 0 -5        # move only B channel -5 frames (ie. reverse)\n"
	 << "    seek -H A .25    # Home the A channel\n"
	 << "    seek -H B .25    # Home the B channel\n"
	 << "    seek -R          # reset the driver\n"
	 << "    seek -Q          # query error status of driver\n"
	 << "    seek -l AB       # load the projectors\n"
	 << endl;

     exit(1);
}

int main(int argc, const char **argv)
{
    int   a_frames   = 0;	// seek 12
    int   b_frames   = 0;	// seek 0 12
    char  home_chan  = 0;	// seek -H A
    const char 
         *load_chans = 0;	// seek -l AB
    int   waitstop   = 1;	// seek -nw disables this
    int   allstop    = 0;	// seek -S
    int   wait       = 0;	// seek -W
    int   reset      = 0;	// seek -R
    const char 
         *p1  = NULL,
         *p2  = NULL;
    char  cmd[80];
    char  defsfile[1024];

    if ( argc <= 1 ) HelpAndExit();

    // LOAD DEFAULT SETTINGS
    GetDefsPathname(defsfile, sizeof(defsfile));
    LoadDefs(defsfile);

    // PARSE COMMAND LINE
    //     Overrides defaults
    //
    for ( int t=1; t<argc; t++ )
    {
	ISIT("-d")  { G_defs.debug = 1; continue; }	       // seek -d
	ISIT("-v")  { G_defs.verbose = 1; continue; }	       // seek -v
	ISIT("-nm") { G_defs.motors = 0; continue; }           // seek -nm
	ISIT("-nw") { waitstop = 0; continue; }	               // seek -nw
	ISIT("-Q")  { exit(QueryFault()); continue; }	       // seek -Q
	ISIT("-S")  { allstop = 1; continue; }                 // seek -S
	ISIT("-W")  { wait = 1; continue; }                    // seek -W
	ISIT("-R")  { reset = 1; continue; }                   // seek -R
	ISIT("-H")                                             // seek -H A .25 
	{
	    if ( ( t + 1 ) >= argc )
	    {
	        cerr << "seek: expected -H <chan>, eg. 'seek -H A'" << endl;
		exit(1);
	    }
	    home_chan = Index2Chan(Chan2Index(argv[++t][0]));
	    continue;
	}
	ISIT("-l")                                             // seek -H A .25 
	{
	    if ( ++t >= argc )
	    {
	        cerr << "seek: expected -l <chans>, eg. 'seek -l AB'" << endl;
		exit(1);
	    }
	    load_chans = argv[t];
	    continue;
	}
	ISIT("-s")
	{ 
	    if ( ( t + 2 ) >= argc )
	    {
	        cerr << "seek: expected -s <chan> <speed>, "
		     << "eg. 'seek -r A .25'" << endl;
		exit(1);
	    }
	    char chan = Index2Chan(Chan2Index(argv[++t][0]));
	    int i = Chan2Index(chan);
	    G_defs.speed[i] = atof(argv[++t]);
	    continue;
	}
	ISIT("-r")
	{ 
	    if ( ( t + 2 ) >= argc )
	    {
	        cerr << "seek: expected -r <chan> <speed>, "
		     << "eg. 'seek -r A 10'" << endl;
		exit(1);
	    }
	    char chan = Index2Chan(Chan2Index(argv[++t][0]));
	    int i = Chan2Index(chan);
	    G_defs.ramp[i] = atof(argv[++t]);
	    continue;
	}
	ISIT("-h")    HelpAndExit();     		      // seek -h
	ISIT("-help") HelpAndExit();	 		      // seek -help

	if ( p1 == NULL ) 
	    { p1 = argv[t]; a_frames = atoi(p1); continue; } // seek 12

	if ( p2 == NULL )
	    { p2 = argv[t]; b_frames = atoi(p2); continue; } // seek 12 24

	HelpAndExit();
    }

    if ( G_defs.debug )
        { cerr << "DEFSFILE=" << defsfile << endl; }

    // INITIALIZE THE COMM PORT
    SerialInit();

    // START
    SendComm("\n", 0); 

    // ALLSTOP? (seek -S)
    if ( allstop )
    {
        SendComm("A@\n", 1);
	SendComm("B@\n", 1);
	if ( wait || waitstop )
	{
	    WaitForStop('A');
	    WaitForStop('B');
	}
	exit(0);
    }

    // RESET? (seek -R)
    if ( reset )
    {
        SendComm("A\003\n", 0);
	SendComm("B\003\n", 0);
	exit(0);
    }

    // WAIT? (seek -W)
    if ( wait )
    {
	WaitForStop('A');
	WaitForStop('B');
	exit(0);
    }

    // SET RUNNING SPEEDS
    sprintf(cmd, "AV%.0f\n", ( 1 / G_defs.speed[A] ) * G_defs.ppr[A]); 
    SendComm(cmd, 1);

    sprintf(cmd, "BV%.0f\n", ( 1 / G_defs.speed[B] ) * G_defs.ppr[B]);
    SendComm(cmd, 1);

    // SET RESOLUTION
    SendComm("AH0\n", 1);
    SendComm("BH0\n", 1);

    // SET DIVISOR
    SendComm("AD0\n", 1);
    SendComm("BD0\n", 1);

    // SET RAMPING
    sprintf(cmd, "AK %d %d\n", 
        (int)G_defs.ramp[A],
	(int)G_defs.ramp[A]);
    SendComm(cmd, 1);
    sprintf(cmd, "BK %d %d\n",
        (int)G_defs.ramp[B],
	(int)G_defs.ramp[B]);
    SendComm(cmd, 1);

    // SET START/STOP SPEED
    sprintf(cmd, "AI %d\n",
        (int)( ( 1 / G_defs.startstopspeed[A] ) * G_defs.ppr[A] ) );
    SendComm(cmd, 1);
    sprintf(cmd, "BI %d\n",
        (int)( ( 1 / G_defs.startstopspeed[B] ) * G_defs.ppr[B] ) );
    SendComm(cmd, 1);

    // HOME CHANNEL?
    if ( home_chan )
    {
        int i = Chan2Index(home_chan);
	sprintf(cmd, "%cF %.0f %d\n", 
	    (char)home_chan, 
	    (float) ( ( 1 / G_defs.homespeed[i] ) * G_defs.ppr[i] ),
	    (int)G_defs.homedir[i]);
	SendComm(cmd, 1);
	if ( waitstop )
	    WaitForStop(home_chan);
	exit(0);
    }

    // LOAD?
    if ( load_chans )
    {
        int t,r;

        // PUT EACH MOTOR INTO THE LOAD POSITION
        for ( t=0; load_chans[t]; t++ )
	{
	    r = Chan2Index(load_chans[t]);
	    sprintf(cmd, "%c%+d\n",
	        (char)load_chans[r],
		(int)(G_defs.loadsteps[r] * G_defs.dirinvert[r]));
	    SendComm(cmd, 1);
	}

        // WAIT FOR EACH MOTOR TO STOP
        for ( t=0; load_chans[t]; t++ )
	{
	    char chan = load_chans[t];
	    WaitForStop(chan);
	}

	// PROMPT USER
	fprintf(stderr, "-- MOTOR(S) IN LOAD POSITION --\n"
	                "HIT RETURN WHEN DONE: ");
	fgets(cmd, sizeof(cmd)-1, stdin);

        // RETURN MOTOR FROM LOAD POSITION
        for ( t=0; load_chans[t]; t++ )
	{
	    r = Chan2Index(load_chans[t]);
	    sprintf(cmd, "%c%+d\n",
	        (char)load_chans[r],
		(int)(G_defs.loadsteps[r] * G_defs.dirinvert[r] * -1 ));
	    SendComm(cmd, 1);
	}

        // WAIT FOR EACH MOTOR TO STOP
        for ( t=0; load_chans[t]; t++ )
	{
	    char chan = load_chans[t];
	    WaitForStop(chan);
	}

        exit(0);
    }

    // INDEX THE MOTORS
    if ( G_defs.debug || G_defs.verbose )
        cerr << "--- AFRAMES=" << a_frames 
	     << ", PPR=" << G_defs.ppr[A]
	     << ", DIRINVERT=" << G_defs.dirinvert[A] 
	     << ", SPEED=" << G_defs.homespeed[A] << endl
             << "--- BFRAMES=" << b_frames 
	     << ", PPR=" << G_defs.ppr[B]
	     << ", DIRINVERT=" << G_defs.dirinvert[B]
	     << ", SPEED=" << G_defs.homespeed[B] << endl;

    sprintf(cmd, "A%+d\n",
        (int)(a_frames * G_defs.ppr[A] * G_defs.dirinvert[A])); 
    SendComm(cmd, 1);

    sprintf(cmd, "B%+d\n",
        (int)(b_frames * G_defs.ppr[B] * G_defs.dirinvert[B])); 
    SendComm(cmd, 1);

    // MAKE SURE BOTH MOTORS HAVE STOPPED
    if ( waitstop )
    {
	WaitForStop('A');
	WaitForStop('B');
    }

    cerr << "OK.." << endl;

    return(0);
}