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

/* Standard bare-bones config file for a PUMA 560, Mark II controller,
   with LSI-11 73 processor */

#include "cdefs.h"
#include "../h/moper_com.h"
#include "sys.h"
#include "sensorIO.h"
#include "armtypes.h"

#include "lsi11_73.h"

#define VERBOSE		0
#define PUMAJ 		6
#define M_NULL		0x8000
#define M_PROG_RUNNING  0x8d00

/* -=-=-=-=-=- SYSTEM INITIALIZATION AND ROBOT PARAMETERS -=-=-=-=-=-=-=-=- */

int robotID = 0;		/* robot id */
int robotType = PUMA560;	/* describe the type of robot */
int moperID = 100;		/* software id */

int potsAvail = 1;		/* calibration potentiometers available */
int pendantAvail = 1;		/* teach pendant available */
int pendantMsgRun = M_NULL;	/* set to M_PROG_RUNNING to have message */
				/*    when program running, M_NULL otherwise */

int rbtComDelay = 0;		/* delay used when doing robot I/O */
int rbtTimeout = 400;		/* timeout count for doing robot I/O */

int powerChan = 1;		/* parallel port used to turn power on ... */
int powerBit = 0x08;		/* the bit which is set ... */
int powerNegative = 1;		/* and its sign (negative or positive) */
int powerPulse = 1000;		/* if set to non-zero, then the 'powerOn' bit
				   will be asserted only momentarily, using
				   'powerPulse' as a spin count. */
int traceBit = 0x02;		/* bit set when rbt cntrl routine running */

int intStretch = 1;		/* stretch the interpolation interval */

/* ----- robot controller definitions ----- */

#include "mk1_old.560.c"

/* addresses of sample rate registers in joint micros */
int ninterAddr = NINTER;
int intsclAddr = INTSCL;

/* =-=-=-=-=-=-=-=-=- BASIC SENSOR I/O PARAMETERS -=-=-=-=-=-=-=-=-=-=-=-=- */

int jstickAvail = 0;		/* defines if joystick is available */
int forceAvail = 0;		/* defines if force is available */
int torqueAvail = 0;		/* defines if torque readings are available */
int numADC = 0;			/* number of ADC channels */

/* ----- Number of parallel ports and their addresses ----- */

unsigned short *pioOutAddr[2] = { (unsigned short *)0176740,
				  (unsigned short *)0176744,
				};

int numPioOut = ARRAY_SIZE(pioOutAddr);

unsigned short *pioInAddr[2] =  { (unsigned short *)0176742,
				  (unsigned short *)0176746,
				};

int numPioIn = ARRAY_SIZE(pioInAddr);

/* ----- Number of DAC devices ----- */

#if 0
short *DACAddr[] = 		{ (short *)0170404,
				  (short *)0170406,
				};
int numDAC = ARRAY_SIZE(DACAddr);
#else
short *DACAddr[] = NULL;
int numDAC = 0;
#endif

/* ----- Serial line unit information (DLV11 type devices) ----- */

DEV_DESC serialDevs[]  =	{ (unsigned  *)0176500,  0300,
				  (unsigned  *)0176510,  0310,
				  (unsigned  *)0176520,  0320,
/* Include if you have a spare serial board */
#if 0
				  (unsigned  *)0176600,  0340,
				  (unsigned  *)0176610,  0350,
				  (unsigned  *)0176620,  0360,
				  (unsigned  *)0176630,  0370,
#endif
				};

int numSerialDevs = ARRAY_SIZE(serialDevs);

/* RCI serial line unit info:	  Channel   read buf size   write buf size */

SLU_DESC sluDesc[] = 		{    1,		 100,		  100,
				     2,		 100,		  100,
				};

int numSlu = ARRAY_SIZE(sluDesc);

/* -=-=-=-=-=-=-=-=-=-=- PARAMETER INITIALIZATION ROUTINE -=-=-=-=-=-=-=-=- */

paramInit()
{
	/* ---- set up basic parameters and addresses ---- */

	if (numSerialDevs)
	 { sluInit (numSerialDevs, serialDevs);
	 }
	hostDevice (0175770, 0200);	/* address of VAX interface */ 
	configSlu();			/* set up serial line units */
}

/* -=-=-=-=-=-=-=-=-=-=-=- SENSOR I/O ROUTINES -=-=-=-=-=-=-=-=-=-=-=-=-=-= */

/* ----- Force Sensor Routines ------ */

#define FORCE_OFF	0
#define FORCE_INIT	1
#define FORCE_ON	2

static char forceInitCmd[] = {  3, 13, 83, 69, 13, 83, 79, 13, 70, 84, 13,
			       79, 66, 13 };
static char forceSendCmd[] = { 024 };

#define F_REPLY_SIZE	27

static char forceRbuf[64];
static SLU *forceSlu;
static int forceMode = FORCE_OFF;

initForce()
{
	forceSlu = (SLU*)sluGet (3);
}

startForce()
{
#if VERBOSE
     	printf ("force on\n");
#endif
	if (forceAvail)
	 { sluOn (forceSlu);
	   sluRead (forceSlu, forceRbuf, sizeof(forceRbuf));
	   sluWrite (forceSlu, forceInitCmd, sizeof(forceInitCmd));
	   forceMode = FORCE_INIT;
	 }
}

stopForce()
{
	if (forceAvail)
	 { 
#if VERBOSE
	   printf ("force off\n");
#endif
	   sluOff (forceSlu);
	   forceMode = FORCE_OFF;
	 }
}

getForce(buf)
unsigned short *buf;
{
	int j;
	int code = 0;

	if (forceMode == FORCE_INIT)
	 { if (sluRcount (forceSlu) == F_REPLY_SIZE)
	    { sluRead (forceSlu, forceRbuf, sizeof(short)*6);
	      sluWrite (forceSlu, forceSendCmd, sizeof(forceSendCmd));
	      forceMode = FORCE_ON;
	    }
	 }
	else if (forceMode == FORCE_ON)
	 { if (sluRcount (forceSlu) == 6*sizeof(int))
	    { int k = 1;
	      code = 1;
	      for (j=0; j<6; j++)
	       { union {
		       char c[2];
		       int i;
		  } pack;
		 pack.c[1] = forceRbuf[k++];
		 pack.c[0] = forceRbuf[k++];
		 buf[j] = pack.i;
	       }
	    }
	   sluRead (forceSlu, forceRbuf, sizeof(short)*6);
	   sluWrite (forceSlu, forceSendCmd, sizeof(forceSendCmd));
	 }
	return (code);
}

/* ----- Joystick I/O routines ----- */

#define S6RCSR	*(unsigned  *)0176620
#define S6RBUF	*(unsigned char *)0176622
#define S6XCSR	*(unsigned  *)0176624
#define S6XBUF	*(unsigned char *)0176626

#define S6RVEC	0360
#define S6XVEC  0364
#define S_IEA	0100

#define JTHRESH		7

static unsigned char jstkXbuf[6] = { 1, 0, 3, 4, 5, 2 };
static unsigned char jstkRbuf[64];
static int jstkCount = 0;
static int jstkOn = 0;
static int ignoreByte = 1;
VECTOR *jstkVector = NULL;

jstkInterrupt()
{
	int flush;

/*	pushr();	*/

	if (ignoreByte)
	 { ignoreByte = 0;
	   flush = S6RBUF;
	 }
	else
	 { jstkRbuf[jstkCount] = S6RBUF;
	   if (++jstkCount < 6)
	    { S6XBUF = jstkXbuf[jstkCount];
	    }
	   if (jstkCount == 6)
	    { S6RCSR = 0;
	    }
	   ignoreByte = 1;
	 }

/*	popr();	*/
}

initJstick()	/* called once when moper initializes */
{
	VECT_DESC vd;
	   
	if (jstickAvail)
	 { vd.saveregs = 1;
	   vd.arg = 0;
	   vd.fxn = jstkInterrupt;
	   jstkVector = (VECTOR*)vectorAlloc (S6RVEC, &vd);
	 }
}

startJstick ()
{
	/* called when 'moper()' starts up the joystick */

	if (jstickAvail)
	 { 
	   if (S6RVEC == S6RVEC+4)
	   S6XCSR = 0;
	   jstkOn = 1;
	   jstkStartRead();
	 }
}

jstkStartRead()
{
	/* called by 'moper' each control cycle to start reading
	   6 bytes from the joystick. The rest of the read protocal
	   is handled by the interupt routine jstkInterrupt() */

	int k;

	S6RCSR = 0;		/* block interrupts */

	jstkCount = 0;
	S6XBUF = jstkXbuf[jstkCount];
	k = S6RBUF;
	ignoreByte = 1;

	S6RCSR = S_IEA;		/* enable interrupts */
}

stopJstick ()
{
	/* Called by moper when we shut down the joystick */

	int k;

	if (jstickAvail)
	 { S6RCSR = 0;
	   S6XCSR = 0;
	   k = S6RBUF;
	   jstkCount = 0;
	   jstkOn = 0;
	 }
}

getJstick (buf)
unsigned short *buf;
{
	/* Called by moper once per control cycle to collect the joystick
	   values that were read in over the previous control cycle */

	int code = 0;
	int j;

	if (jstkOn)
	 { if (jstkCount == 6)
	    { for (j=0; j<6; j++)
	       { int value;
		 value = jstkRbuf[j] - 128;
		 if (ABS(value) < JTHRESH)
		  { buf[j] = 0;
		  }
		 else
		  { buf[j] = value;
		  }
	       }
	      code = 1;
	    }
	   jstkStartRead();
	 }
	return (code);
}

/* ----- analog to digital converter I/O ----- */

#define	ADC_MADR	(*(unsigned short *)0170400)
#define ADC_DATA	(*(unsigned short *)0170402)
#define ADC_EOC		0x8000

getADC (j, addr)
register int j; 
int *addr;
{
	if (0 <= j && j < numADC)
	 { ADC_MADR = j+6;
	   ADC_DATA = -1;
	   while (!(ADC_MADR & ADC_EOC))
	      ;
	   *addr = ADC_DATA;
	 }
}

/* ----- Torque sensor I/O package ----- */

static int torqueOn = 0;

startTorque ()
{
	if (torqueAvail)
	 { torqueOn = 1;
	 }
#if VERBOSE
	printf ("%% torque on\n");
#endif
}

stopTorque ()
{
	torqueOn = 0;
#if VERBOSE
	printf ("%% torque off\n");
#endif
}

getTorque(buf)
short *buf;
{
	int j;
	for (j=0; j<PUMAJ; j++)
  	 { ADC_MADR = j;
	   ADC_DATA = -1;
	   while (!(ADC_MADR & ADC_EOC))
	      ;
	   *buf++ = ADC_DATA;
	 }
}
