---

newstg.c

Go to the documentation of this file.

/*
  newstg.c

  Servo To Go board functions for new boards

  Modification history:

  26-Oct-2001 WPS created
*/

/*
  Porting Notes:

  The code that writes and read the encoders to get the number of axes
  causes encoder values to be 0 until the axes moves around a bit.
*/

/* ident tag */
#ifndef __GNUC__
#ifndef __attribute__
#define __attribute__(x)
#endif
#endif

static char __attribute__((unused)) ident[] = "$Id: newstg.c,v 1.6 2001/10/31 20:21:47 wshackle Exp $";


#if !defined(rtlinux) && !defined(rtai)
#include <stdio.h>
#endif

#include "newstg.h"               /* these decls */
#include "extintf.h"            /* EXT_ENCODER_INDEX_MODEL */

/* base address-- override default at compile time in stg.h,
   or at module load time with STG_BASE_ADDRESS=value */
unsigned short STG_BASE_ADDRESS = DEFAULT_STG_BASE_ADDRESS;
int FIND_STG_BASE_ADDRESS=1;

int STG_IRQ=DEFAULT_STG_IRQ;

#define MAX_AXES 8

#include "stgdefs.h"

/* Implementations of external functions */

#include "locsysio.h"

/* saved outputs, for stgAioCheck(), since we can't really read outputs */
static double checkedOutputs[MAX_AXES];

#define STG_INIT_WAIT 1000      /* usecs to wait after stgMotInit() */
int stgMotInit(const char * stuff)
{
  int t;

  /* call STG init, which is the "constructor" */
  ServoToGoConstructor(STG_IRQ);
  StopInterrupts();      /* we don't want interrupts */

  /* output 0's to amps */
  for (t = 0; t < GetAxes(); t++)
    {
      /* write 0 to DACs */
      stgDacWrite(t, 0.0);
    }

  /* init digital IO directions */
  stgDioInit(0);

  /* need to force a wait here-- calls to stgEncoderReadAll()
     immediately after this return garbage. Is there status to
     poll on for 'ready'? */
  for (t = 0; t < STG_INIT_WAIT; t++)
    {
      /* do a dummy 1 usec write */
      outb(0x80, 0);
    }

  return 0;
}

int stgMotQuit(void)
{
  int t;

  for (t = 0; t < GetAxes(); t++)
    {
      /* write 0's to DACs */
      stgDacWrite(t, 0.0);
    }

  ServoToGoDestructor();

  return 0;
}

int stgAdcNum(void)
{
  return GetAxes();
}

int stgAdcStart(int adc)
{
  if (adc < 0 ||
      adc >= GetAxes()) {
    return 0;                 /* don't care */
  }

  StartADC((unsigned short) adc);

  return 0;
}

#define STG_ADC_WAIT_USEC 20    /* microsecs to wait for conversion */

void stgAdcWait(void)
{
  int t;

  for (t = 0; t < STG_ADC_WAIT_USEC; t++) {
    outb(0x80, 0);              /* 0x80 is historical dummy, 1 usec write */
  }
}

int stgAdcRead(int adc, double * volts)
{
  if (adc < 0 ||
      adc >= GetAxes()) {
    return 0;                 /* don't care */
  }

  *volts = (double) SpinReadADC((unsigned short) adc);

  return 0;
}

int stgDacNum(void)
{
  return GetAxes();
}

int stgDacWrite(int dac, double volts)
{
  long lCounts;

  if (dac < 0 ||
      dac >= GetAxes())
    {
      return 0;                 /* don't care */
    }

  checkedOutputs[dac] = volts;

  // WPS -- Hack: This made little sense to be but this hack seems necessary.
  lCounts = (long) (( volts * 0x1FFF) / 20.0);

  RawDAC( dac, lCounts);

  return 0;
}

int stgDacWriteAll(int max, double * volts)
{
  int t;
  int smax;

  /* clip smax to max supported-- if they want more, ignore */
  if (max > GetAxes()) {
    smax = GetAxes();
  }
  else {
    smax = max;
  }

  for (t = 0; t < smax; t++) {
    if (0 != stgDacWrite(t, volts[t])){
      return -1;
    }
  }

  return 0;
}

unsigned int stgEncoderIndexModel(void)
{
  return EXT_ENCODER_INDEX_MODEL_MANUAL;
}

int stgEncoderSetIndexModel(unsigned int model)
{
  if (model != EXT_ENCODER_INDEX_MODEL_MANUAL)
    {
      return -1;
    }

  return 0;
}

int stgEncoderNum(void)
{
  return GetAxes();
}

int stgEncoderRead(int encoder, double * counts)
{
  double allCounts[GetAxes()];

  if (encoder < 0 ||
      encoder >= GetAxes()) {
    *counts = 0.0;
    return 0;
  }

  stgEncoderReadAll(encoder + 1, allCounts);

  *counts = allCounts[encoder];

  return 0;
}

int stgEncoderReadAll(int max, double * counts)
{
  LONGBYTE lbEnc[MAX_AXES];
  int t;
  int smax;                     /* how many we actually have */

  /* clip smax to max supported-- if they want more, give
     them zeros */
  if (max > GetAxes()) {
    smax = GetAxes();
  }
  else {
    smax = max;
  }

  EncoderLatch();
  EncReadAll( lbEnc);

  /* fill ours with the actual values */
  for (t = 0; t < smax; t++) {
    counts[t] = (double) lbEnc[t].Long;
  }
  /* and fill the rest with zeros */
  for (t = smax; t < max; t++) {
    counts[t] = 0.0;
  }

  return 0;
}

int stgEncoderResetIndex(int encoder)
{
  unsigned char latch_mask;
  if (encoder < 0 ||
      encoder >= GetAxes())
    {
      return 0;
    }
  

  if(GetModel() == MODEL1)
    {
      SelectIndexAxis( encoder);
      ResetIndexLatch();
    }
  else
    {
      latch_mask = 0xFF & ~(1 << encoder);
      ResetIndexLatches(0);
    }

  return 0;
}

int stgEncoderReadLatch(int encoder, int * flag)
{      
  
  if(GetModel() == MODEL1)
    {
      SelectIndexAxis(encoder);
      *flag = IndexPulseLatch();
    }
  else
    {
      *flag = GetIndexLatches();
      *flag = ((*flag & (1 << encoder)) != 0);
    }

  return 0;
}

/* basically same code as IndexPulseLatch() except mask is 2 bits higher */
int stgEncoderReadLevel(int encoder, int * flag)
{
  SelectIndexAxis(encoder);
  *flag = IndexPulse();

  return 0;
}

/* Note: board only supports 6 axes max of digital in, out for limit and
   home switches, amp enables  */

/* digital input bit masks, 32-bit word */
/* maps to ports A and B */
#define HOME_0    0x00000001
#define MIN_LIM_0 0x00000002
#define MAX_LIM_0 0x00000004
#define FAULT_0   0x00000008
#define HOME_1    0x00000010
#define MIN_LIM_1 0x00000020
#define MAX_LIM_1 0x00000040
#define FAULT_1   0x00000080
#define HOME_2    0x00000100
#define MIN_LIM_2 0x00000200
#define MAX_LIM_2 0x00000400
#define FAULT_2   0x00000800
#define HOME_3    0x00001000
#define MIN_LIM_3 0x00002000
#define MAX_LIM_3 0x00004000
#define FAULT_3   0x00008000

#ifdef STG_8_AXES
/* skip C, maps to port D */
#define HOME_4    0x01000000
#define MIN_LIM_4 0x02000000
#define MAX_LIM_4 0x04000000
#define FAULT_4   0x08000000
#define HOME_5    0x10000000
#define MIN_LIM_5 0x20000000
#define MAX_LIM_5 0x40000000
#define FAULT_5   0x80000000

#endif

/* no space for axes 7 and 8, so these won't go through */
int stgMaxLimitSwitchRead(int axis, int * flag)
{
  long bits;
  int retval = 0;

  bits = RawDIAll();
  *flag = 0;

  switch (axis)
    {
    case 0:
      if (bits & MAX_LIM_0)
        {
          *flag = 1;
        }
      break;

    case 1:
      if (bits & MAX_LIM_1)
        {
          *flag = 1;
        }
      break;

    case 2:
      if (bits & MAX_LIM_2)
        {
          *flag = 1;
        }
      break;

    case 3:
      if (bits & MAX_LIM_3)
        {
          *flag = 1;
        }
      break;

#ifdef STG_8_AXES
    case 4:
      if (bits & MAX_LIM_4)
        {
          *flag = 1;
        }
      break;

    case 5:
      if (bits & MAX_LIM_5)
        {
          *flag = 1;
        }
      break;

#endif

    default:
      retval = -1;
      break;
    }

  return retval;
}

/* no space for axes 7 and 8, so these won't go through */
int stgMinLimitSwitchRead(int axis, int * flag)
{
  long bits;
  int retval = 0;

  bits = RawDIAll();
  *flag = 0;

  switch (axis)
    {
    case 0:
      if (bits & MIN_LIM_0)
        {
          *flag = 1;
        }
      break;

    case 1:
      if (bits & MIN_LIM_1)
        {
          *flag = 1;
        }
      break;

    case 2:
      if (bits & MIN_LIM_2)
        {
          *flag = 1;
        }
      break;

    case 3:
      if (bits & MIN_LIM_3)
        {
          *flag = 1;
        }
      break;

#ifdef STG_8_AXES
    case 4:
      if (bits & MIN_LIM_4)
        {
          *flag = 1;
        }
      break;

    case 5:
      if (bits & MIN_LIM_5)
        {
          *flag = 1;
        }
      break;

#endif

    default:
      retval = -1;
      break;
    }

  return retval;
}

/* no space for axes 7 and 8, so these won't go through */
int stgHomeSwitchRead(int axis, int *flag)
{
  long bits;
  int retval = 0;

  bits = RawDIAll();
  *flag = 0;

  switch (axis)
    {
    case 0:
      if (bits & HOME_0)
        {
          *flag = 1;
        }
      break;

    case 1:
      if (bits & HOME_1)
        {
          *flag = 1;
        }
      break;

    case 2:
      if (bits & HOME_2)
        {
          *flag = 1;
        }
      break;

    case 3:
      if (bits & HOME_3)
        {
          *flag = 1;
        }
      break;

#ifdef STG_8_AXES
    case 4:
      if (bits & HOME_4)
        {
          *flag = 1;
        }
      break;

    case 5:
      if (bits & HOME_5)
        {
          *flag = 1;
        }
      break;

#endif

    default:
      retval = -1;
      break;
    }

  return retval;
}

/* there is space for axes 7 and 8, so these will go through */
int stgAmpEnable(int axis, int enable)
{
  if (axis < 0 || axis >= GetAxes())
    {
      return 0;
    }

  RawDOBitValPort( axis, (unsigned char) enable, 2); /* 2 is port C */

  return 0;
}

/* no space for axes 7 and 8, so these won't go through */
int stgAmpFault(int axis, int * flag)
{
  long bits;
  int retval = 0;

  bits = RawDIAll();
  *flag = 0;

  switch (axis)
    {
    case 0:
      if (bits & FAULT_0)
        {
          *flag = 1;
        }
      break;

    case 1:
      if (bits & FAULT_1)
        {
          *flag = 1;
        }
      break;

    case 2:
      if (bits & FAULT_2)
        {
          *flag = 1;
        }
      break;

    case 3:
      if (bits & FAULT_3)
        {
          *flag = 1;
        }
      break;

#ifdef STG_8_AXES
    case 4:
      if (bits & FAULT_4)
        {
          *flag = 1;
        }
      break;

    case 5:
      if (bits & FAULT_5)
        {
          *flag = 1;
        }
      break;

#endif

    default:
      retval = -1;
      break;
    }

  return retval;
}

/*
  Analog and Digital IO

  Analog and digital IO are mapped directly onto the board with no care
  that they don't collide with axes functions above. This is good in the
  sense that you can effect motion, bad in the same sense. This means that
  you should leave indices 0..axis-1 alone and use the higher indices for
  general-purpose IO. For example, if you have a 4-axis board and only
  3 real axes, you can use digital input at indices 12..15 for anything.

  Analog input is an option for the board. There are 8 inputs, which
  don't collide with any motion IO so you can use indices 0..7
  regardless of how many motion axes you have.

  If you have an 8 axis board, only 6 axis can be fully used for motion
  since there is not enough digital input for all the home and limit switches.

  For a 4-axis board, there are:
  24 digital inputs (includes limit switches and amp faults at 0..15)
  8 digital outputs (includes amp enables at 0..3)
  8 analog inputs (with option)
  4 analog outputs (includes amp outputs at 0..3)

  For an 8-axis board (6 motion axes max), there are:
  24 digital inputs (includes limit switches and amp faults at 0..23)
  8 digital outputs (includes amp enables at 0..5)
  8 analog inputs (with option)
  8 analog outputs (includes amp outputs at 0..5)

  Here's the actual map. If you aren't using some axes then their
  slots are up for grabs.

  DIGITAL INPUT
  -------------
  Index   Function      Connector/Pin
  -----   --------      -------------
  00      X home sw     P1/47
  01      X +lim sw     P1/45
  02      X -lim sw     P1/43
  03      X amp fault   P1/41
  04      Y home sw     P1/39
  05      Y +lim sw     P1/37
  06      Y -lim sw     P1/35
  07      Y amp fault   P1/33
  08      Z home sw     P1/31
  09      Z +lim sw     P1/29
  10      Z -lim sw     P1/27
  11      Z amp fault   P1/25
  12      U home sw     P1/23
  13      U +lim sw     P1/21
  14      U -lim sw     P1/19
  15      U amp fault   P1/17
  16      V home sw     P2/31
  17      V +lim sw     P2/29
  18      V -lim sw     P2/27
  19      V amp fault   P2/25
  20      W home sw     P2/23
  21      W +lim sw     P2/21
  22      W -lim sw     P2/19
  23      W amp fault   P2/17

  DIGITAL OUTPUT
  -------------
  Index   Function      Connector/Pin
  -----   --------      -------------
  00      X amp enable  P1/15
  01      Y amp enable  P1/13
  02      Z amp enable  P1/11
  03      U amp enable  P1/9
  04      V amp enable  P1/7
  05      W amp enable  P1/5
  06      (none)        P1/3
  07      (none)        P1/1

  ANALOG INPUT
  -------------
  Index   Function      Connector/Pin
  -----   --------      -------------
  00*     (none)        P2/1
  01*     (none)        P2/3
  02*     (none)        P2/5
  03*     (none)        P2/7
  04*     (none)        P2/9
  05*     (none)        P2/11
  06*     (none)        P2/13
  07*     (none)        P2/15

  ANALOG OUTPUT
  -------------
  Index   Function      Connector/Pin
  -----   --------      -------------
  00      X amp ref     P3/2
  01      Y amp ref     P3/8
  02      Z amp ref     P3/5
  03      U amp ref     P3/11
  04**    V amp ref     P4/2
  05**    W amp ref     P4/8
  06**    (none)        P4/5
  07**    (none)        P4/11

  * = only available as option
  ** = only available for 8-axis boards, with STG_8_AXES defined

  */

int stgDioInit(const char * stuff)
{
  /* set digital IO bits for A,B input, C output */
  DioDirection2( 0x0C);

  return 0;
}

int stgDioQuit(void)
{
  return 0;
}

int stgDioMaxInputs(void)
{
  return 24;
}

int stgDioMaxOutputs(void)
{
  return 24;
}

int stgDioRead(int index, int *value)
{
  long port = index/8;
  long bit_number = index%8;

  if(0 == value)
    {
      return -1;
    }
  *value = RawDIBitPort(bit_number, port);

  return 0;
}

/* writes go to the C register */
int stgDioWrite(int index, int value)
{

  long port = index/8;
  long bit_number = index%8;


  RawDOBitValPort(port,(value != 0),bit_number);
   return 0;
}

/* we can really read the outputs in the C register */
int stgDioCheck(int index, int *value)
{
  long port = 2;
  long bit_number = index%8;

  if(0 == value)
    {
      return -1;
    }
  *value = RawDIBitPort(bit_number, port);
  return 0;
}

/* can read bytes 0..2 */
int stgDioByteRead(int index, unsigned char *byte)
{
  if(byte ==0)
    {
      return -1;
    }
  *byte = RawDIPort(index);
  return 0;
}

/* can read shorts 0..1, 0 is BA, 1 is 0D */
int stgDioShortRead(int index, unsigned short *sh)
{
  unsigned char lo, hi;

  if (index == 0)
    {
      lo = RawDIPort(0);
      hi = RawDIPort(1);

      *sh = (hi << 8) + lo;
      return 0;
    }

  if (index == 1)
    {
      lo = RawDIPort(1);

      *sh = lo;
      return 0;
    }

  *sh = 0;
  return -1;
}

/* can only read 1 word, and this is 0DBA */
int stgDioWordRead(int index, unsigned int *word)
{
  if (index != 0)
    {
      *word = 0;
      return -1;
    }

  *word = RawDIPort(3) << 16;
  *word += RawDIPort(1) << 8;
  *word += RawDIPort(0);

  return 0;
}

/* index is 0 only for port C */
int stgDioByteWrite(int index, unsigned char byte)
{
  if(index == 0)
    {
      RawDOBytePort(byte,2);
      return 0;
    }
  
  return -1;
}

/* can't do a short write-- only 1 byte of digital out */
int stgDioShortWrite(int index, unsigned short sh)
{
  return -1;
}

/* can't do a word write-- only 1 byte of digital out */
int stgDioWordWrite(int index, unsigned int word)
{
  return -1;
}

/* can do a byte check on output C register */
int stgDioByteCheck(int index, unsigned char *byte)
{
  if(byte ==0)
    {
      return -1;
    }

  if (index == 0)
    {
      *byte = RawDIPort(2);
      return 0;
    }

  *byte = 0;
  return -1;
}

/* can't read a short-- only 8 bits of digital out */
int stgDioShortCheck(int index, unsigned short *sh)
{
  *sh = 0;
  return -1;
}

/* cant' read a word-- only 8 bits of digital out */
int stgDioWordCheck(int index, unsigned int *word)
{
  *word = 0;
  return -1;
}

int stgAioInit(const char * stuff)
{
  /* nothing need be done */
  return 0;
}

int stgAioQuit(void)
{
  return 0;
}

int stgAioMaxInputs(void)
{
  return 8;                     /* make sure you have this option */
}

int stgAioMaxOutputs(void)
{
  return GetAxes();
}

int stgAioStart(int index)
{
  return stgAdcStart(index);
}

void stgAioWait(void)
{
  stgAdcWait();
}

int stgAioRead(int index, double *volts)
{
  return stgAdcRead(index, volts);
}

int stgAioWrite(int index, double volts)
{
  return stgDacWrite(index, volts);
}

int stgAioCheck(int index, double *volts)
{
  if (index < 0 || index >= GetAxes()) {
    *volts = 0.0;
    return 0;
  }

  /* can't read outputs directly, so return stored value */
  *volts = checkedOutputs[index];

  return 0;
}

int stgModel()
{
  return GetModel();
}

---