/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 2001 by Matthew R. Stein, based on code from Richard  *
 *                         Voyles.                               	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*-=-=-=-=-=-=-=-=-=-=-=- Description -=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-*
 This file should encapsulate, to the degree possible, the details of the
 Trident TRC004 card.  These routines will be called by the JLS interrupt
 routine to read/write to the Puma using the TRC004-TRC006 cards.

 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
#include <rtl.h>
#include <sys/io.h>
#include "../h/rciConfig.h"
#include "../h/pumaInterface.h"

short encoderOffsetG[6] = {0,0,0,8000,0,0};



short pumaDacWr(short val, short chan)
{
  outw(val+0x0800,TRC004_BASE + DAC_BASE + 2*chan);
  return I_OK;
}


short pumaEncWr(short val,short chan)
{

  outw(val+encoderOffsetG[chan], TRC004_BASE + ENC_LOAD_BASE + 2*chan);
  return I_OK;
}

short pumaDesWr(short val)
{
  outw(val, TRC004_BASE + DISCRETE_REG);
 return I_OK;
}

short pumaEncRd(short chan)
{

  return((short)(inw(TRC004_BASE + ENC_COUNT_BASE + 2*chan)
	       -encoderOffsetG[chan]));
  
}

short pumaIndRd(short chan)
{

  /* Index registers start at base address so no base offset for these */
  return((short)(inw(TRC004_BASE + 2*chan)
	-encoderOffsetG[chan]));
}
/* MS This looping is not acceptable during real-time execution.  Allow it here
 * because on the PUMA560 pots are only read once, during calibration.  If
 * this routine causes errors during calibration then need to do something to fix it
 */
short pumaPotRd(short chan, short discrete)
{
  short temp_val;
  short val;
  /* --- Enable the ADC conversion complete bit. */
  discrete = discrete | ADC_MASK_BIT;
  outw(discrete,TRC004_BASE + DISCRETE_REG);
  
  outw(chan,TRC004_BASE + ADC_MUX_SELECT); /* select the MUX channel */
  /*MS rev E AND trigger the conversion */
  /* --- Wait for the conversion complete flag. */
  temp_val=0;
  while ((inw(TRC004_BASE + STATUS_REG) & ADC_STAT_MASK) != 0)
    {
      if (temp_val++ > 10000){
	return(I_ERR);
      }
    }
  val = (short)(inw(TRC004_BASE + ADC_VALUE) & 0xFFF0);
  /* --- Reset the ADC conversion complete bit. */
  discrete = discrete & (~ADC_MASK_BIT);
  outw(discrete,TRC004_BASE + DISCRETE_REG);
  return(val);

}

short pumaDesRd()
{
  return((short)inw( TRC004_BASE + STATUS_REG));
}
/* Some useful control procedures, taken from RMV's robot.c
 */
short pumaControl(short option, short discrete, short chan, short *val)
{
  short tmp;

  switch (option) {
    case CLR_INDEX_BIT:         /* Clear one index bit */
      tmp = JT1_INDEX_BIT << chan;
      /* Clear the index bit */
      outw(discrete & ~tmp,TRC004_BASE + DISCRETE_REG);
      tmp = discrete = discrete | tmp;
      /* Re-enable the index bit */
      outw(tmp,TRC004_BASE + DISCRETE_REG);
      return I_OK;
    case CLR_INDEX_BITS:        /* Clear all index bits */
      /* Clear all index bits */
      outw(discrete & ~((short)ALL_INDEX_BITS),TRC004_BASE + DISCRETE_REG);
      tmp = discrete = discrete | ALL_INDEX_BITS;
      /* Re-enable all index bits */
      outw(tmp,TRC004_BASE + DISCRETE_REG);
      return I_OK;
    case CLR_ADC_BIT:   /* Clear A/D status bit */
      outw(discrete & ~((short)ADC_MASK_BIT),
	   TRC004_BASE + DISCRETE_REG);
      tmp = discrete = discrete | ADC_MASK_BIT;
      outw(tmp,TRC004_BASE + DISCRETE_REG );
      return I_OK;
    case POWER:         /* Enable/disable arm power */
      if (*val != 0){
        discrete = discrete | POWER_BIT;
        outw(discrete,TRC004_BASE + DISCRETE_REG);
      } else {
        discrete = discrete & ~(POWER_BIT);
        outw(discrete,TRC004_BASE + DISCRETE_REG);
      } /* endif */
      return I_OK;
    case STATUS:        /* Write the entire status word */
      outw(*val,TRC004_BASE + DISCRETE_REG);
      return I_OK;
    default:
      return I_BAD_OPTION;
  } /* endswitch */
}

short pumaStatus(short option, short chan, short *val)
{
  /* MS Also from Richard Voyles robot.c
     --- This function reads 1 or 6 short values of the robot. The particular
     --- value is chosen with the OPTION parameter, which is one out of a
     --- list of enumerated ioOptionsT. CHAN ranges from 0 to 5 (corresponding
     --- to PUMA joints 1 - 6). It returns one of the enumerated ioErrorsT. */
  
  
  short tmp;
  
  switch (option) {
  case INDEX_BIT:             /* Read one index bit */
    *val = inw(TRC004_BASE + STATUS_REG);
    tmp = JT1_INDEX_BIT << chan;
    *val = *val & tmp;
    if (*val !=0)
      *val = 1;
    return I_OK;
  case ALL_INDEX_BTS:        /* Read all index bits */
    *val = inw(TRC004_BASE + STATUS_REG);
    *val = *val & ALL_INDEX_BITS;
    return I_OK;
  case STATUS:        /* Read the entire status word */
    *val = inw(TRC004_BASE + STATUS_REG);
    return I_OK;
  default:
    return I_BAD_OPTION;
  } /* endswitch */
}



