/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/* An interactive C-tree matcher program for exercising most of the 
   RCCL robot control commands. As to just how useful this program is,
   I'm not sure yet. I think it may disappear, since the C-tree matcher
   isn't powerfull enough to allow very general control scripts to 
   be written. You can write some simple demos with this program, however.
*/

#include <stdio.h>
#include <signal.h>

#ifdef VXWORKS_CFG
#define main rccltest_
#endif

#include <ctype.h>

#include "../h/rccl.h"
#include "../h/ctree.h"
#include "../h/timeval.h"
#include "setpoint.h"
#include "rccltest.h"

char key_buf[256];

static int ignore = 0;

handler1()
{
	printf ("\n\n Signal SIGUSR1 delivered\n\n");
}

handler2()
{
	printf ("\n\n Signal SIGUSR2 delivered\n\n");
}

onAbort(code, msg, td)
int code;
char *msg;
RCI_DESC *td;
{
	FILE *af;

	af = fopen ("abortlog", "w");
	fprintf (af, "\nRCI ABORT: task '%s' on cpu %d\n%s\n",
		      rciTaskName(td), td->cpu, msg);
	fclose (af);

	printf ("\nRCI ABORT: task '%s' on cpu %d\n%s\n",
		 rciTaskName(td), td->cpu, msg);
	exit (-1);
}

#define DEFAMPLITUDE 	25.0
#define DEFTABLERADIUS	150.0
#define DEFSPEED	1.0

extern sineFxn();
extern sawFxn();
extern rampFxn();
extern cosineFxn();
extern turntableFxn();
extern timeRipple();

extern int setpDebug1;
extern int setpDebug2;

main(argc, argv)
int argc;
char **argv;
{	
	MOTION_CONTROL_BLOCK *mcb;
	MANIP *mnp;
	char *keytree;
	float ang;
	int mid;
	int code;
	int signum;
	float dist;
	float scale;
	float tvel, rvel;
	float tmot, tacc;
	float paramVal;
	float naptime;
	char config[10];
	int size;
	int delayCount;
       	int count;
	int interval = 0;
	JNTS jvel;
	TRSF target;
	unsigned long mode;
	int cpuMask = UNDEF;
	POS *point1;
	POS *point2;
	POS *point3;
	POS *point4;
	POS *sine;
	POS *saw;
	POS *ramp;
	POS *cosine;
	POS *rotate;
	POS *turntable;
	POS *bouncetable;

	TRSF *point1Trsf;
	TRSF *point2Trsf;
	TRSF *point3Trsf;
	TRSF *point4Trsf;
	TRSF *sineTrsf;
	TRSF *sawTrsf;
	TRSF *rampTrsf;
	TRSF *cosineTrsf;
	TRSF *turntableTrsf;

	TRSF *point1Const;
	TRSF *point2Const;
	TRSF *point3Const;
	TRSF *point4Const;
	TRSF *sineConst;
	TRSF *sawConst;
	TRSF *rampConst;
	TRSF *cosineConst;
	TRSF *rotateConst;

	struct fxnParams *fxnParams;

	char *robotName = NULL;

	JNTS setJnts;

	argc--; argv++;
	while (argc)
	 { if (strcmp (*argv, "-sim") == 0)
	    { rcclSetOptions (RCCL_SIMULATE);
	    }
	   else if (strcmp (*argv, "-cpu") == 0)
	    { argc--; argv++;
	      cpuMask = (1<<(atoi(*argv)));
	    }
	   else if (strcmp (*argv, "-D1") == 0)
	    { setpDebug1 = 1;
	    }
	   else if (strcmp (*argv, "-D2") == 0)
	    { setpDebug2 = 1;
	    }
	   else if (strcmp (*argv, "-r") == 0)
	    { argc--; argv++;
	      interval = atoi(*argv);
	      if (interval <= 0)
	       { fatalMsg (-1, "bogus rate interval (%d)\n", interval);
	       }
	    }
	   else if (!robotName)
	    { 
	      robotName = *argv;
	    }
	   else
	    { argc = -1;
	      break;
	    }
	   argc--; argv++;
	 }
	if (argc < 0)
	 { usageMsg (-1, "rccltest [-sim] [-cpu <number>] [-r <rate>]\n");
	 }
	if (cpuMask == UNDEF)
	 { cpuMask = ~0;
	 }
	if (!robotName)
	 { robotName = (char*)getDefaultRobot();
	 }

	ignore = 0;
	
	keytree = tree_match_parse("(					\
	change ([point1 point2 point3 point4 cosine saw sine rotate]	\
	 ([x y z rx ry rz]						\
	   (%f", &dist, ",,,	\"distance (mm. or degrees) \")))	\
	confirm								\
	flushMotionQueue						\
	getCartAccel 							\
	getCartVel							\
	getActiveConf							\
	getDefaultCartVel						\
	getDefaultJointVel						\
	getJointAccel							\
	getJointVel							\
	getJvelScale							\
	getMod								\
	getMotionQueueSize						\
	getSpeed 							\
	here ([point1 point2 point3 point4 cosine saw sine rotate])	\
	setMotionHold							\
	motionStatus							\
	 (%d", &mid, ",,,	\"motion id (integer)\")		\
	move								\
	 ([x y z rx ry rz]						\
	   (%f", &dist, ",,,	\"distance (mm. or degrees) \")		\
	  [point1 point2 point3 point4 cosine saw sine rotate ramp	\
	   turntable bouncetable]					\
	 )								\
	movej								\
	 ([1 2 3 4 5 6]							\
	   ([to by]							\
	     (%f", &ang, ",,,	\"angle (degrees) \"))			\
	  ready								\
	  rcclpark							\
	  all								\
	 )								\
	nap								\
	 (%f", &naptime, ",,,	\"nap time (seconds)\")			\
	numMotionsQueued						\
	rcclPause							\
	 (%f", &tmot, ",,,	\"pause time (msec)\"			\
	   (%f", &tacc, ",,,	\"transition time\"))			\
	quit								\
	rcclActive							\
	rcclBlock							\
	 (%d", &count, ",,,	\"number of times\")			\
	rcclClearModes							\
	 (%x", &mode, ",,,	\"mode bits (hex)\")			\
	rcclGetModes							\
	 (%x", &mode, ",,,	\"mode bits (hex)\")			\
	rcclPutModes							\
	 (%x", &mode, ",,,	\"mode bits (hex)\")			\
	rcclRelease							\
	rcclSetModes							\
	 (%x", &mode, ",,,	\"mode bits (hex)\")			\
	rcclStart							\
	rcclResume							\
	set (fxnParams 							\
	 ([amplitude tableRadius speed]					\
	   (%f", &paramVal, ",,,\"parameter value\")))			\
	[setCartAccel setCartVel]					\
	 (%f", &tvel, ",,,	\"translational velocity\"		\
	   (%f", &rvel, ",,,	\"rotational velocity\"))		\
	setCompletedSig							\
	 (%d", &signum, ",,,	\"signal number\")			\
	setConf								\
	 (%s", config, ",	\"configuration string\")		\
	setJointAccel							\
	setJointVel							\
	setJvelScale							\
	 (%f", &scale, ",,,	\"scale value (float)\")		\
	setMod								\
	 (joint Cartesian)						\
	setMotionFlag							\
	 (%d", &mid, ",,,	\"motion id (integer)\")		\
	setMotionQueueSize						\
	 (%d", &size, ",,,	\"size of motion queue\")		\
	setSpeed							\
	 (%f", &scale, ",,,	\"scale value (float)\")		\
	setStopSig							\
	 (%d", &signum, ",,,	\"signal number\"			\
	   (%d", &mid, ",,,	\"motion id (integer)\"))		\
	setTime								\
	 (%f", &tacc, ",,,	\"transition time (msec)\"		\
	   (%f", &tmot, ",,,	\"motion time\"))			\
	show								\
	 (completed here tool lastTC t6 j6 t6o jvel conf		\
	  sine saw cosine point1 point2 point3 point4 rotate)		\
	startMotion							\
	 (%d", &mid, ",,,	\"motion id (integer)\"			\
	   (%d", &code, ",,,	\"code value\"))			\
	stopCurrentMotion						\
	 (%d", &code, ",,,	\"motion stop code\")			\
	stopMotion							\
	 (%d", &mid, ",,,	\"motion id (integer)\"			\
	   (%d", &code, ",,,	\"code value\"))			\
	rippleFxn							\
	 (%d", &delayCount, ",,,	\"delay count\")		\
	teach								\
	waitForCompleted						\
	[waitForStop waitForStart]					\
	 (%d", &mid, ",,,	\"motion id\")				\
)",0);
	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }
	tree_match_catch_sig (0, sigmask(SIGALRM));

	rcclSetOptions (RCCL_ERROR_EXIT);
	
	mnp = rcclCreate(robotName, cpuMask);
	mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;

	if (interval)
	 { rcclSetInterval(interval);
	 }

	point1Trsf   = allocTrans ("point1Trsf", 0);
	point2Trsf   = allocTrans ("point2Trsf", 0);
	point3Trsf   = allocTrans ("point3Trsf", 0);
	point4Trsf   = allocTrans ("point4Trsf", 0);
	sineTrsf     = allocTrans ("sineTrsf", 0);
	sawTrsf      = allocTrans ("sawTrsf", 0);
	rampTrsf      = allocTrans ("rampTrsf", 0);
	cosineTrsf   = allocTrans ("cosineTrsf", 0);
	turntableTrsf   = allocTrans ("turntableTrsf", 0);

	point1Const  = allocTrans ("point1Const", 0);
	point2Const  = allocTrans ("point2Const", 0);
	point3Const  = allocTrans ("point3Const", 0);
	point4Const  = allocTrans ("point4Const", 0);
	sineConst    = allocTrans ("sineConst", 0);
	sawConst     = allocTrans ("sawConst", 0);
	rampConst     = allocTrans ("rampConst", 0);
	cosineConst  = allocTrans ("cosineConst", 0);
	rotateConst  = allocTrans ("rotateConst", 0);

	fxnParams    = (struct fxnParams*)allocMem (NULL, 
						       sizeof(struct fxnParams),0);

	fxnParams->amplitude = DEFAMPLITUDE;
	fxnParams->tableRadius = DEFTABLERADIUS;
	fxnParams->speed = DEFSPEED;

	point1 = makePosition ("POINT1", T6, EQ, point1Const, point1Trsf, 
					TL, T6);
	point2 = makePosition ("POINT2", T6, EQ, point2Const, point2Trsf, 
					TL, T6);
	point3 = makePosition ("POINT3", T6, EQ, point3Const, point3Trsf, 
					TL, T6);
	point4 = makePosition ("POINT4", T6, EQ, point4Const, point4Trsf, 
					TL, T6);
	sine = makePosition ("SINE", T6, EQ, sineConst, sineTrsf,
					TL, T6);
	saw = makePosition ("SAW", T6, EQ, sawConst, sawTrsf,
					TL, T6);
	ramp = makePosition ("RAMP", T6, EQ, rampConst, rampTrsf,
					TL, T6);
	cosine = makePosition ("COSINE", T6, EQ, cosineConst, cosineTrsf,
					TL, T6);
	rotate = makePosition ("ROTATE", T6, EQ, rotateConst, sineTrsf,
					cosineTrsf, TL, cosineTrsf);
	turntable = makePosition ("TURNTABLE", T6, EQ, point1Const, turntableTrsf,
					TL, T6);
	bouncetable = makePosition ("BOUNCETABLE", T6, EQ, point1Const, 
					turntableTrsf, cosineTrsf, TL, T6);

	signal (SIGUSR1, handler1);
	signal (SIGUSR2, handler2);

	rcclStart();

	rciAbortHandler(onAbort);

	transEval (sineTrsf, sineFxn, (int)fxnParams);
	transEval (sawTrsf, sawFxn, (int)fxnParams);
	transEval (cosineTrsf, cosineFxn, (int)fxnParams);
	transEval (turntableTrsf, turntableFxn, (int)fxnParams);
	transMotionEval (rampTrsf, rampFxn, (int)fxnParams, mnp);

	readTrans (mnp->t6, sineConst);
	readTrans (mnp->t6, sawConst);
	readTrans (mnp->t6, rampConst);
	readTrans (mnp->t6, cosineConst);
	readTrans (mnp->t6, rotateConst);
	readTrans (mnp->t6, point1Const);
	readTrans (mnp->t6, point2Const);
	point2Const->p.x += 100.0;
	readTrans (mnp->t6, point3Const);
	point3Const->p.x += 100.0;
	point3Const->p.z += 100.0;
	readTrans (mnp->t6, point4Const);
	point4Const->p.z += 100.0;

	rcclClearOptions (RCCL_ERROR_EXIT);

	readMem (mnp->j6, &setJnts);

	do
 	 { tree_match (keytree, "> ", key_buf);

#if 0
	   if (COMMAND("teachCtrl"))
	    { mcb->controlModeReq = TEACH_CTRL;
	      while (mcb->controlModeReq)
		 ;
	    }
	   else if (COMMAND ("setpointCtrl"))
	    { mcb->controlModeReq = SETPOINT_CTRL;
	      while (mcb->controlModeReq)
		 ;
	    }
	   else if (COMMAND ("idleCtrl"))
	    { mcb->controlModeReq = IDLE_CTRL;
	      while (mcb->controlModeReq)
		 ;
	    }
	   else
#endif
	   if (COMMAND ("rcclStart"))
	    { rcclStart();
	    }
	   else if (COMMAND ("rcclRelease"))
	    { rcclRelease(0);
	    }
	   else if (COMMAND ("rcclBlock"))
	    { int i;
	      for (i=0; i<count; i++)
	       { rcclBlock();
	       }
	    }
	   else if (COMMAND ("nap"))
	    { delay (naptime*1000.0);
	    }
	   else if (COMMAND ("setMotionHold"))
	    { setMotionHold (mnp);
	    }
	   else if (COMMAND ("setMotionFlag"))
	    { setMotionFlag (mnp, mid);
	    }
	   else if (COMMAND ("stopMotion"))
	    { stopMotion (mid, code);
	    }
	   else if (COMMAND ("startMotion"))
	    { startMotion (mid, code);
	    }
	   else if (COMMAND ("motionStatus"))
	    { int stopCode, startCode, status;
	      float scale;
	
	      if ((stopCode = motionStopCode (mid)) == MSTAT_ERR ||
		  (startCode = motionStartCode (mid)) == MSTAT_ERR ||
		  (scale = motionScale(mid)) == MSTAT_ERR ||
		  (status = motionStatus (mid)) == MSTAT_ERR )
	       { printErrors();
		 clearErrors();
		 continue;
	       }
	      else
	       { printf ("stopCode:  %x\n", stopCode);
		 printf ("startCode: %x\n", startCode);
		 printf ("scale:     %g\n", scale);
		 printf ("status:    %x\n", status);
	       }
	    }
	   else if (COMMAND ("teach"))
	    { rcclTeach (mnp, "TEACH> ", NULL);
	    }
	   else if (COMMAND ("setMod joint"))
	    { setMod (mnp, 'j');
	    }
	   else if (COMMAND ("setMod Cartesian"))
	    { setMod (mnp, 'c');
	    }
	   else if (COMMAND ("getMod"))
	    { switch (getMod(mnp))
	       { case 'c':
		  { printf ("Cartesian\n");
		    break;
		  }
		 case 'j':
		  { printf ("joint\n");
		    break;
		  }
	       }
	    }
		    
	   else if (COMMAND ("setSpeed"))
	    { if ((scale = setSpeed (mnp, scale)) >= 0)
	       { printf ("%g\n", scale);
	       }
	    }
	   else if (COMMAND ("setCartVel"))
	    { setCartVel (mnp, tvel, rvel);
	    }
	   else if (COMMAND ("setCartAccel"))
	    { setCartAccel (mnp, tvel, rvel);
	    }
	   else if (COMMAND ("setJointVel"))
	    { printf ("%d values> ", NUM_JNTS);
	      if (fscanVf ((FILE*)tree_match_input_stream(), 
			   jvel.v, NUM_JNTS) < 0)
	       { printf ("Error reading values\n");
		 continue;
	       }
	      setJointVel (mnp, &jvel);
	    }
	   else if (COMMAND ("setJointAccel"))
	    { printf ("%d values> ", NUM_JNTS);
	      if (fscanVf ((FILE*)tree_match_input_stream(), jvel.v, NUM_JNTS) < 0)
	       { printf ("Error reading values\n");
		 continue;
	       }
	      setJointAccel (mnp, &jvel);
	    }
	   else if (COMMAND ("setJvelScale"))
	    { if ((scale = setJvelScale (mnp, scale)) >= 0)
	       { printf ("%g\n", scale);
	       }
	    }
	   else if (COMMAND ("getSpeed"))
	    { if ((scale = getSpeed(mnp)) >= 0)
	       { printf ("%g\n", scale);
	       }
	    }
	   else if (COMMAND ("getCartVel"))
	    { if (getCartVel (mnp, &tvel, &rvel) >= 0)
	       { printf ("%g %g\n", tvel, rvel);
	       }
	    }
	   else if (COMMAND ("getDefaultCartVel"))
	    { if (getDefaultCartVel (mnp, &tvel, &rvel) >= 0)
	       { printf ("%g %g\n", tvel, rvel);
	       }
	    }
	   else if (COMMAND ("getCartAccel"))
	    { if (getCartAccel (mnp, &tvel, &rvel) >= 0)
	       { printf ("%g %g\n", tvel, rvel);
	       }
	    }
	   else if (COMMAND ("getJointVel"))
	    { if (getJointVel (mnp, &jvel) >= 0)
	       { printVf ("%8.2f\n", jvel.v, NUM_JNTS);
	       }
	    }
	   else if (COMMAND ("getDefaultJointVel"))
	    { if (getDefaultJointVel (mnp, &jvel) >= 0)
	       { printVf ("%8.2f\n", jvel.v, NUM_JNTS);
	       }
	    }
	   else if (COMMAND ("getJointAccel"))
	    { if (getJointAccel (mnp, &jvel) >= 0)
	       { printVf ("%8.2f\n", jvel.v, NUM_JNTS);
	       }
	    }
	   else if (COMMAND ("getJvelScale"))
	    { if ((scale = getJvelScale(mnp)) >= 0)
	       { printf ("%g\n", scale);
	       }
	    }
	   else if (COMMANDS ("movej ready"))
	    { JNTS angles;
	      if (getRobotPosition (angles.v, "ready", robotName) == NUM_JNTS)
	       { movej (mnp, &angles);
	       }
	    }
	   else if (COMMANDS ("movej rcclpark"))
	    { JNTS angles;
	      if (getRobotPosition (angles.v, "rcclpark", robotName) == NUM_JNTS)
	       { movej (mnp, &angles);
	       }
	    }
	   else if (COMMANDS ("movej all"))
	    { JNTS angles;
	      printf ("%d angles (degrees):\n", NUM_JNTS);
	      if (fscanVf ((FILE*)tree_match_input_stream(), angles.v,
			    mnp->jls->numJ) != 1)
	       { printf ("Error scanning values\n");
	       }
	      else
	       { degreeToRadian (angles.v, angles.v, mnp->jls);
		 movej (mnp, &angles);
	       }
	    }
	   else if (COMMANDS ("movej"))
	    { char *kbp = &key_buf[strlen("movej ")];
	      int j = *kbp - '1';
	      float cvt;
	      *kbp = 'X';

	      if (mnp->completed && (numMotionsQueued(mnp) == 0))
	       { copyVf (setJnts.v, mnp->j6->v, NUM_JNTS);
	       }
	      cvt = (mnp->jls->jtype[j]=='R' ? DEGTORAD : 1.0);

	      if (COMMAND ("movej X to"))
	       { setJnts.v[j] = cvt*ang;
	       }
	      else if (COMMAND ("movej X by"))
	       { setJnts.v[j] += cvt*ang;
	       }
	      if ((mid = movej (mnp, &setJnts)) >= 0)
	       { printf ("motion id %d\n", mid);
	       }
	    }
	   else if (COMMANDS ("move point1"))
	    { int mid;
	      if ((mid = move (mnp, point1)) >= 0)
	       { printf ("motion id %d\n", mid);
	       }
	    }
	   else if (COMMANDS ("move point2"))
	    { int mid;
	      if ((mid = move (mnp, point2)) >= 0)
	       { printf ("motion id %d\n", mid);
	       }
	    }
	   else if (COMMANDS ("move point3"))
	    { int mid;
	      if ((mid = move (mnp, point3)) >= 0)
	       { printf ("motion id %d\n", mid);
	       }
	    }
	   else if (COMMANDS ("move point4"))
	    { int mid;
	      if ((mid = move (mnp, point4)) >= 0)
	       { printf ("motion id %d\n", mid);
	       }
	    }
	   else if (COMMANDS ("move rotate"))
	    { int mid;
	      if ((mid = move (mnp, rotate)) >= 0)
	       { printf ("motion id %d\n", mid);
	       }
	    }
	   else if (COMMANDS ("move sine"))
	    { int mid;
	      if ((mid = move (mnp, sine)) >= 0)
	       { printf ("motion id %d\n", mid);
	       }
	    }
	   else if (COMMANDS ("move saw"))
	    { int mid;
	      if ((mid = move (mnp, saw)) >= 0)
	       { printf ("motion id %d\n", mid);
	       }
	    }
	   else if (COMMANDS ("move ramp"))
	    { int mid;
	      identTrsf (rampTrsf);
	      if ((mid = move (mnp, ramp)) >= 0)
	       { printf ("motion id %d\n", mid);
	       }
	    }
	   else if (COMMANDS ("move cosine"))
	    { int mid;
	      if ((mid = move (mnp, cosine)) >= 0)
	       { printf ("motion id %d\n", mid);
	       }
	    }
	   else if (COMMANDS ("move bouncetable"))
	    { int mid;
	      if ((mid = move (mnp, bouncetable)) >= 0)
	       { printf ("motion id %d\n", mid);
	       }
	    }
	   else if (COMMANDS ("move turntable"))
	    { int mid;
	      if ((mid = move (mnp, turntable)) >= 0)
	       { printf ("motion id %d\n", mid);
	       }
	    }
	   else if (COMMANDS ("move ") || COMMANDS ("change "))
	    { TRSF diff;
	      int mid;
	      char *dof;

	      if (COMMANDS ("move"))
	       { dof = key_buf + strlen("move ");
	       }
	      else
	       { dof = (char*)(index(index(key_buf, ' '), ' ') + 1);
	       }

	      identTrsf (&diff);
	      if (strcmp (dof, "x") == 0)
	       { diff.p.x = dist;
	       }
	      else if (strcmp (dof, "y") == 0)
	       { diff.p.y = dist;
	       }
	      else if (strcmp (dof, "z") == 0)
	       { diff.p.z = dist;
	       }
	      else if (strcmp (dof, "rx") == 0)
	       { rpyToTrsf (&diff, 0.0, 0.0, dist);
	       }
	      else if (strcmp (dof, "ry") == 0)
	       { rpyToTrsf (&diff, 0.0, dist, 0.0);
	       }
	      else if (strcmp (dof, "rz") == 0)
	       { rpyToTrsf (&diff, dist, 0.0, 0.0);
	       }

	      if (COMMANDS ("move"))
	       { setDistance (mnp, &diff);
		 if ((mid = move (mnp, mnp->last)) >= 0)
		  { printf ("motion id %d\n", mid);
		  }
	       }
	      else 
	       { if (COMMANDS ("change point1"))
		  { multTrsf (point1Const, point1Const, &diff);
		  }
		 else if (COMMANDS ("change point2"))
		  { multTrsf (point2Const, point2Const, &diff);
		  }
		 else if (COMMANDS ("change point3"))
		  { multTrsf (point3Const, point3Const, &diff);
		  }
		 else if (COMMANDS ("change point4"))
		  { multTrsf (point4Const, point4Const, &diff);
		  }
		 else if (COMMANDS ("change sine"))
		  { multTrsf (sineConst, sineConst, &diff);
		  }
		 else if (COMMANDS ("change saw"))
		  { multTrsf (sawConst, sawConst, &diff);
		  }
		 else if (COMMANDS ("change cosine"))
		  { multTrsf (cosineConst, cosineConst, &diff);
		  }
		 else if (COMMANDS ("change rotate"))
		  { multTrsf (rotateConst, rotateConst, &diff);
		  }
	       }
	    }
	   else if (COMMAND ("setCompletedSig"))
	    { setCompletedSig (mnp, signum);
	    }
	   else if (COMMAND ("setStopSig"))
	    { setStopSig (mnp, signum, mid);
	    }
	   else if (COMMAND ("rcclPause"))
	    { rcclPause (tacc, tmot);
	    }
	   else if (COMMAND ("rcclResume"))
	    { rcclResume ();
	    }
	   else if (COMMAND("rcclActive"))
	    { printf ("%d\n", rcclActive());
	    }
	   else if (COMMAND("rcclActive"))
	    { printf ("%d\n", rcclActive());
	    }
	   else if (COMMAND("setMotionQueueSize"))
	    { setMotionQueueSize (mnp, size);
	    }
	   else if (COMMAND("getMotionQueueSize"))
	    { printf ("%d\n", getMotionQueueSize (mnp));
	    }
	   else if (COMMAND("numMotionsQueued"))
	    { printf ("%d\n", numMotionsQueued (mnp));
	    }
	   else if (COMMAND("flushMotionQueue"))
	    { flushMotionQueue (mnp);
	    }
	   else if (COMMAND("stopCurrentMotion"))
	    { stopCurrentMotion (mnp, code);
	    }
	   else if (COMMAND ("waitForStart"))
	    { waitForStart (mid);
	    }
	   else if (COMMAND ("waitForStop"))
	    { waitForStop (mid);
	    }
	   else if (COMMAND ("waitForCompleted"))
	    { waitfor (mnp->completed);
	    }
	   else if (COMMAND ("setTime"))
	    { setTime (mnp, tacc, tmot);
	    }
	   else if (COMMAND ("setConf"))
	    { int status = setConf (mnp, config);
	      if (status != -1)
	       { printf ("0x%x\n", status);
	       }
	    }
	   else if (COMMAND ("here point1"))
	    { readTrans (mnp->t6, point1Const);
	    }
	   else if (COMMAND ("here point2"))
	    { readTrans (mnp->t6, point2Const);
	    }
	   else if (COMMAND ("here point3"))
	    { readTrans (mnp->t6, point3Const);
	    }
	   else if (COMMAND ("here point4"))
	    { readTrans (mnp->t6, point4Const);
	    }
	   else if (COMMAND ("here sine"))
	    { readTrans (mnp->t6, sineConst);
	    }
	   else if (COMMAND ("here saw"))
	    { readTrans (mnp->t6, sawConst);
	    }
	   else if (COMMAND ("here cosine"))
	    { readTrans (mnp->t6, cosineConst);
	    }
	   else if (COMMAND ("here rotate"))
	    { readTrans (mnp->t6, rotateConst);
	    }
	   else if (COMMAND ("getActiveConf"))
	    { char confstr[8];
	      int config = getActiveConf(mnp, confstr);
	      printf ("0x%x  %s\n", config, confstr);
	    }
	   else if (COMMAND ("rcclSetModes"))
	    { printf ("0x%x\n", rcclSetModes(mnp, mode));
	    }
	   else if (COMMAND ("rcclClearModes"))
	    { printf ("0x%x\n", rcclClearModes(mnp, mode));
	    }
	   else if (COMMAND ("rcclPutModes"))
	    { printf ("0x%x\n", rcclPutModes(mnp, mode));
	    }
	   else if (COMMAND ("rcclGetModes"))
	    { printf ("0x%x\n", rcclGetModes(mnp, mode));
	    }
	   else if (COMMAND ("show completed"))
	    { printf ("%d\n", mnp->completed);
	    }
	   else if (COMMAND ("show t6"))
	    { TRSF t6;
	      readTrans (mnp->t6, &t6);
	      printTrsf ("%m\n", &t6);
	    }
	   else if (COMMAND ("show t6o"))
	    { TRSF t6o;
	      readTrans (mnp->t6o, &t6o);
	      printTrsf ("%m\n", &t6o);
	    }
	   else if (COMMAND ("show tool"))
	    { TRSF tool;
	      readTrans (mnp->tool, &tool);
	      printTrsf ("%m\n", &tool);
	    }
	   else if (COMMAND ("show here"))
	    { TRSF here;
	      readTrans (mnp->here, &here);
	      printTrsf ("%m\n", &here);
	    }
	   else if (COMMAND ("show lastTC"))
	    { TRSF lastTC;
	      readTrans (mnp->lastTC, &lastTC);
	      printTrsf ("%m\n", &lastTC);
	    }
	   else if (COMMAND ("show jvel"))
	    { JNTS jvel;
	      readMem (mnp->jvel, &jvel);
	      radianToDegree (jvel.v, jvel.v, mnp->jls);
	      printVf ("%8.3\n", jvel.v, NUM_JNTS);
	    }
	   else if (COMMAND ("show j6"))
	    { JNTS j6;
	      readMem (mnp->j6, &j6);
	      radianToDegree (j6.v, j6.v, mnp->jls);
	      printVf ("%8.3f\n", j6.v, NUM_JNTS);
	    }
	   else if (COMMAND ("show conf"))
	    { printf ("0x%x\n", mnp->conf);
	    }
	   else if (COMMAND ("show point1"))
	    { printTrsf ("%m\n", point1Const);
	    }
	   else if (COMMAND ("show point2"))
	    { printTrsf ("%m\n", point2Const);
	    }
	   else if (COMMAND ("show point3"))
	    { printTrsf ("%m\n", point3Const);
	    }
	   else if (COMMAND ("show point4"))
	    { printTrsf ("%m\n", point4Const);
	    }
	   else if (COMMAND ("show sine"))
	    { printTrsf ("%m\n", sineConst);
	    }
	   else if (COMMAND ("show saw"))
	    { printTrsf ("%m\n", sawConst);
	    }
	   else if (COMMAND ("show cosine"))
	    { printTrsf ("%m\n", cosineConst);
	    }
	   else if (COMMAND ("show rotate"))
	    { printTrsf ("%m\n", rotateConst);
	    }
	   else if (COMMAND ("set fxnParams amplitude"))
	    { fxnParams->amplitude = paramVal;
	    }
	   else if (COMMAND ("set fxnParams tableRadius"))
	    { fxnParams->tableRadius = paramVal;
	    }
	   else if (COMMAND ("set fxnParams speed"))
	    { fxnParams->speed = paramVal;
	    }
	   else if (COMMAND ("rippleFxn"))
	    { runMonitorFxn (mnp, timeRipple, delayCount);	      
	    }
	   else if (COMMAND ("confirm"))
	    { int c;

	      if (!ignore)
	       { printf ("<CR> to continue, 'q' to quit:");
		 switch (c = getchar())
		  { case 'q':
		     { rcclRelease(1);
		       rcclDelete(mnp);
		       return (0);
		       break;
		     }
		    case '!':
		     { ignore = 1;
		       break;
		     }
		  }
		 while (c != '\n')
		  { c = getchar();
		  }
	       }
	    }
	   if (numErrors() > 0)
	    { printErrors();
	      clearErrors();
	    }
	
	 }
	while (!(COMMAND("quit")));

	rcclRelease(1);
	rcclDelete(mnp);
}
