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

#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_PROG_RUNNING;/* set to M_PROG_RUNNING to have message */
				/*    when program running, or 0 otherwise */

int rbtComDelay = 0;		/* delay used when doing robot I/O */
int rbtTimeout = 4000;		/* 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 = 0;		/* 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 = 1;		/* defines if torque readings are available */
int numADC = 10;		/* 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 ----- */

short *DACAddr[] = 		{ (short *)0170400,
				  (short *)0170402,
				};

int numDAC = ARRAY_SIZE(DACAddr);

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

static DEV_DESC serialDevs[]  =	{ (unsigned  *)0176500,  0300,
				  (unsigned  *)0176510,  0310,
				  (unsigned  *)0176520,  0320,
#if 0
				  (unsigned  *)0177560,  060,
				  (unsigned  *)0176600,  0340,
				  (unsigned  *)0176610,  0350,
				  (unsigned  *)0176620,  0360,
				  (unsigned  *)0176630,  0370,
#endif
				};

static 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, 0370);	/* address of VAX interface */ 
	configSlu();			/* set up serial line units */
}

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

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

static int forceOn = 0;

initForce()
{
}

startForce()
{
#if VERBOSE
     	printf ("force on\n");
#endif
	if (forceAvail)
	 { forceOn = 1;
	 }
}

stopForce()
{
#if VERBOSE
     	printf ("force off\n");
#endif
	forceOn = 0;
}

getForce(buf)
unsigned short *buf;
{
	int j;
	if (forceOn)
	 { for (j=0; j<6; j++)
	    { buf[j] = (j+1)*10;
	    }
	   return (1);
	 }
	else
	 {
	   return (0);
	 }
}

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

static int jstickOn = 0;

initJstick()
{
}

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

stopJstick ()
{
#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);
	 }
}
