---

simmot.c

Go to the documentation of this file.

/*
  simmot.c

  Simulated external motion interface for a DC servomotor system

  The simulation is clocked by calls to simEncoderRead(). Each time
  this call is done, the motor advances. Calls to simDacWrite() usually
  precede calls to simEncoderRead(), but need not. If not, the last
  value is used.

  Modification history:

  28-Jun-2000 WPS eliminated some unused variables.
  17-Apr-2000 WPS moved SIM_MAX_AXIS to simmot_n.h.
  14-Apr-2000 WPS added _attribute__((unused)) to ident
  21-Sep-1999  WPS eliminate sscanf and printf calls not supported under CE.
  21-Jun-1999  FMP changed SIM_MAX_AXIS to 8
  17-Apr-1998  FMP compile-switched stat'ing of switch files using
  USE_STAT_FILES
  17-Mar-1998  FMP added amp, motor, encoder simulations; changed
  SIM_MAX_AXIS from 3 to 4
  25-Nov-1997 FMP changed extLimitSwitchRead to extPos,NegLimit...
  01-Aug-1997  FMP changed encoder counts from int to double
  28-Jul-1997  FMP added extLimitSwitchRead()
  24-Jun-1997  FMP created
  */

/* FIXME-- encoder calibration stuff not used, stat files used instead */

/* Define this if you want to test limit switches, home switches by
   touching files and having stat() look for them. On an NFS file system,
   this can generate lots of traffic.
   Otherwise, the functions will just return "normal" status. */
/* #define USE_STAT_FILES */


#ifndef UNDER_CE
#include <stdio.h>              /* printf() */
#endif

#if !defined(rtlinux) && !defined(rtai)
#include <stdlib.h>             /* sizeof(), _itoa() */
#include <string.h>             /* strcpy() */
#ifndef UNDER_CE
#include <sys/types.h>
#endif
#endif
#ifdef USE_STAT_FILES
#include <sys/stat.h>
#endif
#include "sim.h"                /* these decls */
#include "extintf.h"            /* EXT_ENCODER_INDEX_MODEL */

#if !defined(rtlinux) && !defined(rtai)
#include "rcs_prnt.hh"          /* rcs_print() */
#include "inifile.h"
#endif
#include "amplifier.h"          /* AMPLIFIER_STRUCT */
#include "dcmotor2.h"           /* DCMOTOR_STRUCT */
#include "encoder.h"            /* ENCODER_STRUCT */

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

static char __attribute__((unused)) ident[] = "$Id: simmot.c,v 1.11 2001/12/01 21:24:29 paul_c Exp $";

#define NAMELEN 8               /* length of stat file names */

#include "simmot_n.h"

#if defined(rtlinux) || defined(rtai)
#include "rtlnml.h"

rtlnml_t nml_sim_command_channel = NULL;
rtlnml_t nml_sim_status_channel = NULL;

#endif

static AMPLIFIER_STRUCT amplifier[SIM_MAX_AXIS];
static double ampOutput[SIM_MAX_AXIS];
static DC_MOTOR_STRUCT dcmotor[SIM_MAX_AXIS];
static ENCODER_STRUCT encoder[SIM_MAX_AXIS];

#if defined(rtlinux) || defined(rtai)
static struct simmot_status_struct stat;
#endif


/*
  simInit takes INI file, then composes section strings as:

  AXIS_<0..SIM_MAX_AXIS-1>
  */
int simMotInit(const char * filename)
{
  int t;
#if !defined(rtlinux) && !defined(rtai)
  char sectionString[INIFILE_MAX_LINELEN];
#endif
  int retval = 0;
#if defined(rtlinux) || defined(rtai)
  struct simmot_command_struct *cmd;
#endif

#if defined(rtlinux) || defined(rtai)
  if (NULL == nml_sim_command_channel) {
    rtlnml_init();
#ifdef rtlinux
    nml_sim_command_channel = rtlnml_open("simmot_cmd","simmot","emc.nml",4096,1);
#else
    nml_sim_command_channel = rtainml_open("simmot_cmd","simmot","emc.nml",4096, 1,1014);
#endif
    if (NULL == nml_sim_command_channel) {
      return -1;
    }
  }

  if (NULL == nml_sim_status_channel) {
#ifdef rtlinux
    nml_sim_status_channel = rtlnml_open("simmot_stat","simmot","emc.nml",4096,1);
#else
    nml_sim_status_channel = rtainml_open("simmot_stat","simmot","emc.nml",4096, 1, 1015);
#endif
    if (NULL == nml_sim_status_channel) {
      return -1;
    }
  }

  switch(rtlnml_read(nml_sim_command_channel)) {
  case 0:
    /* FIXME  this function needs to be polled */
    break;

  case SIMMOT_COMMAND_TYPE:
    cmd = (struct simmot_command_struct *) rtlnml_get_local_pointer(nml_sim_command_channel);
    for (t = 0; t < SIM_MAX_AXIS; t++) {
      stat.amplifier[t] = amplifier[t] = cmd->amplifier[t];
      stat.dcmotor[t] = dcmotor[t] = cmd->dcmotor[t];
      stat.encoder[t] = encoder[t] = cmd->encoder[t];
    }
    stat.initialized = 1;
    stat.cmd_count++;
    rtlnml_write(nml_sim_status_channel, &stat,SIMMOT_STATUS_TYPE,sizeof(stat));
    break;

  case -1:
    /* Error from rtlnml_read() */
    return -1;

  default:
    /* THIS should never happen */
    break;
  }

#else

  for (t = 0; t < SIM_MAX_AXIS; t++) {
#ifndef UNDER_CE
     sprintf(sectionString, "AXIS_%d", t);
#else
    strcpy(sectionString,"AXIS_");
    _itoa(t,sectionString+5,10);
#endif

    if (0 != amplifierIniLoad(&amplifier[t], filename, sectionString)) {
      retval = -1;
    }
    ampOutput[t] = 0.0;

    if (0 != dcmotorIniLoad(&dcmotor[t], filename, sectionString)) {
      retval = -1;
    }

    if (0 != encoderIniLoad(&encoder[t], filename, sectionString)) {
      retval = -1;
    }
  }
#endif

  return retval;
}

int simMotQuit()
{
#if defined(rtlinux) || defined(rtai)

  if (NULL != nml_sim_command_channel) {
    rtlnml_close(nml_sim_command_channel,"sim_cmd");
  }

  if (NULL != nml_sim_status_channel) {
    rtlnml_close(nml_sim_status_channel,"sim_stat");
  }

  rtlnml_exit();
#endif

  return 0;
}

int simDacNum()
{
  return SIM_MAX_AXIS;
}

/*
  simDacWrite just runs a cycle on the amplifier simulator to get
  the resulting output voltage. This is saved in the ampOutput[]
  array for use by simEncoderRead(), which calls the motor simulation
  */
int simDacWrite(int dac, double volts)
{
  ampOutput[dac] = amplifierRunCycle(&amplifier[dac], volts);

  return 0;
}

int simDacWriteAll(int max, double * volts)
{
  int t;

  if (max > SIM_MAX_AXIS) {
    return -1;
  }

  for (t = 0; t < max; t++) {
    if (-1 == simDacWrite(t, volts[t])) {
      return -1;
    }
  }

  return 0;
}

unsigned int simEncoderIndexModel(void)
{
  return EXT_ENCODER_INDEX_MODEL_MANUAL;
}

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

  return 0;
}

int simEncoderNum()
{
  return SIM_MAX_AXIS;
}

/*
  simEncoderRead() clocks the simulation. The amp output set by the
  call to simDacWrite() for this axis is used as the motor input.
  */
int simEncoderRead(int axis, double * counts)
{
  double motorPos;
  int iCounts;

  motorPos = dcmotorRunCycle(&dcmotor[axis], ampOutput[axis]);
  iCounts = encoderGetCounts(&encoder[axis], motorPos);

  *counts = (double) iCounts;

  simPos[axis] = *counts;

  return 0;
}

int simEncoderReadAll(int max, double * counts)
{
  int t;

  if (max > SIM_MAX_AXIS) {
    return -1;
  }

  for (t = 0; t < max; t++) {
    if (-1 == simEncoderRead(t, &counts[t])) {
      return -1;
    }
  }

#if defined(rtlinux) || defined(rtai)
  if (NULL !=  nml_sim_status_channel) {
    for (t = 0; t < SIM_MAX_AXIS; t++) {
      stat.amplifier[t] = amplifier[t];
      stat.dcmotor[t] = dcmotor[t];
      stat.encoder[t] = encoder[t];
      stat.ampOutput[t] = ampOutput[t];
    }
    rtlnml_write(nml_sim_status_channel, &stat,SIMMOT_STATUS_TYPE,sizeof(stat));
  }
#endif
  return 0;
}

int simEncoderResetIndex(int encoder)
{
  return 0;
}

int simEncoderReadLatch(int encoder, int * flag)
{
#ifdef USE_STAT_FILES
  struct stat buf;
  char homeFile[NAMELEN];
#endif

  if (encoder < 0 ||
      encoder >= SIM_MAX_AXIS) {
    return -1;
  }

#ifdef USE_STAT_FILES
  /* to test home flag logic, this code tests for the existence
     of files "home<n>", for axis n.
     During controller debug, touch these files to create them
     and the controller should see a home flag. */

#ifndef UNDER_CE
  sprintf(homeFile, "home%d", encoder);
#else
  strcpy(homeFile,"home");
  _itoa(encoder, homeFile+4,10);
#endif

  if (0 == stat(homeFile, &buf)) {
    /* positive limit file exists */
    *flag = 1;
  }
  else {
    *flag = 0;
  }

#else
  *flag = 1;                    /* always show latch so homing will work */

#endif

  return 0;
}

int simEncoderReadLevel(int encoder, int * flag)
{
  *flag = 0;

  return 0;
}

int simMaxLimitSwitchRead(int axis, int * flag)
{
#ifdef USE_STAT_FILES
  struct stat buf;
  char maxFile[NAMELEN];
#endif

  if (axis < 0 ||
      axis >= SIM_MAX_AXIS) {
    return -1;
  }

#ifdef USE_STAT_FILES
  /* to test limit switch logic, this code tests for the existence
     of files "lim<n>p" for max limits on axis n.
     During controller debug, touch these files to create them
     and the controller should see a limit. */
#ifndef UNDER_CE
  sprintf(maxFile, "lim%dp", axis);
#else
  strcpy(maxFile,"lim");
  _itoa(axis, maxFile+3,10);
  strcat(maxFile,"p");
#endif

  if (0 == stat(maxFile, &buf)) {
    /* maximum limit file exists */
    *flag = 1;
  }
  else {
    *flag = 0;
  }

#else
  *flag = 0;

#endif

  return 0;
}

int simMinLimitSwitchRead(int axis, int * flag)
{
#ifdef USE_STAT_FILES
  struct stat buf;
  char minFile[NAMELEN];
#endif

  if (axis < 0 ||
      axis >= SIM_MAX_AXIS) {
    return -1;
  }

#ifdef USE_STAT_FILES
  /* to test limit switch logic, this code tests for the existence
     of files "lim<n>n" for min limits on axis n.
     During controller debug, touch these files to create them
     and the controller should see a limit. */

#ifndef UNDER_CE
  sprintf(minFile, "lim%dn", axis);
#else
  strcpy(minFile,"lim");
  _itoa(axis, minFile+3,10);
  strcat(minFile,"n");
#endif

  if (0 == stat(minFile, &buf)) {
    /* minimum limit file exists */
    *flag = 1;
  }
  else {
    *flag = 0;
  }

#else
  *flag = 0;

#endif

  return 0;
}

static int silly_count=0;

int simHomeSwitchRead(int axis, int * flag)
{
#ifdef USE_STAT_FILES
  struct stat buf;
  char homeFile[NAMELEN];
#endif

  if (axis < 0 ||
      axis >= SIM_MAX_AXIS) {
    return -1;
  }

#ifdef USE_STAT_FILES
  /* to test home switch logic, this code tests for the existence
     of file "hs<n>" for home switch on axis n.
     During controller debug, touch these files to create them
     and the controller should see a home. */

#ifndef UNDER_CE
  sprintf(homeFile, "hs%d", axis);
#else
  strcpy(homeFile,"hs");
  _itoa(axis, homeFile+2,10);
#endif

  if (0 == stat(homeFile, &buf)) {
    /* home switch file exists */
    *flag = 1;
  }
  else {
    *flag = 0;
  }

#else
  *flag = ((silly_count++)%100)>50; /* always show switch on so homing will work */

#endif

  return 0;
}

int simAmpEnable(int axis, int enable)
{
  if (axis < 0 ||
      axis >= SIM_MAX_AXIS) {
    return -1;
  }

  amplifierEnable(&amplifier[axis]);

  return 0;
}

int simAmpFault(int axis, int * fault)
{
  if (axis < 0 ||
      axis >= SIM_MAX_AXIS) {
    return -1;
  }

  *fault = amplifierIsTripped(&amplifier[axis]);

  return 0;
}

---