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

#ifndef _ROBOTCONTROL
#define _ROBOTCONTROL

/* ---- Definition of the how data structure ---- */

#include "../h/maxjnts.h"

#define HOW_MAXDAC	 2	/* max. no. of DAC channels */
#define HOW_MAXADC	16	/* max. no. of ADC channels */
#define HOW_MAXPIO 	 2	/* max. no. of parallel ports */
#define HOW_PENDANT	 2	/* number of words describing teach pendant */

/* ---- variable size char buffer ---- */

#define HOW_MAXSLU	   2	/* max. no. of serial line units */
#define HOW_SLUBUF	  32	/* max. no. of bytes per serial transfer */
typedef struct {
	int num;
	char com;
	unsigned char buf[HOW_SLUBUF];
} SLU;

/* ---- parameter control data structure ---- */

#define HOW_MAXPARAM	16
typedef struct {
	int num;
	int val;
	unsigned long jnts;
} PARAMSPEC;

/* ---- Flags defining the control level ---- */

#define MOTOR_LEVEL	1
#define JOINT_LEVEL	2

/* ---- Flags describing which IO options are available ---- */

#define TORQUE_AVAIL	0x01	/* torque readings available */
#define POTS_AVAIL	0x02	/* potentiometer readings available */
#define PENDANT_AVAIL	0x04	/* teach pendant readings available */
#define FORCE_AVAIL	0x08	/* force readings available */
#define JSTICK_AVAIL	0x10	/* joystick interface available */
#define TELEOP_AVAIL	0x20	/* teleop readings available */

/* ---- Flags describing which checking options are available ---- */

#define CHK_MAXPOS	0x0001		/* maximum position exceeded */
#define CHK_MAXVEL	0x0002		/* maximum velocity exceeded */
#define CHK_MAXTOR	0x0004		/* maximum torque exceeded */
#define CHK_REQPOS	0x0008		/* requested position past limits */
#define CHK_REQVEL	0x0010		/* requested velocity past limits */
#define CHK_REQTOR	0x0020		/* requested torque past limits */

#define CHK_RBTALL	0x003f

#define ulong unsigned long
#define ushort unsigned short

typedef struct _HOW {

/* ---- robot ID, interface version, and options ---- */

    int	robotID;	/* ID of the robot */
    int	controllerID;	/* ID of the software interface (version) */
   long	options;	/* available interface options */
unsigned char numAdc;	/* number of ADC channels available */
unsigned char numDac;	/* number of DAC channels available */
unsigned char numPioIn;	/* number of parallel input words available*/
unsigned char numPioOut;/* number of parallel output words available*/
unsigned char numSlu;	/* number of serial line units available */

    int	time;		/* time that how was last updated (ms) */

/* ---- robot position and torque information ---- */
	
    int	controlLevel;		/* MOTOR or JOINT level ctrl */
  float ang[MAXJNTS];		/* joint angles */
    int	enc[MAXJNTS];		/* encoder counts */

  float	angVel[MAXJNTS];	/* joint velocities */
  float	encVel[MAXJNTS];	/* encoder velocities */

  float	angSet[MAXJNTS]; 	/* last COMMANDED angle values */
    int	encSet[MAXJNTS];	/* last COMMANDED encoder value */

  float lastAngSet[MAXJNTS];	/* last previous commanded angle to moper */
    int lastEncSet[MAXJNTS];	/* last previous commanded encoder to moper */

  ulong torqueMode;		/* torque controlled joints */

  float	jtorque[MAXJNTS];	/* joint torques */
  float	mtorque[MAXJNTS];	/* motor torques */
	
  float	mtorqueSet[MAXJNTS];    /* last COMMANDED joint torques */
  float	jtorqueSet[MAXJNTS];    /* last COMMANDED motor torques */

  float	mtorqueBias[MAXJNTS];    /* last joint torques bias settings */
  float	jtorqueBias[MAXJNTS];    /* last motor torques bias settings */

    int	hand;			/* hand position */
    int	handForce;		/* hand force */

/* ---- robot and system state information ---- */

    int	state;			/* misc. robot information */
    int	ctrlParams[HOW_MAXPARAM]
		  [MAXJNTS]; 	/* control parameters */

  ulong integration;	/* integration is enabled (per joint) */
  ulong calibMode; 	/* joint is in calibration mode */
  ulong markFound; 	/* joint found its calibration mark */

   char	power;		/* power is on */
   char calibrated;	/* robot is calibrated */

   char torqueOK;	/* joint torque readings present */
   char	potOK;		/* potentiometer readings check out */
   char	pendantOK;	/* teach pendant readings are OK */
   char forceOK;	/* force sensor readings OK */
   char teleopOK;       /* teleop data readings OK - TSL */
   char jstickOK;	/* joystick sensor redaings OK */
   long adc;		/* ADC channels which have valid readings */
   long	slu;		/* SLU channels which have valid readings */

/* ---- calibration and IO information ---- */

    int	potInput[MAXJNTS];	/* potentiometer readings */ 
   long	pendantInput[HOW_PENDANT]; /* teach pendant */
  float	forceInput[6];		/* Cartesian force sensor vals */
  float teleopInput[6];         /* teleop data - TSL */
 ushort teleopButton;           /* teleop pressed button number - TSL */
  short teleopTrigger;          /* teleop trigger reading - TSL */
    int	jstickInput[6]; 	/* six DOF joystick values */
    int	adcInput[HOW_MAXADC];	/* ADC readings */
    SLU	sluInput[HOW_MAXSLU];	/* SLU readings */

 ushort pioInput[HOW_MAXPIO];	/* input parallel ports */
 ushort	pioOutput[HOW_MAXPIO];	/* output parallel ports */
    int	dacOutput[HOW_MAXDAC];	/* DAC set values */

    SLU	pendantMsg;		/* last msg sent to pendant */ 

   long	checking;		/* enabled checking modes */
   char	powerOnRelease;		/* keep power on when released */

/* ---- system internal stuff ----- */

  ulong loadSettings;		/* reload encSet and angSet */
  ulong newEncValues;		/* new encoder values */
   long adcSet;			/* ADC channels which have been turned on */
   long	sluSet;			/* SLU channels which have been turned on */

} HOW;

/* ---- Definition of the chg data structure ---- */

typedef struct {
	char com;
	union {
	   short s[2];
	   long  l;
	   int   i;
	   float f; } val;
} CMDVAL;

#define MAXPUTVECTOR	12

typedef struct _CHG {

/* motion control primitives */

	CMDVAL controlLevel;
	CMDVAL motion [MAXJNTS];
	CMDVAL bias [MAXJNTS];
	CMDVAL stop;
	CMDVAL hand;
	int encSet;
	int angSet;

	/* control primitives */

	  char ctrlcom;
	  char paramset;
     PARAMSPEC pspec [HOW_MAXPARAM];
	   int numParamsPosted;
	CMDVAL powerOnRelease;
	CMDVAL integration;
	CMDVAL power;
	CMDVAL calibMode;
	CMDVAL calibflag;
	CMDVAL checking;

	/* IO */

	  char readcom;
	CMDVAL torque;
	CMDVAL pot;
	CMDVAL pendant;
	CMDVAL force;
	CMDVAL teleop;
	CMDVAL jstick;
	CMDVAL adc;
	CMDVAL slu;
	CMDVAL setTest;

	  char writecom;
	CMDVAL pioOutput [HOW_MAXPIO];
	CMDVAL dacOutput [HOW_MAXDAC];
	   SLU sluOutput [HOW_MAXSLU];
	   SLU pendantMsg;
	CMDVAL maintCom;
	CMDVAL putTest;

	   int putVectorSet;
	 float putVector[MAXPUTVECTOR];
} CHG;

#define CHG_ENCODER		1
#define CHG_MTORQUE		2
#define CHG_ENCODER_VALUE	3
#define CHG_ANGLE		4
#define CHG_JTORQUE		5
#define CHG_ANGLE_VALUE		6
#define CHG_STOP		7

#define CHG_HANDPOS		8
#define CHG_HANDFORCE		9

#define CHG_MTORQUE_BIAS	10
#define CHG_JTORQUE_BIAS	11

#define CHG_PUT			1
#define CHG_SET			2
#define CHG_CLEAR		3

#define MOTOR_LEVEL_CONTROL(r)	((r)->chg->controlLevel.val.i = MOTOR_LEVEL, \
				 (r)->chg->controlLevel.com = 1)

#define MOVE_ENCODER(r,ival,j) 	((r)->chg->motion[j].val.i = (ival), \
				 (r)->chg->motion[j].com = CHG_ENCODER, \
				 (r)->chg->encSet = 1)

#define SET_MTORQUE(r,fval,j) 	((r)->chg->motion[j].val.f = (fval), \
				 (r)->chg->motion[j].com = CHG_MTORQUE)

#define SET_MTORQUE_BIAS(r,fval,j) ((r)->chg->bias[j].val.f = (fval), \
				 (r)->chg->bias[j].com = CHG_MTORQUE_BIAS)

#define SET_ENCODER(r,ival,j) ((r)->chg->motion[j].val.i = (ival), \
				 (r)->chg->motion[j].com = CHG_ENCODER_VALUE)

#define JOINT_LEVEL_CONTROL(r)	((r)->chg->controlLevel.val.i = JOINT_LEVEL, \
				 (r)->chg->controlLevel.com = 1)

#define MOVE_ANGLE(r,fval,j) 	((r)->chg->motion[j].val.f = (fval), \
				 (r)->chg->motion[j].com = CHG_ANGLE, \
				 (r)->chg->angSet = 1)

#define SET_JTORQUE(r,fval,j) 	((r)->chg->motion[j].val.f = (fval), \
				 (r)->chg->motion[j].com = CHG_JTORQUE)

#define SET_JTORQUE_BIAS(r,fval,j) ((r)->chg->bias[j].val.f = (fval), \
				   (r)->chg->bias[j].com = CHG_JTORQUE_BIAS)

#define SET_ANGLE(r,ival,j) ((r)->chg->motion[j].val.i = (ival), \
				 (r)->chg->motion[j].com = CHG_ANGLE_VALUE)

#define SET_STOP(r,j)		((r)->chg->motion[j].com = CHG_STOP)

#define SET_STOP_ALL(r)		((r)->chg->stop.com = 1)

#define SET_HANDPOS(r,ival)	((r)->chg->hand.val.i = (ival), \
				 (r)->chg->hand.com = CHG_HANDPOS )

#define SET_HANDFORCE(r,ival)	((r)->chg->hand.val.i = (ival), \
				 (r)->chg->hand.com = CHG_HANDFORCE )

#define POST_CTRLPARAM(r,p,v,j) {int sp = (r)->chg->numParamsPosted++; \
				 if (sp < HOW_MAXPARAM) \
				  { (r)->chg->pspec[sp].num = (p); \
				    (r)->chg->pspec[sp].val = (v); \
				    (r)->chg->pspec[sp].jnts = (j); \
				  } \
				 (r)->chg->ctrlcom = 1; }
				
#define LOAD_CTRLPARAMS(r)	((r)->chg->paramset = 1, \
				 (r)->chg->ctrlcom = 1)

#define SET_INTEGRATION(r,lval)	((r)->chg->integration.val.l = (lval), \
				 (r)->chg->integration.com = 1, \
				 (r)->chg->ctrlcom = 1)

#define SET_CALIBMODE(r,lval)	((r)->chg->calibMode.val.l = (lval), \
				 (r)->chg->calibMode.com = 1, \
				 (r)->chg->ctrlcom = 1)

#define SET_CALIBFLAG(r,ival)	((r)->chg->calibflag.val.i = (ival), \
				 (r)->chg->calibflag.com = 1, \
				 (r)->chg->ctrlcom = 1)

#define SET_POWER_ON(r)		((r)->chg->power.val.i = 1, \
				 (r)->chg->power.com = 1, \
				 (r)->chg->ctrlcom = 1)

#define SET_POWER_OFF(r)	((r)->chg->power.val.i = 0, \
				 (r)->chg->power.com = 1, \
				 (r)->chg->ctrlcom = 1)

#define SET_CHECKING(r,lval)	((r)->chg->checking.val.l = (lval), \
				 (r)->chg->checking.com = 1, \
				 (r)->chg->ctrlcom = 1)

#define SET_POWER_ON_RELEASE(r,ival) ((r)->chg->powerOnRelease.val.i=(ival), \
				 (r)->chg->powerOnRelease.com = 1, \
				 (r)->chg->ctrlcom = 1)

#define SET_TORQUE_READ(r,ival)	((r)->chg->torque.val.i = (ival), \
				 (r)->chg->torque.com = 1, \
				 (r)->chg->readcom = 1)

#define SET_POT_READ(r,ival)	((r)->chg->pot.val.i = (ival), \
				 (r)->chg->pot.com = 1, \
				 (r)->chg->readcom = 1)

#define SET_PENDANT_READ(r,ival) ((r)->chg->pendant.val.i = (ival), \
				 (r)->chg->pendant.com = 1, \
				 (r)->chg->readcom = 1)

#define SET_FORCE_READ(r,ival)	((r)->chg->force.val.i = (ival), \
				 (r)->chg->force.com = 1, \
				 (r)->chg->readcom = 1)

#define SET_TELEOP_READ(r,ival)	((r)->chg->teleop.val.i = (ival), \
				 (r)->chg->teleop.com = 1, \
				 (r)->chg->readcom = 1)

#define SET_JSTICK_READ(r,ival)	((r)->chg->jstick.val.i = (ival), \
				 (r)->chg->jstick.com = 1, \
				 (r)->chg->readcom = 1)

#define SET_TEST_READ(r,ival)	((r)->chg->setTest.val.i = (ival), \
				 (r)->chg->setTest.com = 1, \
				 (r)->chg->readcom = 1)

#define SET_ADC_CHAN(r,lval)	((r)->chg->adc.val.l = (lval), \
				 (r)->chg->adc.com = 1, \
				 (r)->chg->readcom = 1)

#define SET_SLU_CHAN(r,lval)	((r)->chg->slu.val.l = (lval), \
				 (r)->chg->slu.com = 1, \
				 (r)->chg->readcom = 1)

#define PUT_PIO(r,num,sval)	 if ((num) < HOW_MAXPIO) \
				  { (r)->chg->pioOutput[num].val.s[0]=(sval);\
				    (r)->chg->pioOutput[num].com = CHG_PUT;\
				    (r)->chg->writecom = 1;\
				  }

#define SET_PIO(r,num,sval)	 if ((num) < HOW_MAXPIO) \
				  { (r)->chg->pioOutput[num].val.s[0]=(sval);\
				    (r)->chg->pioOutput[num].com = CHG_SET;\
				    (r)->chg->writecom = 1;\
				  }

#define CLEAR_PIO(r,num,sval)	 if ((num) < HOW_MAXPIO) \
				  { (r)->chg->pioOutput[num].val.s[0]=(sval);\
				    (r)->chg->pioOutput[num].com = CHG_CLEAR;\
				    (r)->chg->writecom = 1;\
				  }

#define PUT_DAC(r,num,ival)	 if ((num) < HOW_MAXDAC) \
				  { (r)->chg->dacOutput[num].val.i = (ival);\
				    (r)->chg->dacOutput[num].com = 1;\
				    (r)->chg->writecom = 1;\
				  }

#define PUT_TEST(r,ival)	  { (r)->chg->putTest.val.i = (ival);\
				    (r)->chg->putTest.com = 1;\
				    (r)->chg->writecom = 1;\
				  }

#define PUT_SLU(r,j,b,s)  	 if ((j) < HOW_MAXSLU) \
				  { int i; (r)->chg->sluOutput[j].num = (s); \
				    for (i=0; i<(s)&&i<HOW_SLUBUF; i++) \
				     { (r)->chg->sluOutput[j].buf[i] =(b)[i];\
				     } \
				    (r)->chg->sluOutput[j].com = 1; \
				    (r)->chg->writecom = 1; }

#define PUT_PENDANT_MSG(r,b,s)	{ int i; (r)->chg->pendantMsg.num = (s); \
				  for (i=0; i<(s)&&i<HOW_SLUBUF; i++) \
				   { (r)->chg->pendantMsg.buf[i] = (b)[i]; \
				   } \
				  (r)->chg->pendantMsg.com = 1; \
				  (r)->chg->writecom = 1; }

#define SET_MAINT_COM(r,ival)	((r)->chg->maintCom.val.i = (ival), \
				 (r)->chg->maintCom.com = 1, \
				 (r)->chg->ctrlcom = 1)

#define PUT_VECTOR(r,v,s)	{ int size = MAX((s), MAXPUTVECTOR); \
				  (r)->chg->putVectorSet = size; \
				  bcopy ((v), (r)->chg->putVector, \
					      size*sizeof(float));

#undef ushort
#undef ulong
#endif
