/*-=-=-=-=-=-=-=-=-=-=-=- 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 "sys.h"
#include "pumaInterface.h"
#include "../h/moper_com.h"
#include "armtypes.h"

#define PUMAJ	6

static int hlog2();
static int nlog2();

/* -------------------  Teach Pendant Interface Package ------------------- 

	This package interfaces moper to the PUMA teach pendant. Communication
	is done using a simple serial line protocol -- moper 'kicks' the pendant
	by shipping 2 bytes up to it, and the pendant then ships 9 bytes back
	and then waits for the next 2 bytes. The 2 bytes sent up contain a code
	telling the pendant what LEDs to light up and what message to display
	(pendant messages are hard coded into ROM on the pendant and hence
	 must be addressed by number). The nine bytes sent back down contain
	a bit map of what buttons are currently being pushed on the pendant.
	The last byte is a check sum (which this code does NOT verify), and
	the last two bytes before that are not used, so we are only interested
	in the forst 6 bytes.

 -------------------------------------------------------------------------- */

#define PRMSG	9	/* number of bytes pendant is sending back to us */
#define PRBUF	5	/* size of buffer (words) reading back from pendant */
#define PVBUF	3	/* size of intermediate buffer storing pendant data */
#define PRETRY  50	/* number of cycles before kicking the pendant again */

int pdntDev = 0;	/* pendant serial line unit number */

static SLU *pdntSlu = (SLU*)0;	/* pendant serial line unit descriptor */
static short pdntMsg = 0;	/* 2-byte message sent down to pendant */
static short pdntWbuf;		/* buffer for writing to pendant */
static short pdntRbuf[PRBUF];	/* buffer for reading from pendant */
static short pdntVbuf[PVBUF];	/* intermediate buffer for pendant data */
static int   pdntOK = 0;	/* pendant info has been read and is valid */
static int   pdntActive = 0;	/* pendant communication is currently active */
static int   pdntTimeout = 0;	/* No. of cycles since last info read in */
int   pdntEstop = 0;		/* Pendant E-Stop has been hit */

servicePendant()
{
	/* Called once every control cycle. If the teach pendant is active, checks
	   to see if all data has been read back from the pendant. If so,
	   then this data is copied into the intermediate buffer, and another
	   write 2-bytes, read 9-bytes cycle is set up */
	   
	if (pdntActive)
	 { pdntTimeout++;
	   if (sluRcount(pdntSlu) == PRMSG)
	    { int i;
	      pdntOK = 1;
	      for (i=0; i<PVBUF; i++)
	       { pdntVbuf[i] = pdntRbuf[i];
	       }
	      /* Check for pattern indicating E stop button hit */

	      if (pdntVbuf[0] == 0x2 && pdntVbuf[2] == 0xc0)
	       { pdntEstop = 1;
	       }
	      else
	       { pdntEstop = 0;
	       }
	    }
	   else
	    { pdntOK = 0;
	    }

	   if (pdntOK || pdntTimeout > PRETRY)
	    { /* kick the pendant again */

	      pdntTimeout = 0;
	      pdntWbuf = pdntMsg;
	      sluRead (pdntSlu, pdntRbuf, sizeof(short)*PRBUF);
	      sluWrite (pdntSlu, &pdntWbuf, sizeof(short));
	    }
	 }
}

getPendant(buf)
short *buf;
{
	/* Called once every control cycle to retrieve the information (if
	   any) that has been read back from the pendant. If there is information,
	   it is copied into the address starting at 'buf', and the routine
	   returns 1. Otherwise, the routine returns 0. */

	if (pdntActive && pdntOK)
	 { int i;
	   for (i=0; i<PVBUF; i++)
	    { buf[i] = pdntVbuf[i];
	    }
	   return (1);
	 }
	else
	 { return (0);
	 }
}

startPendant()
{
	/* Called whenever teach penadnt communication is turned on. If the
	   the system does in fact have pendant support available, then this
	   routine sets up the serial port that will do the communication */
	   
	extern int pendantAvail;

	if (pendantAvail)
	 { sluOn (pdntSlu);
	   pdntOK = 0;
	   pdntEstop = 0;

	   /* kick start the pendant */

	   pdntTimeout = 0;
	   sluRead (pdntSlu, pdntRbuf, sizeof(short)*PRBUF);
	   sluWrite (pdntSlu, &pdntWbuf, sizeof(short));
	   pdntActive = 1;
	 }
}

initPendant()
{
	if (pendantAvail)
	 { pdntSlu = sluGet(pdntDev);
	 }
}

stopPendant()
{
	/* Called whenever teach pendant communication is turned off. Disables
	   the serial line unit */

	pdntActive = 0;
	sluOff (pdntSlu);
}

setPendant(w)
short w;
{
	/* Changes the two byte (1 word) message sent down to the teach pendant */
	pdntMsg = w;
}

extern int robotType;

/*  --------------- robot joint interface package  ---------------------- */

static unsigned short *rbtcsr = (unsigned short *)0176770;
static unsigned short *rbtobuf = (unsigned short *)0176772;
static unsigned short *rbtibuf = (unsigned short *)0176774;

#define RBT_CSR	    	(*rbtcsr)
#define RBT_OBUF    	(*rbtobuf)
#define RBT_IBUF	(*rbtibuf)

#define RBT_SELECT	0x0000	/* sets up data transfer */
#define RBT_CMDVLD	0x0001	/* Command validation */
#define RBT_READY	0x0080	/* request a, acknowledge transfer */
#define RBT_REQB	0x8000	/* request b, signal attention request */
#define RBT_CNTRLR	     7  

extern int rbtTimeout;
extern int rbtComDelay;

#define DELAY	{ int i; for (i=rbtComDelay; i>0; i--); }

getj (com, val, j)
unsigned short com, *val;
int j;
{
	int k = rbtTimeout;

	RBT_CSR = 0;				DELAY;
	RBT_OBUF = com | j;			DELAY;
	RBT_CSR = 1;
	while (!(RBT_CSR & RBT_READY) && --k)
	   ;
	if (k == 0)
	 { return (-1);
	 }
	*val = RBT_IBUF;	
	return (1);
}

putj (com, val, j)
unsigned short com, val;
int j;
{
	int k = rbtTimeout;

	RBT_CSR = 0;				DELAY;
	RBT_OBUF = com | j;			DELAY;
	RBT_CSR = 1;				DELAY;
	RBT_OBUF = val;
	while (!(RBT_CSR & RBT_READY) && --k)
	   ;
	if (k == 0)
	 { return (-1);
	 }
	return (1);
}

putmode (mode, j)
int mode, j;
{
	int k = rbtTimeout;

	if (robotType == PUMA560 && (j == 5 || j == 3))
	 { mode |= MEHLFCT;	/* enable half count on jnt 6 for PUMA 560 */
	 }

	RBT_CSR = 0;				DELAY;
	RBT_OBUF = SETPAR | j;			DELAY;
	RBT_CSR = 1;				DELAY;
	RBT_OBUF = MODE | mode;
	while (!(RBT_CSR & RBT_READY) && --k)
	   ;
	if (k == 0)
	 { return (-1);
	 }
	return (1);
}

static short setbits = 0;

SET_BITS(v)
short v;
{
	setbits |= v;
	SET_ACR(setbits);
}

CLEAR_BITS(v)
short v;
{
	setbits &= ~v;
	SET_ACR(setbits);
}

short GET_ACR()
{
	RBT_CSR = 0;				DELAY;
	RBT_OBUF = RBT_CNTRLR;			DELAY;
	RBT_CSR = 1;				DELAY;
	return (RBT_IBUF);
}

SET_ACR(v)
unsigned short v;
{
	RBT_CSR = 0;				DELAY;
	RBT_OBUF = RBT_CNTRLR;			DELAY;
	RBT_CSR = 1;				DELAY;
	RBT_OBUF = v;
}

extern unsigned short *pioOutAddr[];
extern int powerChan;
extern int powerBit;
extern int powerNegative;
extern int powerPulse;

powerOn()
{
	SET_BITS (ARMPWR);
	if (powerChan != -1)
	 { 
	   if (powerNegative)
	    { *pioOutAddr[powerChan] |= powerBit;
	      *pioOutAddr[powerChan] &= (~powerBit);
	    }
	   else
	    { *pioOutAddr[powerChan] &= (~powerBit);
	      *pioOutAddr[powerChan] |= powerBit;
	    }
	   if (powerPulse)
	    { int i;
	      for (i=0; i<powerPulse; i++)
		 ;
	      if (powerNegative)
	       { *pioOutAddr[powerChan] |= powerBit;
	       }
	      else
	       { *pioOutAddr[powerChan] &= (~powerBit);
	       }
	    }
	 }
}

powerOff()
{
	CLEAR_BITS (ARMPWR);
	if (powerChan != -1)
	 { 
	   if (powerNegative)
	    { *pioOutAddr[powerChan] |= powerBit;
	    }
	   else
	    { *pioOutAddr[powerChan] &= (~powerBit);
	    }
	 }
}

armPowerOn()
{
	return (GET_ACR() & ARMPWR);
}

static int handPos = 0;
static int handForce = 0;

setHndPos (value)
short value;
{
	if (value)
	 { handPos = 1;
	   SET_BITS (HNDOH);
	   CLEAR_BITS (HNDCH);
	 }
	else
	 { handPos = 0;
	   SET_BITS (HNDCH);
	   CLEAR_BITS (HNDOH);
	 }
}

getHndPos()
{
	return (handPos);
}

setHndForce(value)
short value;
{
	handForce = 0;
}

getHndForce()
{
	return (handForce);
}

extern int varClkRate;
extern int baseClkRate;
extern int varClkParam;
extern int jntClkRate;

clockSetup ()
{
	if (varClkRate)
	 { RBT_CSR = 0;	 			DELAY;
	   RBT_OBUF = 7;			DELAY;
	   RBT_CSR = 3;				DELAY;
	   RBT_OBUF = varClkParam;	/* normally 6 */
	   DELAY;
	 }
	return (baseClkRate);
}

getJointRate ()
{
	return (jntClkRate);
}

static int hlog2 (k)
int k;
{
	/* This routines returns log2(k), truncated to the
	   highest integer. In other words, 

			hlog2(7) = 3
			hlog2(8) = 3
			hlog2(9) = 4

	   The minimum value returned is 1.
	*/
	int l, i;

	l = 0;
	i = k;
	for ( ; i & ~1; i >>= 1)
	 { l++;
	 }
	if ((1<<l) < k)
	 { l++;
	 }
	if (l < 1)
	 { l = 1;
	 }
	return (l);
}

static int nlog2 (k)
int k;
{
	/* This routines returns log2(k), truncated to the
	   nearest integer. In other words, 

			nlog2(4) = 2
			nlog2(5) = 2
			nlog2(6) = 3
			nlog2(7) = 3
			nlog2(8) = 3

	   The minimum value returned is 1.
	*/
	int l;

	l = 1;
	for ( ; k & ~1; k >>= 1)
	 { l++;
	 }
	if (k == 3)
	 { l++;
	 }
	return (l);
}

int logNumCycles;
int numRbtCycles;

extern int intStretch;
extern int ninterAddr;
extern int intsclAddr;
extern int intsclBias;

setRobotRate (interval)
int interval;
{
	/* tells the joint micro processors what sample interval
	   to expect position commands to arrive at. This information
	   is passed to the joint micros in terms of the number
	   of joint micro sample ticks, and the log2 of this
	   same number. The log2 information seems to be what is
	   actually used on the joints; the regular count doesn't seem to
	   be, but we send both just in case.

	   Because the joint micros do all of their multiplication
	   and division using bit shift operations, the number passed
	   must also be multiple of a power of 2 of the base sample rate
	   (hence the reason for passing log2 of the number in the
	    first place).

	   The actual interval may, of course, not be a exactly of
	   multiple of a power of 2 of the base rate. In that case,
	   we have two choices: (1) pick the closest number that is,
	   and send it, or (2) pick the highest number that is.	

	   We take the second choice, since the robot tends to move
	   in a jerky fashion otherwise. If we want the first choice,
	   then the log should be computed using the function nlog2()
	   instead of hlog2().
	*/
	int j;

	interval *= 10 * intStretch;
	
	logNumCycles = hlog2 (interval/jntClkRate);
	numRbtCycles = (1<<logNumCycles);
	if (logNumCycles >= 8)
	 { logNumCycles = 8;
	   numRbtCycles = 255;
	 }

	for (j=0; j<PUMAJ; j++)
	 { if (putj (SETPAR, ninterAddr | (0xff & numRbtCycles), j) < 0)
	    { harderror (SRV_TIMEOUT_E, 24);
	    }
	   if (intsclBias)
	    { if (putj (SETPAR, intsclAddr | (8-(0xff&logNumCycles)), j) < 0)
	       { harderror (SRV_TIMEOUT_E, 25);
	       }
	    }
	   else
	    { if (putj (SETPAR, intsclAddr | (0xff&logNumCycles), j) < 0)
	       { harderror (SRV_TIMEOUT_E, 25);
	       }
	    }
	 }
}

joint_ram_load (loadTable)
unsigned short loadTable [][7];
{
	int i, j, numEntries;
	unsigned short setvalues[6];
	
	/* load each entry into memory */

	i = 0;
	while (loadTable[i][0] != -1)
	 { for (j=0; j<6; j++)
	    { if (putj (SETPAR, (loadTable[i][0] | loadTable[i][j+1]), j) < 0)
	       { printf ("putj SETPAR failed, (i,j) = (%d,%d)\n", i,j);
		 return (-1);
	       }
	    }
	   wait();
	   i++;
	 }
	numEntries = i;

	/* Now read the results back */

	for (i=0; i<numEntries; i++)
	 { 
	   for (j=0; j<6; j++)
	    { if (putj (SETPAR, ADDR | (loadTable[i][0] >> 8), j) < 0)
	       { printf ("putj SETPAR failed, (i,j) = (%d,%d)\n", i,j);
		 return (-1);
	       }
	    }
	   wait();
	   wait();

	   for (j=0; j<6; j++)
	    { if (getj (READM, &setvalues[j], j) < 0)
	       { printf ("putj READM failed, (i,j) = (%d,%d)\n", i,j);
		 return (-1);
	       }
	    }
	   for (j=0; j<6; j++)
	    { if ((0xff & setvalues[j]) != loadTable[i][j+1])
	       { printf ("ram load error, entry %d, joint %d: %02x vs %02x\n",
			 i, j+1, 0xff & setvalues[j], loadTable[i][j+1]);
		 return (-1);
	       }
	    }
	   wait();
	 }
	return (1);
}
