/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1991 by John E. Lloyd.				    *
 *									    *
 *      Modified by Matthew Stein June 2001
 *
 *	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/puma_kynvar.h"
#include "../h/robot_jls.h"
#include "../h/priDevice.h"
#include "../h/robotMath.h"
#include "../h/rciMonitor.h"
#include "../h/rciMacros.h"
#include "../h/IOdevice.h"
/* MS */
#include "../h/mbuff.h"
#include "../h/controlLevel.h"
#include "../h/pumaInterface.h"



extern int rciCpu;
static robot_JLS_type *robot_JLS;
extern int puma_readytest();

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

#define PUMAJ_MASK	((1<<PUMAJ)-1)

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

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

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

  float ang[6];
  int   enc[6];
  register int n, k;
  register float rate;
  unsigned short *hp;
  int readFlags;
  int j;


  enc[0] = robot_JLS->pos[0];
  enc[1] = robot_JLS->pos[1];
  enc[2] = robot_JLS->pos[2];
  enc[3] = robot_JLS->pos[3];
  enc[4] = robot_JLS->pos[4];
  enc[5] = robot_JLS->pos[5];

  /* ---- 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;
       
       rciAbort (EMaxVel,
		 "velocity limit exceeded for joints 0x%x\n", jnts);
       
       for (j=0; j<6; j++)
	 { if (jnts & (1<<j))
	   { break;
	   }
	 }
       rciPrintf ("\nangVel[%d] = %g ang[%d] = %g rate = %g\n",
		 j, how->angVel[j]*RADTODEG,
		  j, how->ang[j]*RADTODEG,
		  rate);
       return (-1);
    }
    }
  
  how->hand 	    = (short)robot_JLS->discrete & HAND_OPEN_BIT;
  how->handForce   = 0;
  how->state 	    = 0;
	   
  how->power 	    = ((robot_JLS->status & POWER_ON_MASK) != 0);
  how->calibrated  = ((robot_JLS->status & CALIB_BIT) != 0);
	   
  readFlags = robot_JLS->readFlags;
	   
  how->markFound = ((~(robot_JLS->status >> 8)) & 0x003f);


  how->torqueOK = 1;
  /* Triident card does not have individual torque checking */
  /* MS Pot reading needs to be turned ON and OFF with SET_POT_READ.  Only
     copy pot vales if pot is currently being read.
  */
  if (how->potOK && (robot_JLS->readFlags == JLS_POT_READ))
    { how->potInput[0] = robot_JLS->pot[0];
    how->potInput[1] = robot_JLS->pot[1];
    how->potInput[2] = robot_JLS->pot[2];
    
    how->potInput[3] = robot_JLS->pot[3];
    how->potInput[4] = robot_JLS->pot[4];
    how->potInput[5] = robot_JLS->pot[5];
    }
  
  how->pendantOK = 1; /* Trident card will not read the pendant */ 
  how->forceOK = 1;   /* ... or the force sensor */ 
  how->jstickOK =  1; /* ... or the joystick */
  how->adc = 0; /* ... or an A/D converter */
  how->slu = 0;  /* ... or SLU */
  /* MS These devices (e.g. pendant) will be read directly
     by other devices on the PC and not through the trident 
     TRC004 card */
  return (1);
}


puma_output(td, rbt)
RCI_DESC *td;
RCI_RBT *rbt;
{

	RCI_TCB *tcb = td->tcb;
	HOW *how = rbt->how;
	JLS *jls = rbt->jls;
	register CHG *chg = rbt->chg;

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

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

	if (tcb->newRate)
	  { /* Rate change not supported */
	 }
	   
	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);
	       }
	      /* MS Need to work on parameter changes */
	      chg->numParamsPosted = 0;
	      chg->paramset = 0;
	    }

	   if (chg->integration.com)
	    { int mask = chg->integration.val.l & PUMAJ_MASK;
	      how->integration = mask;
	      /* MS don't know what this is
		 PUSH_COM1 (SET_INTEGRATION_C, mask);
	      */
	      chg->integration.com = 0;
	    }

	   if (chg->calibMode.com)
	    { int mask = chg->calibMode.val.l & PUMAJ_MASK;
	      how->calibMode = mask;
	      /* Don't know what this is 
	      PUSH_COM1 (SET_CALMODE_C, mask)
	      */
	      chg->calibMode.com = 0;
	    }

	   if (chg->calibflag.com)
	     { 	      
	       robot_JLS->discrete=robot_JLS->discrete | CALIB_BIT;
	      robot_JLS->cl_request=JLS_WRITE_DES;
	       chg->calibflag.com = 0;
	     }
	   
	   if (chg->power.com)
	    { if (chg->power.val.i)
	       { robot_JLS->discrete=robot_JLS->discrete | POWER_BIT;
	       }
	      else
	       { robot_JLS->discrete=robot_JLS->discrete & ~POWER_BIT;
	       }
	      chg->power.com = 0;
	      robot_JLS->cl_request=JLS_WRITE_DES;
	    }
	    
	   if (chg->maintCom.com)
	    { /* Don't know what this is
		 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<PUMAJ; j++)
	       { chg->motion[j].com = 0;
		 how->mtorqueSet[j] = 0.0;
		 how->jtorqueSet[j] = 0.0;
	       }
	      robot_JLS->cl_request=JLS_STOP_REQ;
	      chg->stop.com = 0;
	    }

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

	      for (j=0; j<PUMAJ; 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)
			{ if (ang < jls->jmin[j] || ang > jls->jmax[j])
			   { rciAbort (EReqPos,
	"%s requested angle %g exceeds limits, joint %d\n",pfx,
			     ang*RADTODEG, j+1);
			     return (-1);
			   }
			}
		       if (how->checking & CHK_REQVEL)
			{ if (FABS(ang - how->angSet[j]) > jls->maxVel[j]*deltaT)
			   { rciAbort (EReqVel,
	"%s requested velocity %g exceeds velocity limits, joint %d\n", pfx,
			     RADTODEG*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]*RADTODEG, deltaT, 
			ang*RADTODEG);
			     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 for PUMAs\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<PUMAJ; 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);
			   }
			}
		     /* MS Potential future implementation.  Trident
			TRC004 does allow writes to the DAC specifying
			motor torque.  Leave alone for now */
		     /* 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))
		     { 
		       /* MS HERE IT IS. Here is where new setpoint is
			  sent to the JLS causing robot to move
		       */
		       robot_JLS->setpt[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);
			}
		     }
		  /* Future implementation PUSH_COM1 (BIAS_CUR_C | j, newBias);
		   */
		    how->mtorqueBias[j] = newBias;
		  }	    
	       }
	      how->loadSettings |= newTorqueMode;
	    }
	   else 	/* control level == MOTOR LEVEL */
	    { int newEnc[PUMAJ];
	      float newAng[PUMAJ];
	      float newMtorque[PUMAJ];
	      int biasSet = NO;
	      int encValueSet = NO;
	      char com[PUMAJ];
   
	      newTorqueMode = how->torqueMode;

	      for (j=0; j<PUMAJ; 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);
			      }
			   }
			}
			/* Potential future implementation 
			   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<PUMAJ; j++)
	       { 		 
		 if (how->checking & CHK_REQPOS)
		  { if (newAng[j] < jls->jmin[j] || newAng[j] > jls->jmax[j])
		     { rciAbort (EReqPos,
	"%s requested angle %g exceeds limits, joint %d\n",pfx,
		       RADTODEG*newAng[j],j+1);
		       return (-1);
		     }
		  }
		 if (how->checking & CHK_REQVEL)
		  { if (FABS(newAng[j] - how->angSet[j]) > jls->maxVel[j]*deltaT)
		     { rciAbort (EReqVel,
	"%s requested velocity %g exceeds limits, joint %d\n", pfx,
		       RADTODEG*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);
			   }
			}
		     /* Again, potential future implementation.  There is
			the ability to write to DACs 
		       PUSH_COM1 (MOVE_CUR_C | j, 
				  mtor*jls->mtorqueToCurrent[j] +
				  jls->currentBias[j]);
		     */
		     }
		  }
		 else
		  { if (com[j] == CHG_ENCODER_VALUE)
		     { 
		       robot_JLS->setpt[j]=chg->motion[j].val.i;
		       robot_JLS->cl_request=JLS_WRITE_ENC;
		       how->calibMode &= ~(1<<j);
		     }
		    else if (com[j] == CHG_STOP)
		     { how->loadSettings |= (1<<j);
		     /* Not sure what this is for 
			PUSH_COM0 (STOP_JNT_C | j);
		     */
		     }
		    else if (newEnc[j] != how->enc[j] || modeChange&(1<<j))
		     { robot_JLS->setpt[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:
		 if(chg->hand.val.i) 
		    robot_JLS->discrete = 
		      (robot_JLS->discrete & (~HAND_CLOSED_BIT)) | HAND_OPEN_BIT ;
		 else                             
		 robot_JLS->discrete = 
		      (robot_JLS->discrete & (~HAND_OPEN_BIT)) | HAND_CLOSED_BIT;
		 robot_JLS->cl_request=JLS_WRITE_DES;		 
		    break;

		    
		 case CHG_HANDFORCE:
		   { /* No hand force capability on trident */
		    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)
	     { /* No direct torque reading on trident */
	      chg->torque.com = 0;
	    }

	   if (chg->pot.com)
	     { 
	       if(chg->pot.val.i) robot_JLS->cl_request=JLS_READ_POT ;
	       else robot_JLS->cl_request=JLS_DONT_READ_POT ;
	       chg->pot.com = 0;
	     }

	   if (chg->pendant.com)
	     { /* No pendant on trident */
	      chg->pendant.com = 0;
	    }

	   if (chg->force.com)
	     { /* No force on trident */
	      chg->force.com = 0;
	    }

	   if (chg->teleop.com)
	     { /* No teleop on trident */
	      chg->teleop.com = 0;
	    }

	   if (chg->jstick.com)
	     { /* No joystick on trident */
	      chg->jstick.com = 0;
	    }

	   if (chg->adc.com)
	     { /* No ADC on trident */
	      chg->adc.com = 0;
	    }

	   if (chg->slu.com)
	     { /* No SLU on trident */
	      chg->slu.com = 0;
	    }

	   if (chg->setTest.com)
	     { /* No test read on trident */
	      chg->setTest.com = 0;
	    }

	   chg->readcom = 0;
	 }

	if (chg->writecom)
  	 { int n;
	 
	 for (n=0; n<how->numPioOut; n++)
	   { int cmd; /* There are three spare PIO channels on the trident */
	   if (cmd = chg->pioOutput[n].com)
	     { switch (cmd)
	       { 
	       case CHG_PUT: /* Don't know diff. between put and set */
	       case CHG_SET:
		 robot_JLS->discrete = 
		   robot_JLS->discrete | SPARE_PIO_START<<n;
		 break;
		 
	       case CHG_CLEAR:
		 
		 robot_JLS->discrete = 
		   robot_JLS->discrete & (~(SPARE_PIO_START<<n));
		 break;
		 
	       }
	     chg->pioOutput[n].com = 0;
	     }
	   }
	   robot_JLS->cl_request=JLS_WRITE_DES;

	   for (n=0; n<how->numDac; n++)
	    { if (chg->dacOutput[n].com)
	       { int value = chg->dacOutput[n].val.i;
	       /* Trident has no DACs other than joint ones */
		 chg->dacOutput[n].com = 0;
		 how->dacOutput[n] = value;
	       }
	    }

	   for (n=0; n<how->numSlu; n++)
	     { if (chg->sluOutput[n].com)
	       /* No SLU */
	       chg->sluOutput[n].com = 0;
	     }
	   
   
	   if (chg->pendantMsg.com)
	     { /* No pendant */
	    }

	   if (chg->putTest.com)
	    { 
	      /* No Test */
	    }
	   
	   chg->writecom = 0;
	 }
	
	if (chg->putVectorSet)
	  { /* No isea what this is */
	  }
	
	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
 
puma_signal (dev)
  IO_DEVICE *dev;
 {
   /* No need to signal robot */
   return (1);
 }	
 
puma_startup (td, rbt)
RCI_DESC *td;
RCI_RBT *rbt;
{
	int n;
	RCI_TCB *tcb = td->tcb;
 	register HOW *how = rbt->how;
	IO_DEVICE *dev = (IO_DEVICE*)rbt->dev;
	JLS *jls = rbt->jls;
	CHG *chg = rbt->chg;
	volatile char * shm1;

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

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


	/*  Probe the Trident card *outside* of kernel-level task to
	    avoid hard crash */
	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;
	 }

	how->options = POTS_AVAIL | PENDANT_AVAIL;

	/* Access the shared memory structure for the JLS process */
	shm1 = (volatile char*) mbuff_alloc("control",sizeof(robot_JLS_type));
	robot_JLS = (robot_JLS_type *)shm1;


	if (rciStartJLS() < 0)
	  { rciAbort (ENoDev, "Failed to start JLS\n");
	  return (-1);
	  }
	
	rbt->notice = ACTIVATED;


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

/* *** */

	rbt->ready = puma_readytest;
	rbt->readyArg = 0;

	rbt->wakeup = puma_readytest;;
	rbt->wakeupArg = 0;

	return (1);
}

puma_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.
	*/

  
  robot_JLS->cl_request = JLS_STOP_REQ;
  
  if (rciStopJLS() < 0)
    { rciAbort (ENoDev, "Failed to stop JLS\n");
    return (-1);
    }
	
  
}

int
puma_readytest()
{

}
