/*-=-=-=-=-=-=-=-=-=-=-=- 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 260, Mark I 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"		/* defines the system CPU */

#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 = PUMA260;	/* describe the type of robot */
int moperID = 100;		/* software id */

int potsAvail = 0;		/* 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 = 2;		/* 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 = 0x01;		/* 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_std.260.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[]  =	{ 
				 /* first serial port board */
				  (unsigned  *)0176500,  0300,
				  (unsigned  *)0176510,  0310,
				  (unsigned  *)0176520,  0320,
/* Include if you have a second serial board */
#if 0
				 /* second serial port board */
				  (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 (0xfe00, 0xa0);	/* address of VAX interface */ 
	configSlu();			/* set up serial line units */
}

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

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

/* These force sensor routines drive a Lord force torque sensor connected
   to a serial port on the Unimate controller. The serial port used is
   defined by FORCE_PORT. '2' is the default, and corresponds to the
   last port on the first serial board. If your controller has 2 serial
   boards, you might want to use another port (like '3', the first port
   on the second board).
*/

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

/* A string sent to the Lord sensor when force readings are first requested,
   to put the sensor into a mode where it will send back binary force
   data. If you change this string, you will probably need to change
   F_REPLY_SIZE, below. */
static char forceInitCmd[] = {  3, '\r', 'S', 'E', '\r', 
					 'S', 'O', '\r',
					 'F', 'T', '\r',
					 'B', 'S', '\r',
					 'O', 'B', '\r' };
/* The number of characters that the Lord system will send back in response
   to the initialization string "forceInitCmd". This includes (roughly)
   1 echo character for each character in the string, plus 3 characters for
   each carriage return. */
#define F_REPLY_SIZE	35
static char forceSendCmd[] = { 024 };	/* command to ask sensor for data */

static char forceRbuf[64];		/* buffer to read sensor data into */
static SLU *forceSlu;			/* sensor serial line structure */ 
static int forceMode = FORCE_OFF;	/* sensor software state */

initForce()
{
	/* called when moper first starts up (i.e., is loaded) */
	if (forceAvail)
	 { forceSlu = (SLU*)sluGet (2);
	 }
}

startForce()
{
	/* called whenever RCI "turns on the force sensor" with
	   SET_FORCE_READ (rbt, 1) */

	if (forceAvail)
	 { 
#if VERBOSE
	   printf ("force on\n");
#endif
	   sluOn (forceSlu);

	   /* Queue a write of the initialization sequence for the force
	      sensor. This contains commands (defined in the Lord manual)
	      to put the sensor into binary mode, disable echo, etc. */
	   sluRead (forceSlu, forceRbuf, sizeof(forceRbuf));
	   sluWrite (forceSlu, forceInitCmd, sizeof(forceInitCmd));
	   forceMode = FORCE_INIT;	/* "initialization" state */
	 }
}

stopForce()
{
	/* called whenever RCI "turns off the force sensor" with
	   SET_FORCE_READ (rbt, 0) */

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

getForce(buf)
unsigned short *buf;
{
	/* Called once every control cycle. What happens depends on the
	   software state of the sensor. */

	int j;
	int code = 0;			/* set true if force data available */

	if (forceMode == FORCE_INIT)	/* initializing */
	 { /* Check to see if initialization has finished */
	   if (sluRcount (forceSlu) == F_REPLY_SIZE)
	    { sluRead (forceSlu, forceRbuf, sizeof(short)*6+1);
	      sluWrite (forceSlu, forceSendCmd, sizeof(forceSendCmd));
	      forceMode = FORCE_ON;	/* "run" state */
	    }
#if VERBOSE
	   else
	    { printf ("%d\n", sluRcount(forceSlu));
	    }
#endif
	 }
	else if (forceMode == FORCE_ON)	/* sensor is running */
	 { 
	   /* check to see if all force data has been read in; if not
	      we'll try again next cycle. */
	   if (sluRcount (forceSlu) == 6*sizeof(int)+1)
	    { 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+1);
	   sluWrite (forceSlu, forceSendCmd, sizeof(forceSendCmd));
	 }
	return (code);
}

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

static int jstickOn = 0;

initJstick()
{
}

startJstick ()
{
#if VERBOSE
     	printf ("%% jstick on\n");
#endif
	if (jstickAvail)
	 { jstickOn = 1;
	 }
}

stopJstick ()
{
	if (jstickAvail)
	 { 
#if VERBOSE
	   printf ("%% jstick off\n");
#endif
	   jstickOn = 0;
	 }
}

getJstick (buf)
unsigned short *buf;
{
	int j;
	if (jstickOn)
	 { for (j=0; j<6; j++)
	    { buf[j] = (j+1)*100;
	    }
	   return (1);
	 }
 	else
  	 { return (0);
	 }
}

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

/* These routines read an ADC value from the 
   indicated channel of an Analog devices RTI 1251 
   analog to digital converter board. */

#define	ADC_MADR	(*(unsigned short *)0170404)
#define ADC_DATA	(*(unsigned short *)0170406)
#define ADC_EOC		0x8000

getADC (j, addr)
register int j; 
int *addr;
{
	short trashCan;

	if (0 <= j && j < numADC)
	 { ADC_MADR = j+6;	/* set ADC channel no. */
	   trashCan = ADC_MADR;	/* clear EOC bit */
	   ADC_DATA = -1;	/* start conversion */
	   while (!(ADC_MADR & ADC_EOC))
	      ;
	   *addr = ADC_DATA;
	 }
}

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

/* These torque routines read an ADC value indicating joint 
   current from channels 0-5 of an Analog devices RTI 1251 
   analog to digital converter board. */

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;
{
	short trashCan;
	int j;

	if (torqueOn)
	 { for (j=0; j<PUMAJ; j++)
	    { ADC_MADR = j;
	      trashCan = ADC_MADR;
	      ADC_DATA = -1;
	      while (!(ADC_MADR & ADC_EOC))
		 ;
	      *buf++ = ADC_DATA;
	    }
	   return (1);
	 }
	else
	 { return (0);
	 }
}
