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

#include "../h/cdefs.h"
#include "../h/rciTask.h"
#include "../h/robotControl.h"
#include "../h/errorCodes.h"
#include "../h/moper_com.h"
#include "../h/robot_jls.h"
#include "../h/priDevice.h"
#include "../h/IOdevice.h"
#include "../h/robotMath.h"
#include "../h/rciMonitor.h"
#include "../h/rciMacros.h"

static int testForBigEndian();

extern int rciCpu;
char *genErrorString();
static bigEndian;		/* machine byte order, set in gen_startup()*/

/* ---- error codes and directives to the wrapup function ---- */

#define ACTIVATED	1	/* robot must be ready to allow a write */
#define NO_NOTIFY	2	/* robot error; don't bother telling robot */
#define DOING_IO	3	/* doing I/O; can;t tell robot anything */

union _pack {
	unsigned short s;
	unsigned char b[2];
};	

/* Macros to help arrange proper byte order. The controller at the
   other end, is littleEndian (VAX-like). Bytes should be placed into
   and removed from the packing structure accordingly. */

#define BYTE0	(bigEndian ? 1 : 0)	
#define BYTE1	(bigEndian ? 0 : 1)

union _float {
	float f;
	unsigned short s[2];
};

union _int {
	int i;
	unsigned short s[2];
};

gen_input(td, rbt)
RCI_DESC *td;
RCI_RBT *rbt;
{
 	register HOW *how = rbt->how;
 	register JLS *jls = rbt->jls;
	RCI_TCB *tcb = td->tcb;
	IO_DEVICE *dev = (IO_DEVICE*)rbt->dev;

	char *pfx = "rbtInput() --";

	/* ---- read stuff in from the robot and load the 'how' struct ---- */

	 { char dataBuf[MAX_DATABUF];
	   float ang[6];
	   int   enc[6];
	   register unsigned short *dp = (unsigned short *)dataBuf;
	   register int n, k;
	   register float rate;
	   unsigned short *hp;
	   int readFlags;
	   int stateFlags;
	   union _int iencread;
	   int j;

	   rbt->notice = DOING_IO;
	   if ((n = (*dev->read) (dev, dataBuf, MAX_DATABUF)) < 0)
	    { errorMsg (getErrorCode(0), "%s error reading from robot\n",pfx);
	      rciAbortErrors();
	      return (-1);
	    }
	   rbt->notice = ACTIVATED;

	   if (n/sizeof(short) == 1)
	    { rciAbort (ERobotError, "controller error 0x%x:\n%s\n",
			(int)*dp, genErrorString ((int)*dp) );
	      rbt->notice = NO_NOTIFY;
	      return (-1);
	    }
	   enc[0] = *dp++;
	   enc[1] = *dp++;
	   enc[2] = *dp++;
	   enc[3] = *dp++;
	   enc[4] = *dp++;
	   enc[5] = *dp++;

	   /* ---- compute angle and velocity ---- */

	   (*jls->encoderToAngle)(ang, enc, jls);	   

	   how->time = rciTime();

	   if (tcb->cycleCount == 1)
	    { 
	      how->angVel[0] = 0.0;
	      how->angVel[1] = 0.0;
	      how->angVel[2] = 0.0;

	      how->angVel[3] = 0.0;
	      how->angVel[4] = 0.0;
	      how->angVel[5] = 0.0;

	      how->encVel[0] = 0.0;
	      how->encVel[1] = 0.0;
	      how->encVel[2] = 0.0;

	      how->encVel[3] = 0.0;
	      how->encVel[4] = 0.0;
	      how->encVel[5] = 0.0;

	      how->angSet[0] = ang[0];
	      how->angSet[1] = ang[1];
	      how->angSet[2] = ang[2];

	      how->angSet[3] = ang[3];
	      how->angSet[4] = ang[4];
	      how->angSet[5] = ang[5];

	      how->encSet[0] = enc[0];
	      how->encSet[1] = enc[1];
	      how->encSet[2] = enc[2];

	      how->encSet[3] = enc[3];
	      how->encSet[4] = enc[4];
	      how->encSet[5] = enc[5];
	    }
	   else
	    { rate = 1000.0/(float)(tcb->interval);

	      how->angVel[0] = (ang[0] - how->ang[0]) * rate;
	      how->angVel[1] = (ang[1] - how->ang[1]) * rate;
	      how->angVel[2] = (ang[2] - how->ang[2]) * rate;

	      how->angVel[3] = (ang[3] - how->ang[3]) * rate;
	      how->angVel[4] = (ang[4] - how->ang[4]) * rate;
	      how->angVel[5] = (ang[5] - how->ang[5]) * rate;

	      how->encVel[0] = (enc[0] - how->enc[0]) * rate;
	      how->encVel[1] = (enc[1] - how->enc[1]) * rate;
	      how->encVel[2] = (enc[2] - how->enc[2]) * rate;

	      how->encVel[3] = (enc[3] - how->enc[3]) * rate;
	      how->encVel[4] = (enc[4] - how->enc[4]) * rate;
	      how->encVel[5] = (enc[5] - how->enc[5]) * rate;
	    }

	   if (k = how->loadSettings)
	    { if (how->controlLevel == MOTOR_LEVEL)
	       { 
		 if (k & 0x01)
		  { how->encSet[0] = enc[0];
		  }
		 if (k & 0x02)
		  { how->encSet[1] = enc[1];
		  }
		 if (k & 0x04)
		  { how->encSet[2] = enc[2];
		  }
   
		 if (k & 0x08)
		  { how->encSet[3] = enc[3];
		  }
		 if (k & 0x10)
		  { how->encSet[4] = enc[4];
		  }
		 if (k & 0x20)
		  { how->encSet[5] = enc[5];
		  }
	       }
	      else
	       { 
		 if (k & 0x01)
		  { how->angSet[0] = ang[0];
		  }
		 if (k & 0x02)
		  { how->angSet[1] = ang[1];
		  }
		 if (k & 0x04)
		  { how->angSet[2] = ang[2];
		  }
   
		 if (k & 0x08)
		  { how->angSet[3] = ang[3];
		  }
		 if (k & 0x10)
		  { how->angSet[4] = ang[4];
		  }
		 if (k & 0x20)
		  { how->angSet[5] = ang[5];
		  }
	       }
	      how->loadSettings = 0;		    
	    }

	   how->ang[0] = ang[0];
	   how->ang[1] = ang[1];
	   how->ang[2] = ang[2];

	   how->ang[3] = ang[3];
	   how->ang[4] = ang[4];
	   how->ang[5] = ang[5];

	   how->enc[0] = enc[0];
	   how->enc[1] = enc[1];
	   how->enc[2] = enc[2];

	   how->enc[3] = enc[3];
	   how->enc[4] = enc[4];
	   how->enc[5] = enc[5];

	   if (how->checking & CHK_MAXPOS)
	    { int jnts = 0;
	      if (ang[0] < jls->jmin[0] || ang[0] > jls->jmax[0])
	       { jnts |= (1<<0);
	       }
	      if (ang[1] < jls->jmin[1] || ang[1] > jls->jmax[1])
	       { jnts |= (1<<1);
	       }
	      if (ang[2] < jls->jmin[2] || ang[2] > jls->jmax[2])
	       { jnts |= (1<<2);
	       }
	      if (ang[3] < jls->jmin[3] || ang[3] > jls->jmax[3])
	       { jnts |= (1<<3);
	       }
	      if (ang[4] < jls->jmin[4] || ang[4] > jls->jmax[4])
	       { jnts |= (1<<4);
	       }
	      if (ang[5] < jls->jmin[5] || ang[5] > jls->jmax[5])
	       { jnts |= (1<<5);
	       }
	      if (jnts)
	       { rciAbort (EMaxPos,
		"joint limit exceeded for joints 0x%x\n", jnts);
		 return (-1);
	       }
	    }

	   if (how->checking & CHK_MAXVEL)
	    { int jnts = 0;

	      if (FABS (how->angVel[0]) > jls->maxVel[0])
	       { jnts |= (1<<0);
	       }
	      if (FABS (how->angVel[1]) > jls->maxVel[1])
	       { jnts |= (1<<1);
	       }
	      if (FABS (how->angVel[2]) > jls->maxVel[2])
	       { jnts |= (1<<2);
	       }

	      if (FABS (how->angVel[3]) > jls->maxVel[3])
	       { jnts |= (1<<3);
	       }
	      if (FABS (how->angVel[4]) > jls->maxVel[4])
	       { jnts |= (1<<4);
	       }
	      if (FABS (how->angVel[5]) > jls->maxVel[5])
	       { jnts |= (1<<5);
	       }

	      if (jnts)
	       { int j;
		 float cvt;

		 rciAbort (EMaxVel,
		"velocity limit exceeded for joints 0x%x\n", jnts);

		 for (j=0; j<6; j++)
		  { if (jnts & (1<<j))
		     { break;
		     }
		  }
		 cvt = (jls->jtype[j] == 'P' ? 1.0 : RADTODEG);
		 rciPrintf ("\nangVel[%d] = %g ang[%d] = %g rate = %g\n",
			j, cvt*how->angVel[j], j, cvt*how->angVel[j], rate);
		 return (-1);
	       }
	    }

	   how->hand 	    = (short)*dp++;
	   how->handForce   = (short)*dp++;
	   how->state 	    = 0;

	   stateFlags 	    = *dp++;
	   how->power 	    = ((stateFlags & POWER_ON) != 0);
	   how->calibrated  = ((stateFlags & IS_CALIBRATED) != 0);

	   readFlags = *dp++;

	   hp = (unsigned short *)(how->pioInput);
	   for (k=how->numPioIn; k; k--)
	    { *hp++ = *dp++;
	    }
	   hp = (unsigned short *)(how->pioOutput);
	   for (k=how->numPioOut; k; k--)
	    { *hp++ = *dp++;
	    }

	   if (readFlags & MARK_FOUND)
	    { how->markFound = *dp++;
	    }

	   if (how->torqueOK = (readFlags & TORQUE_OK))
	    { how->mtorque[0] = jls->currentToMtorque[0] * (short)*dp++;
	      how->mtorque[1] = jls->currentToMtorque[1] * (short)*dp++;
	      how->mtorque[2] = jls->currentToMtorque[2] * (short)*dp++;

	      how->mtorque[3] = jls->currentToMtorque[3] * (short)*dp++;
	      how->mtorque[4] = jls->currentToMtorque[4] * (short)*dp++;
	      how->mtorque[5] = jls->currentToMtorque[5] * (short)*dp++;

	      if (how->checking & CHK_MAXTOR)
	       { int jnts = 0;
		 if (FABS (how->mtorque[0]) > jls->maxMtorque[0])
		  { jnts |= (1<<0);
		  }
		 if (FABS (how->mtorque[1]) > jls->maxMtorque[1])
		  { jnts |= (1<<1);
		  }
		 if (FABS (how->mtorque[2]) > jls->maxMtorque[2])
		  { jnts |= (1<<2);
		  }
		 if (FABS (how->mtorque[3]) > jls->maxMtorque[3])
		  { jnts |= (1<<3);
		  }
		 if (FABS (how->mtorque[4]) > jls->maxMtorque[4])
		  { jnts |= (1<<4);
		  }
		 if (FABS (how->mtorque[5]) > jls->maxMtorque[5])
		  { jnts |= (1<<5);
		  }
		 if (jnts)
		  { rciAbort (EMaxTor,
		   "maximum motor torque exceeded for joints 0x%x\n", jnts);
		    return (-1);
		  }
	       }
	    }
	   if (how->potOK = (readFlags & POT_OK))
	    { how->potInput[0] = (short)*dp++;
	      how->potInput[1] = (short)*dp++;
	      how->potInput[2] = (short)*dp++;

	      how->potInput[3] = (short)*dp++;
	      how->potInput[4] = (short)*dp++;
	      how->potInput[5] = (short)*dp++;
	    }
	   if (how->pendantOK = (readFlags & PENDNT_OK))
	    { hp = (unsigned short *)how->pendantInput;
	      if (bigEndian)
	       { hp[1] = *dp++;
		 hp[0] = *dp++;
		 hp[3] = *dp++;
	       }
	      else
	       { hp[0] = *dp++;
		 hp[1] = *dp++;
		 hp[2] = *dp++;
	       }
	    }
	   if (how->forceOK = (readFlags & FORCE_OK))
	    { if (readFlags & FORCE_DATA_FLOAT)
	       { union _float force[6]; 

		 force[0].s[0] = *dp++;
		 force[0].s[1] = *dp++;
		 force[1].s[0] = *dp++;
		 force[1].s[1] = *dp++;
		 force[2].s[0] = *dp++;
		 force[2].s[1] = *dp++;
		 force[3].s[0] = *dp++;
		 force[3].s[1] = *dp++;
		 force[4].s[0] = *dp++;
		 force[4].s[1] = *dp++;
		 force[5].s[0] = *dp++;
		 force[5].s[1] = *dp++;
   
		 how->forceInput[0] = force[0].f;
		 how->forceInput[1] = force[1].f;
		 how->forceInput[2] = force[2].f;
		 how->forceInput[3] = force[3].f;
		 how->forceInput[4] = force[4].f;
		 how->forceInput[5] = force[5].f;
	       }
	      else
	       { how->forceInput[0] = (short)*dp++;
		 how->forceInput[1] = (short)*dp++;
		 how->forceInput[2] = (short)*dp++;
   
		 how->forceInput[3] = (short)*dp++;
		 how->forceInput[4] = (short)*dp++;
		 how->forceInput[5] = (short)*dp++;
	       }
	    }
	   if (how->jstickOK = (readFlags & JSTICK_OK))
	    { how->jstickInput[0] = (short)*dp++;
	      how->jstickInput[1] = (short)*dp++;
	      how->jstickInput[2] = (short)*dp++;

	      how->jstickInput[3] = (short)*dp++;
	      how->jstickInput[4] = (short)*dp++;
	      how->jstickInput[5] = (short)*dp++;
	    }
	   if (how->teleopOK = (readFlags & TELEOP_OK))
	    { union _float teleop[6]; 

              teleop[0].s[0] = *dp++;
              teleop[0].s[1] = *dp++;
              teleop[1].s[0] = *dp++;
              teleop[1].s[1] = *dp++;
              teleop[2].s[0] = *dp++;
              teleop[2].s[1] = *dp++;
              teleop[3].s[0] = *dp++;
              teleop[3].s[1] = *dp++;
              teleop[4].s[0] = *dp++;
              teleop[4].s[1] = *dp++;
              teleop[5].s[0] = *dp++;
              teleop[5].s[1] = *dp++;

              how->teleopInput[0] = teleop[0].f;
              how->teleopInput[1] = teleop[1].f;
              how->teleopInput[2] = teleop[2].f;
              how->teleopInput[3] = teleop[3].f;
              how->teleopInput[4] = teleop[4].f;
              how->teleopInput[5] = teleop[5].f;
              how->teleopButton   = *dp++;
              how->teleopTrigger  = (short) *dp++;
	    }
	   if (readFlags & TEST_OK)
	    { int skip;

	      skip = *dp++;
	      dp += skip;
	    }
	   n = 0;
	   how->adc = how->adcSet;
	   for (k = how->adc; k; k >>= 1)
	    { if (k & 1)
	       { how->adcInput[n] = (short)*dp++;
	       }
	      n++;
	    }
	   n = 0;
	   how->slu = how->sluSet;
	   for (k = how->slu; k; k >>= 1)
	    { if (k & 1)
	       { union _pack pack;
		 register int i;
		 int count;

		 count = *dp++;
		 if (count > HOW_SLUBUF)
		  { rciAbort(ECount,
	"bad count (%d) received from slu %d\n", count, n);
		    return (-1);
		  }
		 for (i=0; i<count; i++)
		  { if ((i & 1) == 0)
		     { pack.s = *dp++;
		       how->sluInput[n].buf[i] = pack.b[BYTE0];
		     }
		    else
		     { how->sluInput[n].buf[i] = pack.b[BYTE1];
		     }
		  }
		 how->sluInput[n].num = count;
	       }
	      n++;
	    }
	 }

	return (1);
}

#define PUSH_COM0(c)    if (cmdp + 1 > top) goto overflow; \
			*cmdp++ = (c);

#define PUSH_COM1(c,x)	if (cmdp + 2 > top) goto overflow; \
			*cmdp++ = (c); \
			*cmdp++ = (x);

#define PUSH_COM2(c,x,y) if (cmdp + 3 > top) goto overflow; \
			*cmdp++ = (c); \
			*cmdp++ = (x); \
			*cmdp++ = (y);

gen_output(td, rbt)
RCI_DESC *td;
RCI_RBT *rbt;
{
	char dataBuf[MAX_DATABUF];

	RCI_TCB *tcb = td->tcb;
	IO_DEVICE *dev = (IO_DEVICE*)rbt->dev;
	HOW *how = rbt->how;
	JLS *jls = rbt->jls;
	register CHG *chg = rbt->chg;
	register unsigned short *cmdp = (unsigned short *)dataBuf;
	unsigned short *top = (unsigned short *)&dataBuf[MAX_DATABUF];
	union _int iencread;
	unsigned long jntsMask = ((1<<jls->numJ)-1);

	char *pfx = "rbtOutput() --";

	/* ---- do the control commands first ---- */

	if (tcb->newRate)
	 { PUSH_COM2 (SET_RATE_C, tcb->interval, tcb->offset);
	 }
	   
	if (chg->ctrlcom)
  	 { if (chg->paramset)
	    { register int i;
	      if (chg->numParamsPosted > HOW_MAXPARAM)
	       { rciAbort (ETooMany, 
	"%d ctrl params posted; maximum is %d\n",
		 	   chg->numParamsPosted, HOW_MAXPARAM);
		 return (-1);
	       }
	      if (cmdp + 2 + chg->numParamsPosted*2 > top) goto overflow;
	      *cmdp++ = SET_PARAM_C;
	      *cmdp++ = chg->numParamsPosted;
	      for (i=0; i<chg->numParamsPosted; i++)
	       { union _pack pack;

		 pack.b[BYTE0] = chg->pspec[i].val;
		 pack.b[BYTE1] = chg->pspec[i].num;
		 *cmdp++ = pack.s;
		 *cmdp++ = chg->pspec[i].jnts & jntsMask;
	       }
	      chg->numParamsPosted = 0;
	      chg->paramset = 0;
	    }

	   if (chg->integration.com)
	    { int mask = chg->integration.val.l & jntsMask;
	      how->integration = mask;
	      PUSH_COM1 (SET_INTEGRATION_C, mask);
	      chg->integration.com = 0;
	    }

	   if (chg->calibMode.com)
	    { int mask = chg->calibMode.val.l & jntsMask;
	      how->calibMode = mask;
	      PUSH_COM1 (SET_CALMODE_C, mask)
	      chg->calibMode.com = 0;
	    }

	   if (chg->calibflag.com)
	    { PUSH_COM1 (SET_CALFLAG_C, chg->calibflag.val.l)
	      chg->calibflag.com = 0;
	    }

	   if (chg->power.com)
	    { if (chg->power.val.i)
	       { PUSH_COM0 (ON_POWER_C);
	       }
	      else
	       { PUSH_COM0 (OFF_POWER_C);
	       }
	      chg->power.com = 0;
	    }
	    
	   if (chg->maintCom.com)
	    { PUSH_COM1 (SET_MAINT_COM_C, chg->maintCom.val.l)
	      chg->maintCom.com = 0;
	    }
	    
	   if (chg->checking.com)
	    { how->checking = chg->checking.val.l & CHK_RBTALL;
	      chg->checking.com = 0;
	    }

	   if (chg->powerOnRelease.com)
	    { how->powerOnRelease = !!chg->powerOnRelease.val.i;
	      chg->powerOnRelease.com = 0;
	    }
	    
	   chg->ctrlcom = 0;
	 }


	/* ----- now do the motion control stuff ----- */

	 { unsigned long newTorqueMode;
	   unsigned long modeChange;
	   float deltaT;
	   register int j;
	   char *illCmd;
	
	   deltaT = tcb->interval / 1000.;

	   /* check for a change in the control level */

	   if (chg->controlLevel.com)
	    { how->controlLevel = chg->controlLevel.val.i;
	      chg->controlLevel.com = 0;
	    }

	   /* look for the stop command first */

	   if (chg->stop.com)
	    { how->torqueMode = 0;
	      how->loadSettings = 0x3f;

	      for (j=0; j<jls->numJ; j++)
	       { chg->motion[j].com = 0;
		 how->mtorqueSet[j] = 0.0;
		 how->jtorqueSet[j] = 0.0;
	       }
	      PUSH_COM0 (STOP_ALL_C);
	      chg->stop.com = 0;
	    }

	   if (how->controlLevel == JOINT_LEVEL)
	    { 
	      int newEnc[MAXJNTS];
	      float newMtorque[MAXJNTS];
	      float newMtorqueBias[MAXJNTS];
	      int biasSet = NO;
	      char com[MAXJNTS];
   
	      newTorqueMode = how->torqueMode;

	      for (j=0; j<jls->numJ; j++)
	       { switch (com[j] = chg->motion[j].com)
		  { case 0:
		     { break;
		     }
		    case CHG_ANGLE:
		     { float ang = chg->motion[j].val.f;
   
		       if (how->checking & CHK_REQPOS)
			{ float cvt = (jls->jtype[j] == 'P' ? 1.0 : RADTODEG);
			  if (ang < jls->jmin[j] || ang > jls->jmax[j])
			   { rciAbort (EReqPos,
	"%s requested angle %g exceeds limits, joint %d\n",pfx, ang*cvt, j+1);
			     return (-1);
			   }
			}
		       if (how->checking & CHK_REQVEL)
			{ if (FABS(ang - how->angSet[j]) > jls->maxVel[j]*deltaT)
			   { float cvt = (jls->jtype[j]=='P'?1.0:RADTODEG);
			     rciAbort (EReqVel,
	"%s requested velocity %g exceeds velocity limits, joint %d\n", pfx,
			     cvt*FABS(ang - how->angSet[j])/deltaT, j+1);
rciPrintf ("j = %d how->angSet = %8.3f deltaT = %g ang = %8.3f\n",
			j+1, how->angSet[j]*cvt, deltaT, ang*cvt);
			     return (-1);
			   }
			}
		       how->angSet[j] = ang;
		       how->jtorqueSet[j] = 0.0;
		       newTorqueMode &= ~(1<<j);
		       break;
		     }
		    case CHG_JTORQUE:
		     {
		       how->jtorqueSet[j] = chg->motion[j].val.f;
		       newTorqueMode |= (1<<j);
		       break;
		     }
		    case CHG_STOP:
		     {
		       how->jtorqueSet[j] = 0.0;
		       newTorqueMode &= ~(1<<j);
		       break;
		     }
		    case CHG_ANGLE_VALUE:
		     {
		       rciAbort (EBadCmd,
	"%s SET ANGLE VALUE not currently supported\n");
		       return (-1);
		       break;
		     }
		    case CHG_ENCODER:
		     { illCmd = "SET ENCODER";
		       break;
		     }
		    case CHG_ENCODER_VALUE:
		     { illCmd = "SET ENCODER VALUE";
		       break;
		     }
		    case CHG_MTORQUE:
		     { illCmd = "SET MTORQUE";
		       rciAbort (EBadCmd,
     "%s illegal joint level motion command %s\n", pfx, illCmd);
		       return (-1);
		       break;
		     }
		    default:
		     { rciAbort (EBadCmd,
     "%s illegal joint level motion command (%d)\n", pfx, com[j]);
		       return (-1);
		       break;
		     }
		  }
		 chg->motion[j].com = 0;

		 switch (chg->bias[j].com)
		  { case 0:
		     { break;
		     }
		    case CHG_JTORQUE_BIAS:
		     { how->jtorqueBias[j] = chg->bias[j].val.f;
		       biasSet = YES;
		       break;
		     }
		    case CHG_MTORQUE_BIAS:
		     { illCmd = "SET MTORQUE BIAS";
		       rciAbort (EBadCmd,
     "%s illegal joint level bias command %s\n", pfx, illCmd);
		       return (-1);
		       break;
		     }
		    default:
		     { rciAbort (EBadCmd,
     "%s illegal joint level bias command (%d)\n", pfx, chg->bias[j].com);
		       return (-1);
		       break;
		     }
		  }		    
		 chg->bias[j].com = 0;
	       }

	      modeChange = newTorqueMode ^ how->torqueMode;
	      how->torqueMode = newTorqueMode;
			  
	      (*jls->angleToEncoder)(newEnc, how->angSet, jls);
	      if (how->torqueMode)
	       { (*jls->jtorqueToMtorque)(newMtorque, how->jtorqueSet, jls);
	       }
	      if (biasSet)
	       { (*jls->jtorqueToMtorque)(newMtorqueBias,how->jtorqueBias,jls);
	       }
	      
	      /* now go and dispatch the commands */
   
	      for (j=0; j<jls->numJ; j++)
	       { float newBias;

		 if (how->torqueMode & (1<<j))
		  { float mtor = newMtorque[j];
		    if (mtor != how->mtorqueSet[j] || modeChange & (1<<j))
		     { if (how->checking & CHK_REQTOR)
			{ if (FABS(mtor) > jls->maxMtorque[j])
			   { rciAbort (EReqTor,
      "%s requested motor torque %f exceeds bounds, joint %d\n",pfx,mtor,j+1);
			     return (-1);
			   }
			}
		       PUSH_COM1 (MOVE_CUR_C | j, 
				  mtor*jls->mtorqueToCurrent[j] +
				  jls->currentBias[j]);
		     }
		    how->encSet[j] = newEnc[j];
		    how->mtorqueSet[j] = mtor;
		  }
		 else
		  { if (newEnc[j] != how->enc[j] || modeChange & (1<<j))
		     { PUSH_COM1 (MOVE_ENC_C | j, newEnc[j]);
		     }
		    how->encSet[j] = newEnc[j];
		    how->mtorqueSet[j] = how->torqueMode ? newMtorque[j]:0.0;
		  }
		 if (biasSet &&
		     (newBias = newMtorqueBias[j]) != how->mtorqueBias[j])
		  { if (how->checking & CHK_REQTOR)
		     { if (FABS(newBias) > jls->maxMtorque[j])
			{ rciAbort (EReqTor,
      "%s requested motor torque bias %f exceeds bounds, joint %d\n",
				    pfx, newBias, j+1);
			  return (-1);
			}
		     }
		    PUSH_COM1 (BIAS_CUR_C | j, newBias);
		    how->mtorqueBias[j] = newBias;
		  }	    
	       }
	      how->loadSettings |= newTorqueMode;
	    }
	   else 	/* control level == MOTOR LEVEL */
	    { int newEnc[MAXJNTS];
	      float newAng[MAXJNTS];
	      float newMtorque[MAXJNTS];
	      int biasSet = NO;
	      int encValueSet = NO;
	      char com[MAXJNTS];
   
	      newTorqueMode = how->torqueMode;

	      for (j=0; j<jls->numJ; j++)
	       { switch (com[j] = chg->motion[j].com)
		  { case 0:
		     { newEnc[j] = how->encSet[j];
		       newMtorque[j] = how->mtorqueSet[j];
		       break;
		     }
		    case CHG_ENCODER:
		     { newEnc[j] = chg->motion[j].val.i;
		       newMtorque[j] = 0.0;
		       newTorqueMode &= ~(1<<j);
		       break;
		     }
		    case CHG_MTORQUE:
		     { newEnc[j] = how->encSet[j];
		       newMtorque[j] = chg->motion[j].val.f;
		       newTorqueMode |= (1<<j);
		       break;
		     }
		    case CHG_STOP:
		     { newEnc[j] = how->encSet[j];
		       newMtorque[j] = 0.0;
		       newTorqueMode &= ~(1<<j);
		       break;
		     }
		    case CHG_ENCODER_VALUE:
		     { newEnc[j] = chg->motion[j].val.i;
		       newMtorque[j] = 0.0;
		       newTorqueMode &= ~(1<<j);

		       how->enc[j] = chg->motion[j].val.i;
		       how->encSet[j] = chg->motion[j].val.i;
		       encValueSet = YES;
		       break;
		     }
		    case CHG_ANGLE:
		     { illCmd = "SET ANGLE";
		       break;
		     }
		    case CHG_ANGLE_VALUE:
		     { illCmd = "SET ANGLE VALUE";
		       break;
		     }
		    case CHG_JTORQUE:
		     { illCmd = "SET JTORQUE";
		       rciAbort (EBadCmd,
     "%s illegal motor level motion command %s\n", pfx, illCmd);
		       return (-1);
		       break;
		     }
		    default:
		     { rciAbort (EBadCmd,
     "%s illegal motor level motion command (%d)\n", pfx, com[j]);
		       return (-1);
		       break;
		     }
		  }

		 switch (chg->bias[j].com)
		  { case 0:
		     { break;
		     }
		    case CHG_MTORQUE_BIAS:
		     { float newBias;

		       biasSet = YES;
		       newBias = chg->bias[j].val.f;
			{ if (how->checking & CHK_REQTOR)
			   { if (FABS(newBias) > jls->maxMtorque[j])
			      { rciAbort (EReqTor,
	    "%s requested motor torque bias %f exceeds bounds, joint %d\n",
					  pfx, newBias, j+1);
				return (-1);
			      }
			   }
			}
		       PUSH_COM1 (BIAS_CUR_C | j, newBias);
		       how->mtorqueBias[j] = newBias;
		       break;
		     }
		    case CHG_JTORQUE_BIAS:
		     { illCmd = "SET JTORQUE BIAS";
		       rciAbort (EBadCmd,
     "%s illegal motor level bias command %s\n", pfx, illCmd);
		       return (-1);
		       break;
		     }
		    default:
		     { rciAbort (EBadCmd,
     "%s illegal motor level bias command (%d)\n", pfx, chg->bias[j].com);
		       return (-1);
		       break;
		     }
		  }		    
		 chg->bias[j].com = 0;
	       }

	      if (encValueSet)
	       { (*jls->encoderToAngle)(how->ang, how->enc, jls);
		 (*jls->encoderToAngle)(how->angSet, how->encSet, jls);
	       }
	      modeChange = newTorqueMode ^ how->torqueMode;
	      how->torqueMode = newTorqueMode;
   
	      (*jls->encoderToAngle)(newAng, newEnc, jls);
	      if (how->torqueMode)
	       { (*jls->mtorqueToJtorque)(how->jtorqueSet, newMtorque, jls);
	       }
	      else
	       { how->jtorqueSet[0] = 0.0;
		 how->jtorqueSet[1] = 0.0;
		 how->jtorqueSet[2] = 0.0;

		 how->jtorqueSet[3] = 0.0;
		 how->jtorqueSet[4] = 0.0;
		 how->jtorqueSet[5] = 0.0;
	       }
	      if (biasSet)
	       { (*jls->mtorqueToJtorque)(how->jtorqueBias, 
					  how->mtorqueBias, jls);
	       }

	      for (j=0; j<jls->numJ; j++)
	       { 		 
		 if (how->checking & CHK_REQPOS)
		  { if (newAng[j] < jls->jmin[j] || newAng[j] > jls->jmax[j])
		     { float cvt = (jls->jtype[j] == 'P' ? 1.0 : RADTODEG);
		       rciAbort (EReqPos,
	"%s requested angle %g exceeds limits, joint %d\n",pfx,
		       cvt*newAng[j],j+1);
		       return (-1);
		     }
		  }
		 if (how->checking & CHK_REQVEL)
		  { if (FABS(newAng[j] - how->angSet[j]) > jls->maxVel[j]*deltaT)
		     { float cvt = (jls->jtype[j] == 'P' ? 1.0 : RADTODEG);
		       rciAbort (EReqVel,
	"%s requested velocity %g exceeds limits, joint %d\n", pfx,
		       cvt*FABS(newAng[j] - how->angSet[j])/deltaT, j+1);
		       return (-1);
		     }
		  }

		 if (how->torqueMode & (1<<j))
		  { float mtor = newMtorque[j];
		    if (mtor != how->mtorqueSet[j] || modeChange&(1<<j))
		     { if (how->checking & CHK_REQTOR)
			{ if (FABS(mtor) > jls->maxMtorque[j])
			   { rciAbort (EReqTor,
  "%s requested motor torque %f exceeds bounds, joint %d\n",pfx,mtor,j+1);
			     return (-1);
			   }
			}
		       PUSH_COM1 (MOVE_CUR_C | j, 
				  mtor*jls->mtorqueToCurrent[j] +
				  jls->currentBias[j]);
		     }
		  }
		 else
		  { if (com[j] == CHG_ENCODER_VALUE)
		     { PUSH_COM1 (LOAD_ENC_C | j, chg->motion[j].val.i);
		       how->calibMode &= ~(1<<j);
		     }
		    else if (com[j] == CHG_STOP)
		     { how->loadSettings |= (1<<j);
		       PUSH_COM0 (STOP_JNT_C | j);
		     }
		    else if (newEnc[j] != how->enc[j] || modeChange&(1<<j))
		     { PUSH_COM1 (MOVE_ENC_C | j, newEnc[j]);
		     }
		  }
		 how->mtorqueSet[j] = newMtorque[j];
		 how->encSet[j] = newEnc[j];
		 how->angSet[j] = newAng[j];
		 chg->motion[j].com = 0;
	       }
	      how->loadSettings |= how->torqueMode;
	    }
	
	   if (chg->hand.com)
	    { switch (chg->hand.com)
	       { case CHG_HANDPOS:
		  { PUSH_COM1 (HAND_POS_C, chg->hand.val.i);
		    break;
		  }
		 case CHG_HANDFORCE:
		  { PUSH_COM1 (HAND_FORCE_C, chg->hand.val.i);
		    break;
		  }
		 default:
		  { rciAbort (EBadCmd, 
	"illegal command code (%d) specified for hand\n", chg->hand.com);
		    return (-1);
		  }
	       }
	      chg->hand.com = 0;
	    }
	 }
       
	if (chg->readcom)
 	 {
	   if (chg->torque.com)
	    { PUSH_COM1 (TORQUE_READ_C, chg->torque.val.i);
	      chg->torque.com = 0;
	    }

	   if (chg->pot.com)
	    { PUSH_COM1 (POT_READ_C, chg->pot.val.i);
	      chg->pot.com = 0;
	    }

	   if (chg->pendant.com)
	    { PUSH_COM1 (PENDNT_READ_C, chg->pendant.val.i);
	      chg->pendant.com = 0;
	    }

	   if (chg->force.com)
	    { PUSH_COM1 (FORCE_READ_C, chg->force.val.i);
	      chg->force.com = 0;
	    }

	   if (chg->teleop.com)
	    { PUSH_COM1 (TELEOP_READ_C, chg->teleop.val.i);
	      chg->teleop.com = 0;
	    }

	   if (chg->jstick.com)
	    { PUSH_COM1 (JSTICK_READ_C, chg->jstick.val.i);
	      chg->jstick.com = 0;
	    }

	   if (chg->adc.com)
	    { unsigned long mask = chg->adc.val.l & ((1<<how->numAdc)-1);
	      PUSH_COM1 (ADC_CHAN_C, mask);
	      how->adcSet = mask;
	      chg->adc.com = 0;
	    }

	   if (chg->slu.com)
	    { unsigned long mask = chg->slu.val.l & ((1<<how->numSlu)-1);
	      PUSH_COM1 (SLU_CHAN_C, mask);
	      how->sluSet = mask;
	      chg->slu.com = 0;
	    }

	   if (chg->setTest.com)
	    { PUSH_COM1 (TEST_READ_C, chg->setTest.val.i);
	      chg->setTest.com = 0;
	    }

	   chg->readcom = 0;
	 }

	if (chg->writecom)
  	 { int n;

	   for (n=0; n<how->numPioOut; n++)
	    { int cmd;
	      if (cmd = chg->pioOutput[n].com)
	       { switch (cmd)
		  { case CHG_PUT:
		     { PUSH_COM1 (PUT_PIO_C | n, chg->pioOutput[n].val.s[0]);
		       break;
		     }
		    case CHG_SET:
		     { PUSH_COM1 (SET_PIO_C | n, chg->pioOutput[n].val.s[0]);
		       break;
		     }
		    case CHG_CLEAR:
		     { PUSH_COM1 (CLEAR_PIO_C | n, chg->pioOutput[n].val.s[0]);
		       break;
		     }
		  }
		 chg->pioOutput[n].com = 0;
	       }
	    }

	   for (n=0; n<how->numDac; n++)
	    { if (chg->dacOutput[n].com)
	       { int value = chg->dacOutput[n].val.s[0];
		 PUSH_COM1 (SET_DAC_C | n, value);
		 chg->dacOutput[n].com = 0;
		 how->dacOutput[n] = value;
	       }
	    }

	   for (n=0; n<how->numSlu; n++)
	    { if (chg->sluOutput[n].com)
	       { union _pack pack;
		 register int i;
		 int count = chg->sluOutput[n].num;

		 if (cmdp + 2 + (count+1)/2 > top) goto overflow;

		 *cmdp++ = (SET_SLU_C | n);
		 *cmdp++ = count;
		 for (i=0; i<count; i++)
		  { if ((i & 1) == 0)
		     { pack.b[BYTE0] = chg->sluOutput[n].buf[i];
		     }
		    else
		     { pack.b[BYTE1] = chg->sluOutput[n].buf[i];
		       *cmdp++ = pack.s;
		     }
		  }
		 if ((i & 1) != 0)
		  { *cmdp++ = pack.s;
		  }
		 chg->sluOutput[n].com = 0;
	       }
	    }
   
	   if (chg->pendantMsg.com)
	    { union _pack pack;
	      /* Don't byte swap this because we are passing an 
		 entire word */
	      pack.b[0] = chg->pendantMsg.buf[0];
	      pack.b[1] = chg->pendantMsg.buf[1];
	      PUSH_COM1 (SET_PENDANT_C, pack.s);
	      chg->pendantMsg.com =0;
	    }

	   if (chg->putTest.com)
	    { PUSH_COM1 (PUT_TEST_C, chg->putTest.val.i);
	      chg->putTest.com = 0;
	    }

	   chg->writecom = 0;
	 }

	if (chg->putVectorSet)
	 { union _float fvalue;
	   int size;
	   register int j;

	   size = chg->putVectorSet;
	   PUSH_COM1 (PUT_VECTOR_C, size);
	   if (cmdp + size*sizeof(float)/sizeof(short) > top) goto overflow;
	   for (j=0; j<size; j++)
	    { fvalue.f = chg->putVector[j];
	      *cmdp++ = fvalue.s[0];
	      *cmdp++ = fvalue.s[1];
	    }
	   chg->putVectorSet = 0;
	 }

	 { int n = (char*)cmdp - dataBuf;

	   rbt->notice = DOING_IO;
	   if ((*dev->write) (dev, dataBuf, n) != n)
  	    { errorMsg (getErrorCode(0), "%s error writing to robot\n",pfx);
	      rciAbortErrors();
	      return (-1);
	    }
	   rbt->notice = ACTIVATED;
	 }

	return (1);

overflow:
	rciAbort (ETooMany, "too many commands sent to robot\n");
	return (-1);
}

#define WAKEUP_SIG	1
#define RELEASE_SIG	2

gen_signal (dev)
IO_DEVICE *dev;
{
	if ((*dev->signal)(dev, WAKEUP_SIG) < 0)
	 { errorMsg (getErrorCode(0), "rbtSignal() -- can't signal robot\n");
	   rciAbortErrors();
	   return (-1);
	 }
	return (1);
}	

gen_startup (td, rbt)
RCI_DESC *td;
RCI_RBT *rbt;
{
	int n;
	char dataBuf[MAX_DATABUF];

	RCI_TCB *tcb = td->tcb;
	IO_DEVICE *dev = (IO_DEVICE*)rbt->dev;
 	register HOW *how = rbt->how;
	JLS *jls = rbt->jls;
	register unsigned short *dp = (unsigned short *)dataBuf;
	register unsigned short *cmdp = (unsigned short *)dataBuf;
	CHG *chg = rbt->chg;

	int encSet[6];
	float angSet[6];

	char *pfx = "rbtStartup() --";

	union _pack pack;

	bigEndian = testForBigEndian();

	if ((*dev->probe)(dev) < 0)
	 { rciAbort (ENoDev, "%s IO probe failed at address 0x%x\n", pfx,
		     dev->address);
	   return (-1);
	 }

	/* set any necessary commands */

	if (chg->checking.com)
	 { how->checking = chg->checking.val.l & CHK_RBTALL;
	   chg->checking.com = 0;
	 }

	if (chg->powerOnRelease.com)
	 { how->powerOnRelease = !!chg->powerOnRelease.val.i;
	   chg->powerOnRelease.com = 0;
	 }

	/* interrupt the robot and get startup information */

	rbt->notice = DOING_IO;
	if ((*dev->attach)(dev) < 0)
	 { errorMsg (getErrorCode(0), "%s cannot connect to robot\n",pfx);
	   rciAbortErrors();
	   return (-1);
	 }
	if ((n = (*dev->read) (dev, dataBuf, MAX_DATABUF)) < 0)
	 { errorMsg (getErrorCode(0), "%s error reading from robot\n",pfx);
	   rciAbortErrors();
	   return (-1);
	 }	 

	if (n != STARTUP_COUNT)
	 { rciAbort (ECount, "%s received %d vs. %d bytes from robot\n", 
		     pfx, n, STARTUP_COUNT);
	   return (-1);
	 }

	if (*dp++ != 0x5555)
  	 { rciAbort (ETransmit, "%s illegal test word (0x%hx vs. 0x%hx)\n", 
		     pfx, *--dp, 0x5555);
	   return (-1);
	 }
	if (*dp++ != 0xaaaa)
  	 { rciAbort (ETransmit, "%s illegal test word (0x%hx vs. 0x%hx)\n", 
		     pfx, *--dp, 0xaaaa);
	   return (-1);
	 }

	rbt->notice = ACTIVATED;

	how->robotID = *dp++;
	how->controllerID = *dp++;
	how->options = *dp++; 
	pack.s = *dp++;
	if ((how->numAdc = pack.b[BYTE0]) > HOW_MAXADC)
  	 { rciAbort (ETooMany,
	"%s robot has too many ADC channels (%d vs. %d)\n",
	   pfx, pack.b[BYTE0], HOW_MAXADC);
	   return (-1);
	 }
	if ((how->numDac = pack.b[BYTE1]) > HOW_MAXDAC)
  	 { rciAbort (ETooMany,
	"%s robot has too many DAC channels (%d vs. %d)\n",
	   pfx, pack.b[BYTE1], HOW_MAXDAC);
	   return (-1);
	 }
 	pack.s = *dp++;
	if ((how->numPioIn = pack.b[BYTE0]) > HOW_MAXPIO)
  	 { rciAbort (ETooMany,
	"%s robot has too many PIOin channels (%d vs. %d)\n",
	   pfx, pack.b[BYTE0], HOW_MAXPIO);
	   return (-1);
	 }
	if ((how->numPioOut = pack.b[BYTE1]) > HOW_MAXPIO)
  	 { rciAbort (ETooMany,
	"%s robot has too many PIOout channels (%d vs. %d)\n",
	   pfx, pack.b[BYTE1], HOW_MAXPIO);
	   return (-1);
	 }
	pack.s = *dp++;
	if ((how->numSlu = pack.b[BYTE0]) > HOW_MAXSLU)
  	 { rciAbort (ETooMany,
	"%s robot has too many SLU channels (%d vs. %d)\n",
	   pfx, pack.b[BYTE0], HOW_MAXSLU);
	   return (-1);
	 }
	how->integration = pack.b[BYTE1];
	pack.s = *dp++;
 	how->calibMode = pack.b[BYTE0];
	how->markFound = pack.b[BYTE1];

#if READ_ENCSET

	encSet[0] = *dp++;
	encSet[1] = *dp++;
	encSet[2] = *dp++;

	encSet[3] = *dp++;
	encSet[4] = *dp++;
	encSet[5] = *dp++;

	(*jls->encoderToAngle)(angSet, encSet, jls);	   
 
	how->lastAngSet[0] = angSet[0];
	how->lastAngSet[1] = angSet[1];
	how->lastAngSet[2] = angSet[2];

	how->lastAngSet[3] = angSet[3];
	how->lastAngSet[4] = angSet[4];
	how->lastAngSet[5] = angSet[5];

	how->lastEncSet[0] = encSet[0];
	how->lastEncSet[1] = encSet[1];
	how->lastEncSet[2] = encSet[2];

	how->lastEncSet[3] = encSet[3];
	how->lastEncSet[4] = encSet[4];
	how->lastEncSet[5] = encSet[5];

#endif	

	if (rciCpu == 0)
	 { (*dev->ioctl)(dev, IOC_SIGINTER_ENABLE);
	 }

	cmdp = (unsigned short *)dataBuf;

	*cmdp++ = SET_RATE_C;
	*cmdp++ = tcb->interval;
	*cmdp++ = tcb->offset;
	*cmdp++ = SET_RUNREQ_C;
 	*cmdp++ = tcb->maxLagCount;
	*cmdp++ = START_C;
	if (tcb->scheduleDisp == ON_FUNCTION)
	 { *cmdp++ = MASTER_F;
	 }
	else
	 { *cmdp++ = 0;
	 }

/* *** FOR_SIMULATOR: tell all the robots being used */

	/* XXX do this for simulator only now cause real moper
	   doesn't know this protocol */
	if (td->tcb->scheduleDisp == ON_SIGNAL)
	 { unsigned short *comword;
	   RCI_RBT *r;
	   int k;
	
	   comword = cmdp++;
	   for (k=0; k<MAX_TASKROBOTS; k++)
	    { if (r = TASKROBOT(td,k))
	       { *cmdp++ = r->id;	/* XXX id should be in JLS */
	       }
	    }
	   *comword = ROBOT_IDS_C | (cmdp - comword - 1);
	 }

/* *** */

	n = (int)((char *)cmdp - dataBuf);

	rbt->notice = DOING_IO;
	if ((*dev->write) (dev, dataBuf, n) != n)
  	 { errorMsg (getErrorCode(0), "%s error writing to robot\n", pfx);
	   rciAbortErrors();
	   return (-1);
	 }

	rbt->notice = ACTIVATED;

	rbt->ready = dev->testRead;
	rbt->readyArg = (int)dev;

	rbt->wakeup = gen_signal;
	rbt->wakeupArg = (int)dev;

	return (1);
}

gen_release (td, rbt, code)
RCI_DESC *td;
RCI_RBT *rbt;
int code;
{
	/* Send the release signal to the robot.
	   Send down release commands.
	   Read back information, if any.
	*/

	RCI_TCB *tcb = td->tcb;
	IO_DEVICE *dev = (IO_DEVICE*)rbt->dev;
	HOW *how = rbt->how;

	char *pfx = "rbtRelease() --";

	char dataBuf[MAX_DATABUF];
	unsigned short *cmdp = (unsigned short *)dataBuf;
	int retval = 0;
	int n;

	/* clear how->powerOnRelease if code is not normal release request */
	
	if (code == RBT_HARDERROR)
	 { how->powerOnRelease = NO;
	 }

	if ((rbt->notice == DOING_IO) ||
	    (rbt->notice == NO_NOTIFY) ||
	    (rbt->notice == 0) ||
	    (dev->location == 0))
	 { goto releaseEnd;
	 }
	if ((*dev->signal)(dev, RELEASE_SIG) < 0)
	 { errorMsg (getErrorCode(0), "%s can't signal to controller\n",pfx);
	   rciAbortErrors();
	   retval = -1;
	   goto releaseEnd;
	 }
	*cmdp++ = STOP_ALL_C;
	if (how->powerOnRelease)
	 { how->powerOnRelease = NO;
	 }
	else
	 { *cmdp++ = OFF_POWER_C;
	 }

	n = (int)((char *)cmdp - dataBuf);
	if ((*dev->write) (dev, dataBuf, n) != n)
	 { errorMsg (getErrorCode(0), "%s error writing to robot\n",pfx);
	   rciAbortErrors();
	   retval = -1;
	   goto releaseEnd;
	 }

	if ((n = (*dev->read) (dev, dataBuf, MAX_DATABUF)) < 0)
	 { errorMsg (getErrorCode(0), "%s error reading from robot\n", pfx);
	   rciAbortErrors();
	   retval = -1;
	   goto releaseEnd;
	 }
	if (n/sizeof(short) == 1)
	 { short errno = *(short *)dataBuf;
	   rciAbort (ERobotError, "%s controller reports error, code 0x%x\n",
		     pfx, (int)errno);
	   retval = -1;
	   goto releaseEnd;
	 }
	if (n != 0)
	 { rciAbort (ECount, "%s bad count on reply from controller\n", pfx);
	   retval = -1;
	   goto releaseEnd;
	 }
releaseEnd:
	rbt->notice = 0;
	(*dev->detach)(dev);
	bzero ((char*)rbt->chg, sizeof(CHG));
	return (retval);
}

char *genErrorString (code)
int code;
{
	switch (code)
	 { 
	   case MPR_BADRATE_E:
	    { return ("can't support sample rate");
	      break;
	    }
	   case MPR_TIMEOUT_E:
	    { return ("timeout waiting for host");
	      break;
	    }
	   case MPR_MEMINIT_E:
	    { return ("error intializing memory");
	      break;
	    }
	   case MPR_LIMIT_E:
	    { return ("moper level joint limit error");
	      break;
	    }
	   case MPR_ESTOP_E:
	    { return ("emergency stop switch set");
	      break;
	    }
	   case MPR_GAINS_E:
	    { return ("illegal controller gain values");
	      break;
	    }
	   case MPR_INTERNAL_E:
	    { return ("moper internal software error");
	      break;
	    }
	   case HST_TIMEOUT_E:
	    { return ("timeout talking to host");
	      break;
	    }
	   case HST_HEADER_E:
	    { return ("bad header from host");
	      break;
	    }
	   case HST_CHKSUM_E:
	    { return ("checksum error on data from host");
	      break;
	    }
	   case HST_COUNT_E:
	    { return ("bad count on packet from host");
	      break;
	    }
	   case HST_SYNC_E:
	    { return ("lost sync on communication with host");
	      break;
	    }
	   case HST_BADCMD_E:
	    { return ("garbage command or ACK from host");
	      break;
	    }
	   case SRV_TIMEOUT_E:
	    { return ("timeout talking to servo level");
	      break;
	    }
	   case SRV_HEADER_E:
	    { return ("bad header from servo level");
	      break;
	    }
	   case SRV_CHKSUM_E:
	    { return ("checksum error on data from servo level");
	      break;
	    }
	   case SRV_COUNT_E:
	    { return ("bad count on packet from servo level");
	      break;
	    }
	   case SRV_SYNC_E:
	    { return ("lost sync on communication with servos");
	      break;
	    }
	   case SRV_BADCMD_E:
	    { return ("garbage command or ACK from servo level");
	      break;
	    }
	   case SLE_TIMEOUT_E:
	    { return ("joint servo timed out talking to moper");
	      break;
	    }
	   case SLE_HEADER_E:
	    { return ("servo level got bad header from moper");
	      break;
	    }
	   case SLE_CHKSUM_E:
	    { return ("servo level checksum error on data from moper");
	      break;
	    }
	   case SLE_COUNT_E:
	    { return ("servo level got bad packet count from moper");
	      break;
	    }
	   case SLE_SYNC_E:
	    { return ("servo level lost sync with moper");
	      break;
	    }
	   case SLE_BADCMD_E:
	    { return ("servo level got bad command from moper");
	      break;
	    }
	   case SLE_LIMIT_E:
	    { return ("servo level joint limit error");
	      break;
	    }
	   case SLE_POWERAMP_E:
	    { return ("power amp failure at servo level");
	      break;
	    }
	   case SLE_ENCODERS_E:
	    { return ("encoder failure detected at servo level");
	      break;
	    }
	   case SLE_BRAKES_E:
	    { return ("brakes release is enabled");
	      break;
	    }
	   default:
	    { return ("unknown error code");
	      break;
	    }
	 }
}

static testForBigEndian()
{
	union _pack swap;
	swap.s = 1;
	return (swap.b[1] == 1);
}
