---

encoder.c

Go to the documentation of this file.

/*
   _encoder.c

   C defs for encoder

   Modification history:

   14-Apr-2000 WPS added _attribute__((unused)) to ident
   23-Sep-1999  WPS replaced stdio functions with inet_file functions
   which work under CE.
   17-Mar-1998  FMP added section arg to encoderIniLoad()
   16-Jun-1997  FMP added index init
   24-Apr-1997  FMP added encoderIniLoad()
   17-Apr-1997  FMP created from C portion of original encoder.c
*/

#ifndef NO_STDIO_SUPPORT
#ifndef UNDER_CE
#include <stdio.h>
#endif
#include "inetfile.hh"          /* inet_file_open() */
#include "inifile.h"
#include "rcs_prnt.hh"          /* rcs_print() */
#endif
#include <math.h>               /* fmod() */
#include "encoder.h"            /* these decls */

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

static char __attribute__((unused)) ident[] = "$Id: encoder.c,v 1.2 2000/10/27 20:34:42 terrylr Exp $";

#define COUNTS_PER_REV_SET 0x01
#define ALL_SET (COUNTS_PER_REV_SET)

#define TWO_PI (2.0 * 3.1415926535898)
#define CALIBRATION_FUZZ 0.0001

/*
   returns non-zero if a multiple of TWO_PI lies in range
*/
static int sawIndex(double pos, double lastPos)
{
  if (pos <= 0.0 && lastPos >= 0.0)
  {
    return 1;
  }

  if (pos >= 0.0 && lastPos <= 0.0)
  {
    return 1;
  }

  if (pos - fmod(pos, TWO_PI) != lastPos - fmod(lastPos, TWO_PI))
  {
    return 1;
  }

  return 0;
}

int encoderInit(ENCODER_STRUCT * enc)
{
  if (0 == enc)
  {
    return -1;
  }

  /* parameters */
  enc->countsPerRev = 0;

  /* internal vars */
  enc->configured = 0;
  enc->counts = 0;
  enc->calibrating = 0;
  enc->offset = 0.0;
  enc->lastPosition = 0.0;
  enc->index = 0;

  return 0;
}

int encoderSetCountsPerRev(ENCODER_STRUCT * enc, int countsPerRev)
{
  if (0 == enc ||
      countsPerRev <= 0)
  {
    return -1;
  }

  enc->countsPerRev = countsPerRev;
  enc->configured |= COUNTS_PER_REV_SET;

  return 0;
}

int encoderGetCountsPerRev(ENCODER_STRUCT * enc)
{
  if (0 == enc ||
      ! (enc->configured & COUNTS_PER_REV_SET))
  {
    return 0;
  }

  return enc->countsPerRev;
}

int encoderGetCounts(ENCODER_STRUCT * enc, double position)
{
  if (0 == enc ||
      enc->configured != ALL_SET)
  {
    return 0;
  }

  if (enc->calibrating)
  {
    /* see if range [lastPosition, position] includes a multiple of
       TWO_PI. If so, we saw an index pulse */
    if (sawIndex(position, enc->lastPosition))
    {
      enc->offset = position - fmod(position, TWO_PI);
      enc->calibrating = 0;
    }
  }

  enc->counts = (int) (((double) enc->countsPerRev) *
                       ((position - enc->offset) / TWO_PI));
  enc->lastPosition = position;

  return enc->counts;
}

int encoderCalibrate(ENCODER_STRUCT * enc)
{
  if (0 == enc ||
      enc->configured != ALL_SET)
  {
    return -1;
  }

  enc->calibrating = 1;

  return 0;
}

int encoderNoCalibrate(ENCODER_STRUCT * enc)
{
  if (0 == enc)
  {
    return -1;
  }

  enc->calibrating = 0;

  return 0;
}

int encoderIsCalibrating(ENCODER_STRUCT * enc)
{
  if (0 == enc)
  {
    return -1;
  }

  return enc->calibrating;
}

int encoderIniLoad(ENCODER_STRUCT * enc, const char * filename, const char *section)
{
#ifndef NO_STDIO_SUPPORT

  int countsPerRev;
  int retval = 0;
  const char *inistring;
#ifndef UNDER_CE
  FILE *fp;

  if (NULL == (fp = fopen(filename, "r")))
  {
    rcs_print_error( "can't open ini file %s\n", filename);
    return -1;
  }
#else
  INET_FILE *fp;

  if (NULL == (fp = inet_file_open(filename, "r")))
  {
    rcs_print_error( "can't open ini file %s\n", filename);
    return -1;
  }
#endif

  if (NULL != (inistring = iniFind(fp, "COUNTS_PER_REV", section)))
  {
#ifndef UNDER_CE
    if (1 != sscanf((char *) inistring, "%d", &countsPerRev))
    {
      /* found, but invalid */
      rcs_print_error( "invalid COUNTS_PER_REV: %s\n", inistring);
      retval = -1;
    }
    else
    {
      encoderSetCountsPerRev(enc, countsPerRev);
    }
#else
    countsPerRev = atol(inistring);
    encoderSetCountsPerRev(enc, countsPerRev);
#endif

  }

  /* close inifile */
#ifndef UNDER_CE
  fclose(fp);
#else
  inet_file_close(fp);
#endif

  return retval;

#else

  return -1;

#endif
}

---