/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1991 by John E. Lloyd.				    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/* Program to dump out the contents of the joint servo RAM on a 
   PUMA controller. Load this into the controller, enter a character,
   and it will read and print the contents of the RAM for each of
   the 6 joint servo controllers.
 */

#include <stdio.h>

#include "sys.h"
#include "pumaInterface.h"

int sysCpu = LSI11_73;

main()
{
	printf ("Hit any character to proceed\n");
	getchar();
	dump_joint_ram ();
	halt();
}

#define RBT_CSR	    	(*(unsigned short *) 0176770)
#define RBT_OBUF    	(*(unsigned short *) 0176772)
#define RBT_IBUF	(*(unsigned short *) 0176774)

#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  

int rbtTimeout = 400;
int rbtComDelay = 5;

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

dump_joint_ram ()
{
	int i, j;
	unsigned short values[6];
	
	i = 0;
	while (i<128)						
	 { 
	   for (j=0; j<6; j++)
	    { if (putj (SETPAR, ADDR | i, j) < 0)
	       { printf ("putj SETPAR failed, (i,j) = (%d,%d)\n", i,j);
		 return (-1);
	       }
	    }
	   printf ("%4x ", i);
	   wait();
	   wait();
	   for (j=0; j<6; j++)
	    { if (getj (READM, &values[j], j) < 0)
	       { printf ("putj READM failed, (i,j) = (%d,%d)\n", i,j);
		 return (-1);
	       }
	      printf ("%2x ", 0xff & values[j]);
	    }
	   wait();
	   i++;
	   printf ("\n");
	 }
	return (1);
}
