---

emcsegmot.c

Go to the documentation of this file.

#ifdef rtlinux
#define MODULE                  /* make it a module for RT-Linux */
#endif

/*
  emcsegmot.c

  Real-time motion controller

  Queued 3-axis motion planning and cubic subinterpolation.

  Modification history:

  8-Jun-1999  FMP ran smTask at twice the servo rate, instead of at rate
  configured by now-obsolete SM_CYCLE_TIME. It's twice so the effective
  pulse rate is the same as the servo cycle time, e.g., 100 microsecond
  cycle time means a pulse every 100 microseconds at max speed. Set
  'bigVel' with emcmotStatus->limitVel, for upper velocity limit.
  Added EMCMOT_SET_VEL_LIMIT
  8-Jun-1999 RSB changed the call to sqInitQueue, that now requires a
  valid pointer to the start of the queue as an argument. When
  compiling for rtlinux it will pass a pointer in the shared memory
  area, if not, it allocates memory with malloc (assigned to
  sqMemPool) and passes the pointer of the allocated memory pool
  7-Jun-1999 RSB changed the line for logging of the magnitude of the
  velocity on traj level: now gets it's info by calling sqGetVel()
  7-Jun-1999 RSB copied the following changes
  from emcmot.c (differences between emcsegmot and emcmot are all
  segmentqueue related)

  7-Jun-1999  FMP fixed logging of trajectory cycle points with the logIt
  flag, without which points were logged every servo cycle
  2-Jun-1999  FMP bracketed debounce of inputs so that it's not done when
  using steppers-- the debounce caused motion to stop when jog speeds were
  less than 1.0.
  21-May-1999  FMP added EMCMOT_LOG_TYPE_TRAJ_POS for logging of Cartesian
  points from the trajectory planner at the traj rate
  8-May-1999  FMP bracketed references to emcmotLog out of RT_FIFO sections;
  added 1 to size of fifos in rtf_create. Note: the status structure worked
  fine when created with sizeof(status structure), but the command structure
  caused infinite wait during Linux process write() to fifo if it was exactly
  the size of the command. When it was made one byte larger, it worked.
  8-Mar-1999  FMP changed calls to tpCreate to provide a third argument
  for the queue space, and added queueTcSpace, freeAxisTcSpace arrays for
  this memory.
  3-Mar-1999  FMP took out static malloc stuff, since we now use rtmalloc.h
  in files that do malloc(), which is macroed to kmalloc().
  25-Feb-1999  FMP changed queue.queue.full to tcqFull(&queue.queue) to
  reflect the change in tc.c; added commandEcho to status
  24-Feb-1999  RSB added switched for rtlinux_1_0
  18-Feb-1999  FMP replaced hard-coded fifo numbers with symbolic names
  from emcmot.h


  18-May-1999  RSB added some extra debug printing. Mostly Checking on -1
  return values of segmentqueue functions.
  16-Mar-1999  RSB added the changes FMP had made to emcmot.c: "12-Feb-1999
  FMP reportError'ed following error, and SET_AXIS_ERROR_FLAG in addition
  to SET_AXIS_FERROR_FLAG; cleared error flags when enabling" and "25-Feb-1999
  FMP added commandEcho to status"
   9-Mar-1999  RSB changed some calls to segmentqueue, that has been changed
   to use the PmPose stuff
   8-Mar-1999  FMP changed calls to tpCreate to provide a third argument
  for the queue space, and added queueTcSpace, freeAxisTcSpace arrays for
  this memory.
   8-Mar-1999  RSB changed the implementation of the pause, resume and step
   commands. The code for these commands in coord mode now differs from that
   in free mode
   5-Mar-1999  RSB removed the static malloc stuff, since we now use rtmalloc
   3-Mar-1999  RSB added a call to sqSetFeed every time sqSetVmax is called
  24-Feb-1999  RSB added switches for rtlinux_1_0
  18-Feb-1999  FMP replaced hard-coded fifo numbers with symbolic names
  from emcmot.h

  11-Feb-1999  RSB created from emcmot.c

  10-Feb-1999  RSB changed the rtlinux-0.9H switch to rtlinux-0.9J
  10-Feb-1999  FMP set the non-moving axes for the homing destination to
  0 so they wouldn't erroneously count toward total displacement for move;
  fixed a failure to reset lastInput[] when the input offset changed due
  to homing
  7-Feb-1999  FMP added input debounce with bigVel and lastInput[], so that
  inputs/cycle time > 10X max speed are set to the last input value; made
  emcmotStatus->ferror the mag of the max; added logging of instantaneous
  following error "thisFerror[]"
  4-Feb-1999  FMP removed reportError on limits, since servo jitter about
  a limit caused flooding of these reports; added fix to weird jog problem
  caused when you switched to another axis, and the previous value it had
  for another axis' goal position wasn't "here", so it added to the scalar
  distance and slowed the move
  22-Jan-1999 WPS added memset to init sharedmemory area.
  21-Jan-1999  FMP fixed bug in continuous jogging where values for
  non-jogged axes were uninitialized, instead of read out of status
  12-Nov-1998  FMP made fixes to axis homing and other problems discovered
  by Angelo: axis home destination is twice axis range; setting offset to
  latch position is now accompanied by corresponding correction of input
  position to avoid momentary jump.
  22-Oct-1998  FMP zeroed shared memory; removed call to extEncoderReadAll()
  after extEncoderSetIndexModel() in init_module()
  13-Oct-1998  FMP changed continuous jogging to go to soft limit
  7-Oct-1998 FMP added homing vel stuff
  25-Sep-1998  FMP set axis error flag if on hard or soft limit
  2-Sep-1998  FMP put inRange back in, and included soft and hard limits
  in check to prevent moves from being initiated; added call to tpAbort()
  when inRange() failed in EMCMOT_SET_LINE,CIRCLE command cases
  10-Aug-1998  FMP fixed cut-and-paste bug in which maxLimitSwitchCount
  was being used for min limit switch debounce
  5-Aug-1998  FMP added baseline stepper motor control, keeping all original
  calcs and bracketing new code with ifdef STEPPER_MOTORS
  8-Jul-1998  FMP replaced -shm, -base cmd line args to main() with
  sym=value style, as per insmod, using getArgs() function here
  6-Jul-1998  FMP added -base to main() simulation, not that the simulation
  uses physical shared memory at this point
  15-Jun-1998  FMP added EMCMOT_SET_TERM_COND
  27-May-1998  FMP incremented head and tail in emcmotController()
  11-May-1998  FMP took out <sys/types.h> for size_t, since it gave
  compiler errors. I inlined decl of size_t as unsigned int.
  3-Apr-1998  FMP added active axis concept, with GET_AXIS_ACTIVE_FLAG(),
  SET_AXIS_ACTIVE_FLAG(), etc.
  26-Mar-1998  FMP replaced local etime(), esleep(), and shmget()/shmat()
  with rcslib equivalents, for portability; changed saveLatch[] to doubles
  from ints since that's what they really are
  23-Mar-1998  FMP pulled reportError() out of plat ifdefs since it was
  the same for all
  3-Mar-1998  FMP added -shm to main() process
  25-Feb-1998  FMP added AXIS_ERROR_FLAG
  23-Feb-1998  FMP added logging all inpos, outpos
  17-Feb-1998  FMP added homed bit
  13-Feb-1998  FMP forced inRange() to return 1 always, since it was
  erroneously returning 0 sometimes and causing dropped moves. Will fix later.
  12-Feb-1998  FMP added error logging to memory queue
  9-Feb-1998  FMP fixed bug in which I forgot to set free planner traj
  times in setTrajCycleTime()
  6-Feb-1998  FMP added log types
  22-Jan-1998  FMP changed priority to 1
  20-Jan-1998  FMP added COMM_COPY compile flag to select between direct
  access of upper memory or OS shared memory (fast), or copy-in, copy-out
  access (slowwwww).
  13-Jan-1998  FMP took out stdlib.h, when switching to rtlinux-0.5a
  12-Jan-1998  FMP put RT_FIFO switch in, since we were having NaN
  problems with shared memory (race condition?)
  12-Jan-1998  FMP took rt_use_fp() out of emcmotCommandHandler since
  it's now just a function within emcmotController, which does call it
  8-Jan-1998  FMP wrote polarity for enabling, instead of direct 1, 0;
  disabled amps in cleanup_module since we took it out of extQuit code;
  fixed bug where max lim switch flag was set for both max and min
  7-Jan-1998  FMP got rid of rt_task_suspend call in emcmotCommandHandler,
  when changing cycle times. It hangs now that emcmotCommandHandler is
  part of RT task
  6-Jan-1998  FMP switched to shared memory instead of fifos; added
  PID gain and input/output offset globals for init
  25-Nov-1997  FMP changed calls from extLimitSwitchRead() to
  extPos,NegLimit...
  4-Nov-1997  FMP put call to rt_use_fp() in emcmotController(). It was
  in emcmotCommandHandler(), and everything seemed to work OK, but rtlinux
  fp code example had it in cyclic code.
  16-Oct-1997  FMP added compile-time globals
  15-Oct-1997  FMP fixed bug in jog message handlers where vel was being
  set to status, not command
  25-Sep-1997  FMP added EMCMOT_NO_MAIN flag for not compiling in the
  default main loop
  13-Aug-1997  FMP removed setting of pidOutput[] to 0 when axis disabled,
  to allow for DAC writes to go out
  01-Aug-1997  FMP changed jointPos from int to double; called pidReset()
  upon enabling
  15-Jul-1997  FMP changed estop to enable, for consistency with interface
  30-Jun-1997  FMP added axis software limits; incremental and abs jog
  24-Jun-1997  FMP combined with non-RT-Linux code; added estop; added
  external interface "ext*" calls
  2-May-1997  FMP added logging
  17-Apr-1997  FMP added computeTime calcs
  16-Apr-1997 FMP created */

#ifdef rtlinux

/*
  FIXME-- should include <stdlib.h> for sizeof(), but conflicts with
  a bunch of <linux> headers
  */
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/version.h>
#include <linux/errno.h>
#if defined(rtlinux_09J) || defined(rtlinux_1_0)
#include <rtl_sched.h>
#include <rtl_fifo.h>
#else
#include <linux/rt_sched.h>
#include <linux/rtf.h>
#endif
#include <asm/io.h>

#include <math.h>
#include <stdarg.h>             /* vsprintf() */

#else  /* not rtlinux */

#include "_timer.h"             /* rcslib etime(), esleep() */
#include "_shm.h"               /* rcslib shm_t, rcs_shm_open(), ... */
#include <stdio.h>              /* printf() */
#include <stdlib.h>             /* sizeof() */
#include <math.h>
#include <stdarg.h>             /* va_start() */
#include <string.h>             /* memset() */

#endif /* else not rtlinux */

#include "emcpos.h"
#include "emcmotcfg.h"
#include "emcmotglb.h"
#include "emcmot.h"
#include "pid.h"
#include "cubic.h"

#include "tc.h"
#include "tp.h"

#include "segmentqueue.h"

#include "extintf.h"
#include "mmxavg.h"
#include "emcmotlog.h"

/* ident tag */
static char ident[] = "$Id: emcsegmot.c,v 1.3 2000/12/21 16:22:11 wshackle Exp $";

#ifdef rtlinux

/* useful conversion from secs to RTIME */
/* RT_TICKS_PER_SEC is 1193180, BTW */
#define SECS_TO_RTIME(secs) ((RTIME) (RT_TICKS_PER_SEC * secs))

/* task struct */
static RT_TASK emcmotTask;
#define RT_TASK_PRIORITY 1      /* the highest is 1, the lowest is
                                   RT_LOWEST_PRIORITY */
#define RT_TASK_STACK_SIZE 2048

/* STEPPER MOTORS */
/* stepper motor data */
static RT_TASK smTask;
#define SM_TASK_PRIORITY 2
#define SM_TASK_STACK_SIZE 2048
/* END STEPPER MOTORS */

/* for RT-Linux, can have watchdog sound port, parallel port bit toggling */
#define SOUND_PORT 0x61
#define SOUND_MASK 0xFD;
static unsigned char soundByte;
static unsigned char toggle = 0;

#endif /* rtlinux */

/*
  Principles of communication:

  Data is copied in or out from the various types of comm mechanisms:
  FIFOs or mapped memory for Linux/RT-Linux, or OS shared memory for Unixes.

  For FIFOs,
  emcmotCommandBuffer is used as arg to fifo get, and emcmotCommand
  points to this;
  emcmotStatusBuffer is used as arg to fifo put, and emcmotStatus
  points to this;
  there is no emcmotError, since error reporting is done at report time
  into the error fifo;
  there is no emcmotLog, since logging is done at log time
  into the log fifo.

  For shared memory or mapped memory, emcmotStruct is ptr to this memory.
  emcmotCommand points to emcmotStruct->command,
  emcmotStatus points to emcmotStruct->status,
  emcmotError points to emcmotStruct->error, and
  emcmotLog points to emcmotStruct->log.
 */

#if defined (rtlinux) && defined (RT_FIFO)

/* need to allocate command, status */
static EMCMOT_COMMAND emcmotCommandBuffer;
static EMCMOT_STATUS emcmotStatusBuffer;

#else  /* not rtlinux or not RT_FIFO */

/* need to point to command, status, error, and log */
EMCMOT_STRUCT *emcmotStruct;

#ifndef rtlinux
/* also need to get shared memory id */
static shm_t *shmem = NULL;
#endif

#endif /* not rtlinux or not RT_FIFO */

/* ptrs to either buffered copies or direct memory for
   command and status */
static EMCMOT_COMMAND *emcmotCommand;
static EMCMOT_STATUS *emcmotStatus;
#if defined (rtlinux) && defined (RT_FIFO)
#else
static EMCMOT_ERROR *emcmotError; /* unused for RT_FIFO */
static EMCMOT_LOG *emcmotLog;   /* unused for RT_FIFO */
#endif /* not RT_FIFO */

static EMCMOT_LOG_STRUCT ls;
static int logSkip = 0;         /* how many to skip, for per-cycle logging */
static int loggingAxis = 0;     /* record of which axis to log */
static int logIt = 0;

/* flag for enabling, disabling watchdog; multiple for down-stepping */
static int wdEnable = 0;
static int wdWait = 0;
static int wdCount = 0;

/* debouncing */
#define LIMIT_SWITCH_DEBOUNCE 10
static int maxLimitSwitchCount[EMCMOT_MAX_AXIS];
static int minLimitSwitchCount[EMCMOT_MAX_AXIS];

/* diagnostics printing */
#ifdef rtlinux
#define diagnostics printk
#else
#define diagnostics printf
#endif

/* macros for reading, writing bit flags */

/* motion flags */

#define GET_MOTION_ERROR_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_ERROR_BIT ? 1 : 0)

#define SET_MOTION_ERROR_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_ERROR_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_ERROR_BIT;

#define GET_MOTION_COORD_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_COORD_BIT ? 1 : 0)

#define SET_MOTION_COORD_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_COORD_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_COORD_BIT;

#define GET_MOTION_INPOS_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_INPOS_BIT ? 1 : 0)

#define SET_MOTION_INPOS_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_INPOS_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_INPOS_BIT;

#define GET_MOTION_ENABLE_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_ENABLE_BIT ? 1 : 0)

#define SET_MOTION_ENABLE_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_ENABLE_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_ENABLE_BIT;

/* axis flags */

#define GET_AXIS_ENABLE_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_ENABLE_BIT ? 1 : 0)

#define SET_AXIS_ENABLE_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_ENABLE_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_ENABLE_BIT;

#define GET_AXIS_ACTIVE_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_ACTIVE_BIT ? 1 : 0)

#define SET_AXIS_ACTIVE_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_ACTIVE_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_ACTIVE_BIT;

#define GET_AXIS_INPOS_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_INPOS_BIT ? 1 : 0)

#define SET_AXIS_INPOS_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_INPOS_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_INPOS_BIT;

#define GET_AXIS_ERROR_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_ERROR_BIT ? 1 : 0)

#define SET_AXIS_ERROR_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_ERROR_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_ERROR_BIT;

#define GET_AXIS_PSL_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_MAX_SOFT_LIMIT_BIT ? 1 : 0)

#define SET_AXIS_PSL_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_MAX_SOFT_LIMIT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_MAX_SOFT_LIMIT_BIT;

#define GET_AXIS_NSL_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_MIN_SOFT_LIMIT_BIT ? 1 : 0)

#define SET_AXIS_NSL_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_MIN_SOFT_LIMIT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_MIN_SOFT_LIMIT_BIT;

#define GET_AXIS_PHL_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_MAX_HARD_LIMIT_BIT ? 1 : 0)

#define SET_AXIS_PHL_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_MAX_HARD_LIMIT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_MAX_HARD_LIMIT_BIT;

#define GET_AXIS_NHL_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_MIN_HARD_LIMIT_BIT ? 1 : 0)

#define SET_AXIS_NHL_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_MIN_HARD_LIMIT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_MIN_HARD_LIMIT_BIT;

#define GET_AXIS_HOME_SWITCH_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_HOME_SWITCH_BIT ? 1 : 0)

#define SET_AXIS_HOME_SWITCH_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_HOME_SWITCH_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_HOME_SWITCH_BIT;

#define GET_AXIS_HOMING_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_HOMING_BIT ? 1 : 0)

#define SET_AXIS_HOMING_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_HOMING_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_HOMING_BIT;

#define GET_AXIS_HOMED_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_HOMED_BIT ? 1 : 0)

#define SET_AXIS_HOMED_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_HOMED_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_HOMED_BIT;

#define GET_AXIS_FERROR_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_FERROR_BIT ? 1 : 0)

#define SET_AXIS_FERROR_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_FERROR_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_FERROR_BIT;

#define GET_AXIS_FAULT_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_FAULT_BIT ? 1 : 0)

#define SET_AXIS_FAULT_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_FAULT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_FAULT_BIT;

/* polarity flags */

#define GET_AXIS_ENABLE_POLARITY(ax) (emcmotStatus->axisPolarity[ax] & EMCMOT_AXIS_ENABLE_BIT ? 1 : 0)

#define SET_AXIS_ENABLE_POLARITY(ax,fl) if (fl) emcmotStatus->axisPolarity[ax] |= EMCMOT_AXIS_ENABLE_BIT; else emcmotStatus->axisPolarity[ax] &= ~EMCMOT_AXIS_ENABLE_BIT;

#define GET_AXIS_PHL_POLARITY(ax) (emcmotStatus->axisPolarity[ax] & EMCMOT_AXIS_MAX_HARD_LIMIT_BIT ? 1 : 0)

#define SET_AXIS_PHL_POLARITY(ax,fl) if (fl) emcmotStatus->axisPolarity[ax] |= EMCMOT_AXIS_MAX_HARD_LIMIT_BIT; else emcmotStatus->axisPolarity[ax] &= ~EMCMOT_AXIS_MAX_HARD_LIMIT_BIT;

#define GET_AXIS_NHL_POLARITY(ax) (emcmotStatus->axisPolarity[ax] & EMCMOT_AXIS_MIN_HARD_LIMIT_BIT ? 1 : 0)

#define SET_AXIS_NHL_POLARITY(ax,fl) if (fl) emcmotStatus->axisPolarity[ax] |= EMCMOT_AXIS_MIN_HARD_LIMIT_BIT; else emcmotStatus->axisPolarity[ax] &= ~EMCMOT_AXIS_MIN_HARD_LIMIT_BIT;

#define GET_AXIS_HOME_SWITCH_POLARITY(ax) (emcmotStatus->axisPolarity[ax] & EMCMOT_AXIS_HOME_SWITCH_BIT ? 1 : 0)

#define SET_AXIS_HOME_SWITCH_POLARITY(ax,fl) if (fl) emcmotStatus->axisPolarity[ax] |= EMCMOT_AXIS_HOME_SWITCH_BIT; else emcmotStatus->axisPolarity[ax] &= ~EMCMOT_AXIS_HOME_SWITCH_BIT;

#define GET_AXIS_HOMING_POLARITY(ax) (emcmotStatus->axisPolarity[ax] & EMCMOT_AXIS_HOMING_BIT ? 1 : 0)

#define SET_AXIS_HOMING_POLARITY(ax,fl) if (fl) emcmotStatus->axisPolarity[ax] |= EMCMOT_AXIS_HOMING_BIT; else emcmotStatus->axisPolarity[ax] &= ~EMCMOT_AXIS_HOMING_BIT;

#define GET_AXIS_FAULT_POLARITY(ax) (emcmotStatus->axisPolarity[ax] & EMCMOT_AXIS_FAULT_BIT ? 1 : 0)

#define SET_AXIS_FAULT_POLARITY(ax,fl) if (fl) emcmotStatus->axisPolarity[ax] |= EMCMOT_AXIS_FAULT_BIT; else emcmotStatus->axisPolarity[ax] &= ~EMCMOT_AXIS_FAULT_BIT;

static SEGMENTQUEUE queue;
static SEGMENT *sqMemPool;      /* to use for memory allocation with malloc
                                   for non RT compilations */

static TP_STRUCT freeAxis[EMCMOT_MAX_AXIS]; /* free mode planners */
static CUBIC_STRUCT cubic[EMCMOT_MAX_AXIS];

/* space for trajectory planner queues, plus 10 more for safety */
/* FIXME-- default is used; dynamic is not honored */
static TC_STRUCT queueTcSpace[DEFAULT_TC_QUEUE_SIZE + 10];
#define FREE_AXIS_QUEUE_SIZE 4  /* don't really queue free axis motion */
static TC_STRUCT freeAxisTcSpace[EMCMOT_MAX_AXIS][FREE_AXIS_QUEUE_SIZE];

static double rawInput[EMCMOT_MAX_AXIS];        /* raw feedback from sensors */
static double rawOutput[EMCMOT_MAX_AXIS]; /* raw output to actuators */

static double trajPos[EMCMOT_MAX_AXIS]; /* trajectory point, in joints */
static double jointPos[EMCMOT_MAX_AXIS]; /* interpolated point, in joints */
static double oldJointPos[EMCMOT_MAX_AXIS]; /* ones from last cycle, for vel */
static double oldInput[EMCMOT_MAX_AXIS]; /* ones for actual pos, last cycle */
static EmcPose oldPos;           /* last position, used for vel differencing */
static EmcPose oldVel, newVel;   /* velocities, used for acc differencing */
static EmcPose newAcc;           /* differenced acc */

/* last good value of axis feedback, in user units */
static double lastInput[EMCMOT_MAX_AXIS];

/* value of speed past which we debounce the feedback */
static double bigVel = 1.0;     /* start with reasonable value */

static int waitingForLatch[EMCMOT_MAX_AXIS]; /* flags for homing */
static int latchFlag[EMCMOT_MAX_AXIS]; /* flags for axis latched */
static double saveLatch[EMCMOT_MAX_AXIS]; /* saved axis latch values */

static int enabling = 0;        /* starts up disabled */
static int coordinating = 0;    /* starts up in free mode */

static int wasOnLimit[EMCMOT_MAX_AXIS]; /* flag for being on a limit
                                          last cycle */
static int stepping = 0;
static int idForStep = 0;

/* STEPPER MOTORS */

/* FIXME-- shouldn't hard code parallel port, use ext intf instead */
#include "parport.h"

static double smStepsPerUnit[EMCMOT_MAX_AXIS];
static int smStepsAccum[EMCMOT_MAX_AXIS];
static unsigned char smClkPhase[EMCMOT_MAX_AXIS];

/* END STEPPER MOTORS */

/* min-max-avg structs for traj and servo cycles */
static MMXAVG_STRUCT tMmxavg;
static MMXAVG_STRUCT sMmxavg;
static MMXAVG_STRUCT nMmxavg;

#ifdef rtlinux

static double etime()
{
  RTIME now;

  now = rt_get_time();          /* in RT_TICKS */

  return ((double) now) / ((double) RT_TICKS_PER_SEC);
}

#endif /* if rtlinux */

static void reportError(const char *fmt, ...)
{
  va_list args;
  char error[EMCMOT_ERROR_LEN];

  va_start(args, fmt);
  vsprintf(error, fmt, args);
  va_end(args);

#if defined (rtlinux) && defined (RT_FIFO)
  rtf_put(EMCMOT_ERROR_RTF, error, EMCMOT_ERROR_LEN);

#else
  emcmotErrorPut(emcmotError, error);

#endif
}

/* axis lengths */
#define XRANGE ((emcmotStatus->maxLimit[0] - emcmotStatus->minLimit[0]))
#define YRANGE ((emcmotStatus->maxLimit[1] - emcmotStatus->minLimit[1]))
#define ZRANGE ((emcmotStatus->maxLimit[2] - emcmotStatus->minLimit[2]))

/* inRange() returns non-zero if the position lies within the axis
   limits, or 0 if not */
static int inRange(EmcPose pos)
{
  double joint[EMCMOT_MAX_AXIS];
  int axis;

  /* fill in all joints with 0 */
  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
    {
      joint[axis] = 0.0;
    }

  /* now fill in with real values, for joints that are used */
  inverseKinematics(pos, joint);

  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
    {
      if (! GET_AXIS_ACTIVE_FLAG(axis))
        {
          /* if axis is not active, don't even look at its limits */
          continue;
        }

      if (joint[axis] > emcmotStatus->maxLimit[axis] ||
          joint[axis] < emcmotStatus->minLimit[axis])
        {
          return 0;             /* can't move further past limit */
        }
    }

  /* okay to move */
  return 1;
}

/* checkLimits() returns 1 if none of the soft or hard limits are
   set, 0 if any are set */
static int checkLimits()
{
  int axis;

  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
    {
      if (! GET_AXIS_ACTIVE_FLAG(axis))
        {
          /* if axis is not active, don't even look at its limits */
          continue;
        }

      if (GET_AXIS_PSL_FLAG(axis) ||
          GET_AXIS_NSL_FLAG(axis) ||
          GET_AXIS_PHL_FLAG(axis) ||
          GET_AXIS_NHL_FLAG(axis))
        {
          return 0;
        }
    }

  return 1;
}

/* check the value of the axis and velocity against current position,
   returning 1 (okay) if the request is to jog off the limit, 0 (bad)
   if the request is to jog further past a limit. Software limits are
   ignored if the axis hasn't been homed */
static int checkJog(int axis, double vel)
{
  if (axis < 0 ||
      axis >= EMCMOT_MAX_AXIS)
    {
      return 0;                 /* can't jog out-of-range axis */
    }

  if (vel > 0.0 &&
      (GET_AXIS_PSL_FLAG(axis) ||
       GET_AXIS_PHL_FLAG(axis)))
    {
      return 0;                 /* can't jog further past max limit */
    }

  if (vel < 0.0 &&
      (GET_AXIS_NSL_FLAG(axis) ||
       GET_AXIS_NHL_FLAG(axis)))
    {
      return 0;                 /* can't jog further past min limit */
    }

  /* okay to jog */
  return 1;
}

/* call this when setting the trajectory cycle time */
static void setTrajCycleTime(double secs)
{
  static int t;

  /* make sure it's not zero */
  if (secs <= 0.0)
    {
      return;
    }

  /* compute the interpolation rate as nearest integer to traj/servo*/
  emcmotStatus->interpolationRate =
    (int) (secs / emcmotStatus->servoCycleTime + 0.5);

  /* set traj planner */
  sqSetCycleTime(&queue, secs);

  /* set the free planners, cubic interpolation rate and segment time */
  for (t = 0; t < EMCMOT_MAX_AXIS; t++)
    {
      tpSetCycleTime(&freeAxis[t], secs);
      cubicSetInterpolationRate(&cubic[t], emcmotStatus->interpolationRate);
    }

  /* copy into status out */
  emcmotStatus->trajCycleTime = secs;
}

/* call this when setting the servo cycle time */
static void setServoCycleTime(double secs)
{
  static int t;

  /* make sure it's not zero */
  if (secs <= 0.0)
    {
      return;
    }

  /* compute the interpolation rate as nearest integer to traj/servo*/
  emcmotStatus->interpolationRate =
    (int) (emcmotStatus->trajCycleTime / secs + 0.5);

  /* set the cubic interpolation rate and PID cycle time */
  for (t = 0; t < EMCMOT_MAX_AXIS; t++)
    {
      cubicSetInterpolationRate(&cubic[t], emcmotStatus->interpolationRate);
      cubicSetSegmentTime(&cubic[t], secs);
#ifndef STEPPER_MOTORS
      pidSetCycleTime(&emcmotStatus->pid[t], secs);
#endif
    }

  /* copy into status out */
  emcmotStatus->servoCycleTime = secs;
}

/*
  emcmotCommandHandler() is called each main cycle to read the
  shared memory buffer
  */
static int emcmotCommandHandler()
{
  int axis;                     /* loop counter */
  EmcPose jogPos;                /* where to jog */
  double saveVmax;              /* copy of vMax for restoring */
#ifdef rtlinux
#ifdef RT_FIFO
  int err;
#endif
#endif
  int valid;

  /* copy command from outside into local buffers */

#if defined (rtlinux) && defined (RT_FIFO)
  while ((err = rtf_get(EMCMOT_COMMAND_RTF, &emcmotCommandBuffer, sizeof(EMCMOT_COMMAND))) == sizeof(EMCMOT_COMMAND))
    {

#endif /* rtlinux and RT_FIFO */

  /* check for split read */
  if (emcmotCommand->head != emcmotCommand->tail)
    {
      emcmotStatus->split++;
      return 0;                 /* not really an error */
    }

  if (emcmotCommand->commandNum != emcmotStatus->commandNumEcho)
    {
      /* got a new command-- echo command and number... */
      emcmotStatus->commandEcho = emcmotCommand->command;
      emcmotStatus->commandNumEcho = emcmotCommand->commandNum;

      /* clear status value by default */
      emcmotStatus->commandStatus = EMCMOT_COMMAND_OK;

      /* log it, if appropriate */
      if (emcmotStatus->logStarted &&
          emcmotStatus->logType == EMCMOT_LOG_TYPE_CMD)
        {
          ls.item.cmd.time = etime();
          ls.item.cmd.command = emcmotCommand->command;
          ls.item.cmd.commandNum = emcmotCommand->commandNum;
#if defined (rtlinux) && defined (RT_FIFO)
          rtf_put(EMCMOT_LOG_RTF, &ls, sizeof(EMCMOT_LOG_STRUCT));
#else  /* not rtlinux or not RT_FIFO */
          emcmotLogAdd(emcmotLog, ls);
#endif /* else not rtlinux nor RT_FIFO */
        }

      /* ...and process command */
      switch (emcmotCommand->command)
        {
        case EMCMOT_FREE:
          /* change the mode to free axis motion */
          /* can be done at any time */
          /* reset the coordinating flag to defer transition to
             controller cycle */
          coordinating = 0;
          break;

        case EMCMOT_COORD:
          /* change the mode to coordinated axis motion */
          /* can be done at any time */
          /* set the coordinating flag to defer transition to
             controller cycle */
          coordinating = 1;
          break;

        case EMCMOT_SET_TRAJ_CYCLE_TIME:
          /* set the cycle time for trajectory calculations */
          /* really should be done only at startup before
             controller is run, but at least it requires
             no active motions and the interpolators need
             to be cleared */
          setTrajCycleTime(emcmotCommand->cycleTime);
          break;

        case EMCMOT_SET_SERVO_CYCLE_TIME:
          /* set the cycle time for servo calculations, which is the
             period for emcmotController execution */
          /* really should be done only at startup before
             controller is run, but at least it requires
             no active motions and the interpolators need
             to be cleared */
          setServoCycleTime(emcmotCommand->cycleTime);
#ifdef rtlinux
          rt_task_make_periodic(&emcmotTask,
                                rt_get_time(),
                                (RTIME) (RT_TICKS_PER_SEC *
                                         emcmotStatus->servoCycleTime));
#ifdef STEPPER_MOTORS
          /* make stepper task run twice as fast as servo (axis) rate */
          rt_task_make_periodic(&smTask,
                                rt_get_time(),
                                ((RTIME) (RT_TICKS_PER_SEC *
                                         emcmotStatus->servoCycleTime)) >> 1);
#endif /* STEPPER_MOTORS */
#endif /* rtlinux */
          break;

        case EMCMOT_SET_POSITION_LIMITS:
          /* set the position limits for the axis */
          /* can be done at any time */
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }
          emcmotStatus->minLimit[axis] = emcmotCommand->minLimit;
          emcmotStatus->maxLimit[axis] = emcmotCommand->maxLimit;
          break;

        case EMCMOT_SET_OUTPUT_LIMITS:
          /* set the output limits for the axis */
          /* can be done at any time */
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }

          emcmotStatus->minOutput[axis] = emcmotCommand->minLimit;
          emcmotStatus->maxOutput[axis] = emcmotCommand->maxLimit;
          break;

        case EMCMOT_SET_OUTPUT_SCALE:
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS ||
              emcmotCommand->scale == 0)
            {
              break;
            }
          emcmotStatus->outputScale[axis] = emcmotCommand->scale;
          emcmotStatus->outputOffset[axis] = emcmotCommand->offset;
          break;

        case EMCMOT_SET_INPUT_SCALE:
          /*
            change the scale factor for the position input, e.g.,
            encoder counts per unit. Note that this is not a good idea
            once things have gotten underway, since the axis will
            jump servo to the "new" position, the gains will no longer
            be appropriate, etc.
          */
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS ||
              emcmotCommand->scale == 0)
            {
              break;
            }

          /* adjust last saved input value to match this one, so we
             don't get a spurious following error */
          lastInput[axis] = lastInput[axis] * emcmotStatus->inputScale[axis] +
            emcmotStatus->inputOffset[axis];
          lastInput[axis] = (lastInput[axis] - emcmotCommand->offset) /
            emcmotCommand->scale;

          /* now make them active */
          emcmotStatus->inputScale[axis] = emcmotCommand->scale;
          emcmotStatus->inputOffset[axis] = emcmotCommand->offset;

          /* STEPPER MOTORS */
          smStepsPerUnit[axis] = emcmotCommand->scale;
          /* END STEPPER MOTORS */
          break;

        case EMCMOT_SET_MAX_FERROR:
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS ||
              emcmotCommand->maxFerror < 0.0)
            {
              break;
            }
          emcmotStatus->maxFerror[axis] =
            emcmotCommand->maxFerror;
          break;

        case EMCMOT_JOG_CONT:
          /* do a continuous jog, implemented as an incremental
             jog to the software limit, or the full range of travel
             if software limits don't yet apply because we're not homed */

          /* check axis range */
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }

          /* requires no motion, in free mode, enable on */
          if (GET_MOTION_COORD_FLAG() != 0 ||
              ! GET_MOTION_INPOS_FLAG() ||
              ! GET_MOTION_ENABLE_FLAG())
            {
              SET_AXIS_ERROR_FLAG(axis, 1);
              break;
            }

          /* don't jog further onto limits */
          if (! checkJog(axis, emcmotCommand->vel))
            {
              SET_AXIS_ERROR_FLAG(axis, 1);
              break;
            }

          /* start jogPos "here", and override one value with jog end point */
          jogPos = emcmotStatus->pos;

          switch (axis)
            {
            case 0:
              if (emcmotCommand->vel > 0.0)
                {
                  if (GET_AXIS_HOMED_FLAG(axis))
                    {
                      jogPos.tran.x = emcmotStatus->maxLimit[axis];
                    }
                  else
                    {
                      jogPos.tran.x = emcmotStatus->pos.tran.x + XRANGE;
                    }
                }
              else
                {
                  if (GET_AXIS_HOMED_FLAG(axis))
                    {
                      jogPos.tran.x = emcmotStatus->minLimit[axis];
                    }
                  else
                    {
                      jogPos.tran.x = emcmotStatus->pos.tran.x - XRANGE;
                    }
                }
              jogPos.tran.y = 0.0;
              jogPos.tran.z = 0.0;
              break;

            case 1:
              if (emcmotCommand->vel > 0.0)
                {
                  if (GET_AXIS_HOMED_FLAG(axis))
                    {
                      jogPos.tran.y = emcmotStatus->maxLimit[axis];
                    }
                  else
                    {
                      jogPos.tran.y = emcmotStatus->pos.tran.y + YRANGE;
                    }
                }
              else
                {
                  if (GET_AXIS_HOMED_FLAG(axis))
                    {
                      jogPos.tran.y = emcmotStatus->minLimit[axis];
                    }
                  else
                    {
                      jogPos.tran.y = emcmotStatus->pos.tran.y - YRANGE;
                    }
                }
              jogPos.tran.z = 0.0;
              jogPos.tran.x = 0.0;
              break;

            case 2:
              if (emcmotCommand->vel > 0.0)
                {
                  if (GET_AXIS_HOMED_FLAG(axis))
                    {
                      jogPos.tran.z = emcmotStatus->maxLimit[axis];
                    }
                  else
                    {
                      jogPos.tran.z = emcmotStatus->pos.tran.z + ZRANGE;
                    }
                }
              else
                {
                  if (GET_AXIS_HOMED_FLAG(axis))
                    {
                      jogPos.tran.z = emcmotStatus->minLimit[axis];
                    }
                  else
                    {
                      jogPos.tran.z = emcmotStatus->pos.tran.z - ZRANGE;
                    }
                }
              jogPos.tran.x = 0.0;
              jogPos.tran.y = 0.0;
              break;

            default:
              break;
            }

          saveVmax = freeAxis[axis].vMax;
          tpSetVmax(&freeAxis[axis], fabs(emcmotCommand->vel));
          tpAddLine(&freeAxis[axis], jogPos);
          tpSetVmax(&freeAxis[axis], saveVmax);
          SET_AXIS_ERROR_FLAG(axis, 0);

          break;

        case EMCMOT_JOG_INCR:
          /* do an incremental jog */

          /* check axis range */
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }

          /* requires no motion, in free mode, enable on */
          if (GET_MOTION_COORD_FLAG() != 0 ||
              ! GET_MOTION_INPOS_FLAG() ||
              ! GET_MOTION_ENABLE_FLAG())
            {
              SET_AXIS_ERROR_FLAG(axis, 1);
              break;
            }

          /* don't jog further onto limits */
          if (! checkJog(axis, emcmotCommand->vel))
            {
              SET_AXIS_ERROR_FLAG(axis, 1);
              break;
            }

          /* start jogPos "here", and add increment */
          jogPos = emcmotStatus->pos;

          switch (axis)
            {
            case 0:
              if (emcmotCommand->vel > 0.0)
                jogPos.tran.x += emcmotCommand->pos.tran.x;
              else
                jogPos.tran.x -= emcmotCommand->pos.tran.x;
              jogPos.tran.y = 0.0;
              jogPos.tran.z = 0.0;
              break;

            case 1:
              if (emcmotCommand->vel > 0.0)
                jogPos.tran.y += emcmotCommand->pos.tran.y;
              else
                jogPos.tran.y -= emcmotCommand->pos.tran.y;
              jogPos.tran.z = 0.0;
              jogPos.tran.x = 0.0;
              break;

            case 2:
              if (emcmotCommand->vel > 0.0)
                jogPos.tran.z += emcmotCommand->pos.tran.z;
              else
                jogPos.tran.z -= emcmotCommand->pos.tran.z;
              jogPos.tran.x = 0.0;
              jogPos.tran.y = 0.0;
              break;

            default:
              break;
            }

          saveVmax = freeAxis[axis].vMax;
          tpSetVmax(&freeAxis[axis], fabs(emcmotCommand->vel));
          tpAddLine(&freeAxis[axis], jogPos);
          tpSetVmax(&freeAxis[axis], saveVmax);
          SET_AXIS_ERROR_FLAG(axis, 0);

          break;

        case EMCMOT_JOG_ABS:
          /* do an absolute jog */

          /* check axis range */
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }

          /* requires no motion, in free mode, enable on */
          if (GET_MOTION_COORD_FLAG() != 0 ||
              ! GET_MOTION_INPOS_FLAG() ||
              ! GET_MOTION_ENABLE_FLAG())
            {
              SET_AXIS_ERROR_FLAG(axis, 1);
              break;
            }

          /* don't jog further onto limits */
          if (! checkJog(axis, emcmotCommand->vel))
            {
              SET_AXIS_ERROR_FLAG(axis, 1);
              break;
            }

          /* start jogPos "here", and set dest to arg */
          jogPos = emcmotStatus->pos;

          switch (axis)
            {
            case 0:
              jogPos.tran.x = emcmotCommand->pos.tran.x;
              jogPos.tran.y = 0.0;
              jogPos.tran.z = 0.0;
              break;

            case 1:
              jogPos.tran.y = emcmotCommand->pos.tran.y;
              jogPos.tran.z = 0.0;
              jogPos.tran.x = 0.0;
              break;

            case 2:
              jogPos.tran.z = emcmotCommand->pos.tran.z;
              jogPos.tran.x = 0.0;
              jogPos.tran.y = 0.0;
              break;

            default:
              break;
            }

          saveVmax = freeAxis[axis].vMax;
          tpSetVmax(&freeAxis[axis], fabs(emcmotCommand->vel));
          tpAddLine(&freeAxis[axis], jogPos);
          tpSetVmax(&freeAxis[axis], saveVmax);
          SET_AXIS_ERROR_FLAG(axis, 0);

          break;

        case EMCMOT_SET_TERM_COND:
          /* sets termination condition for motion queue */
/*        tpSetTermCond(&queue, emcmotCommand->termCond); */
          break;

        case EMCMOT_SET_LINE:
          /* queue up a linear move */
          /* requires coordinated mode, enable off, not on limits */
          if (! GET_MOTION_COORD_FLAG() ||
              ! GET_MOTION_ENABLE_FLAG())
            {
              reportError("Need to be enabled, in coord mode for linear move");
              emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
              SET_MOTION_ERROR_FLAG(1);
              break;
            }
          else if (! inRange(emcmotCommand->pos))
            {
              reportError("Linear move %d out of range",
                          emcmotCommand->id);
              emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
              sqAbort(&queue);

              SET_MOTION_ERROR_FLAG(1);
              break;
            }
          else if (! checkLimits())
            {
              reportError("Can't do linear move with limits exceeded");
              emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
              sqAbort(&queue);
              SET_MOTION_ERROR_FLAG(1);
              break;
            }

          /* append it to the queue */
          if ( -1 == sqAddLine( &queue, emcmotCommand->pos, \
                                emcmotCommand->id ) )
            {
              reportError("Can't add linear move");
              emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
              sqAbort(&queue);
              SET_MOTION_ERROR_FLAG(1);
              break;
            }
          else
            {
              SET_MOTION_ERROR_FLAG(0);
            }
          break;

        case EMCMOT_SET_CIRCLE:
          /* queue up a circular move */
          /* requires coordinated mode, enable on, not on limits */
          if (! GET_MOTION_COORD_FLAG() ||
              ! GET_MOTION_ENABLE_FLAG())
            {
              reportError("Need to be enabled, in coord mode for circular move");
              emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
              SET_MOTION_ERROR_FLAG(1);
              break;
            }
          else if (! inRange(emcmotCommand->pos))
            {
              reportError("Circular move %d out of range",
                          emcmotCommand->id);
              emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
              sqAbort(&queue);

              SET_MOTION_ERROR_FLAG(1);
              break;
            }
          else if (! checkLimits())
            {
              reportError("Can't do circular move with limits exceeded");
              emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
              sqAbort(&queue);

              SET_MOTION_ERROR_FLAG(1);
              break;
            }

          /* append it to the queue */
          if ( -1 == sqAddCircle( &queue, emcmotCommand->pos,
                        emcmotCommand->center, emcmotCommand->normal,
                        emcmotCommand->turn, emcmotCommand->id ) )
            {
              reportError("Can't add circular move");
              emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
              sqAbort(&queue);

              SET_MOTION_ERROR_FLAG(1);
              break;
            }
          else
            {
              SET_MOTION_ERROR_FLAG(0);
            }
          break;

        case EMCMOT_SET_VEL:
          /* set the max velocity */
          /* can do it at any time */
          emcmotStatus->vel = emcmotCommand->vel;
          for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
            {
              tpSetVmax(&freeAxis[axis], emcmotStatus->vel);
            }
          sqSetFeed(&queue, emcmotStatus->vel);
          break;

          /* set the "big" vel used for debouncing input, to be the
             10X the max seen so far */
          if (bigVel < 10.0 * emcmotCommand->vel)
            {
              bigVel = 10.0 * emcmotCommand->vel;
            }
          break;

        case EMCMOT_SET_VEL_LIMIT:
          /* set the absolute max velocity for all subsequent moves */
          /* can do it at any time */
          emcmotStatus->limitVel = emcmotCommand->vel;
          for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
            {
              tpSetVlimit(&freeAxis[axis], emcmotStatus->limitVel);
            }
          sqSetVmax(&queue, emcmotStatus->limitVel);
          /* now set the debounce vel */
          bigVel = 10.0 * emcmotStatus->limitVel;
          break;

        case EMCMOT_SET_HOMING_VEL:
          /* set the homing velocity */
          /* can do it at any time */
          /* sign of vel should set polarity, and mag-sign are recorded */

          /* check axis range */
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }

          if (emcmotCommand->vel < 0.0)
            {
              emcmotStatus->homingVel[axis] = - emcmotCommand->vel;
              SET_AXIS_HOMING_POLARITY(axis, 0);
            }
          else
            {
              emcmotStatus->homingVel[axis] = emcmotCommand->vel;
              SET_AXIS_HOMING_POLARITY(axis, 1);
            }
          break;

        case EMCMOT_SET_ACC:
          /* set the max acceleration */
          /* can do it at any time */
          emcmotStatus->acc = emcmotCommand->acc;
          for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
            {
              tpSetAmax(&freeAxis[axis], emcmotStatus->acc);
            }
          sqSetMaxAcc(&queue, emcmotStatus->acc);
          break;

        case EMCMOT_PAUSE:
          /* pause the motion */
          /* can happen at any time */

          if ( 0 == coordinating )
            {
              for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
                {
                  tpPause(&freeAxis[axis]);
                }
              emcmotStatus->paused = 1;
            }
          else
            {
              if (-1 ==sqPause(&queue))
                {
                  reportError("Can't pause");
                  sqAbort(&queue);
                  break;
                }
              emcmotStatus->paused= sqIsPaused(&queue);
            }
          break;

        case EMCMOT_RESUME:
          /* resume paused motion */
          /* can happen at any time */
          /* not true for segment queue!! */
          if ( 0 == coordinating )
            {
              for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
                {
                  tpResume(&freeAxis[axis]);
                }
              emcmotStatus->paused = 0;
              stepping = 0;
            }
          else
            {
              if (-1 == sqResume(&queue))
                {
                  reportError("Can't resume");
                  sqAbort(&queue);
                  break;
                }

              emcmotStatus->paused = sqIsPaused(&queue);
              stepping = sqIsStepping(&queue);
            }

          break;

        case EMCMOT_STEP:
          /* resume paused motion until id changes */
          /* can happen at any time */
          if ( 0 == coordinating )
            {
              idForStep = emcmotStatus->id;
              stepping = 1;
              for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
                {
                  tpResume(&freeAxis[axis]);
                }
              emcmotStatus->paused = 0;
            }
          else
            {
              if (-1 == sqStep(&queue) )
                {
                  reportError("Can't step");
                  sqAbort(&queue);
                  break;
                }
            }

          break;

        case EMCMOT_SCALE:
          /* override speed */
          /* can happen at any time */
          if (emcmotCommand->scale < 0.0)
            {
              emcmotCommand->scale = 0.0; /* clamp it */
            }
          for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
            {
              tpSetVscale(&freeAxis[axis], emcmotCommand->scale);
            }
          if (-1 == sqSetFeedOverride(&queue, emcmotCommand->scale))
            {
              reportError("Can't set new feed");
              sqAbort(&queue);
              break;
            }


          break;

        case EMCMOT_ABORT:
          /* abort motion */
          /* can happen at any time */
          /* check for coord or free space motion active */
          if (GET_MOTION_COORD_FLAG())
            {
              sqAbort(&queue);
              SET_MOTION_ERROR_FLAG(0);
            }
          else
            {
              /* check axis range */
              axis = emcmotCommand->axis;
              if (axis < 0 ||
                  axis >= EMCMOT_MAX_AXIS)
                {
                  break;
                }
              tpAbort(&freeAxis[axis]);
              SET_AXIS_HOMING_FLAG(axis, 0);
              SET_AXIS_ERROR_FLAG(axis, 0);
            }
          break;

        case EMCMOT_DISABLE:
          /* go into disable */
          /* can happen at any time */
          /* reset the enabling flag to defer disable until
             controller cycle (it *will* be honored) */
          enabling = 0;
          break;

        case EMCMOT_ENABLE:
          /* come out of disable */
          /* can happen at any time */
          /* set the enabling flag to defer enable
             until controller cycle */
          enabling = 1;
          break;

        case EMCMOT_SET_PID:
          /* configure the PID gains */
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }
          pidSetGains(&emcmotStatus->pid[axis],
                      emcmotCommand->pid);
          break;

        case EMCMOT_ACTIVATE_AXIS:
          /* make axis active, so that amps will be enabled when
             system is enabled or disabled */
          /* can be done at any time */
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }
          SET_AXIS_ACTIVE_FLAG(axis, 1);
          break;

        case EMCMOT_DEACTIVATE_AXIS:
          /* make axis inactive, so that amps won't be affected when
             system is enabled or disabled */
          /* can be done at any time */
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }
          SET_AXIS_ACTIVE_FLAG(axis, 0);
          break;

        case EMCMOT_ENABLE_AMPLIFIER:
          /* enable the amplifier directly, but don't enable calculations */
          /* can be done at any time */
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }
          extAmpEnable(axis, GET_AXIS_ENABLE_POLARITY(axis));
          break;

        case EMCMOT_DISABLE_AMPLIFIER:
          /* disable the axis calculations and amplifier, but don't
             disable calculations */
          /* can be done at any time */
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }
          extAmpEnable(axis, ! GET_AXIS_ENABLE_POLARITY(axis));
          break;

        case EMCMOT_OPEN_LOG:
          /* open a data log */
          axis = emcmotCommand->axis;
          valid = 0;
          if (emcmotCommand->logSize > 0 &&
              emcmotCommand->logSize <= EMCMOT_LOG_MAX)
            {
              /* handle log-specific data */
              switch (emcmotCommand->logType)
                {
                case EMCMOT_LOG_TYPE_AXIS_POS:
                  if (axis >= 0 &&
                      axis < EMCMOT_MAX_AXIS)
                    {
                      loggingAxis = axis;
                      valid = 1;
                    }
                  break;

                default:
                  valid = 1;
                  break;
                }
            }

          if (valid)
            {
              /* success */
#if defined (rtlinux) && defined (RT_FIFO)
              if (-1 == rtf_create(EMCMOT_LOG_RTF, emcmotCommand->logSize * sizeof(EMCMOT_LOG_STRUCT)))
                {
                  /* error-- can't open log */
                  break;
                }
#else  /* not rtlinux or not RT_FIFO */
              emcmotLogInit(emcmotLog,
                            emcmotCommand->logType,
                            emcmotCommand->logSize);
#endif /* else not rtlinux or not RT_FIFO */
              emcmotStatus->logOpen = 1;
              emcmotStatus->logStarted = 0;
              emcmotStatus->logSize = emcmotCommand->logSize;
              emcmotStatus->logSkip = emcmotCommand->logSkip;
              emcmotStatus->logType = emcmotCommand->logType;
            }
          break;

        case EMCMOT_START_LOG:
          /* start logging */
          if (emcmotStatus->logOpen)
            {
              emcmotStatus->logStarted = 1;
              logSkip = 0;
            }
          break;

        case EMCMOT_STOP_LOG:
          /* stop logging */
          emcmotStatus->logStarted = 0;
          break;

        case EMCMOT_CLOSE_LOG:
#if defined (rtlinux) && defined (RT_FIFO)
          rtf_destroy(EMCMOT_LOG_RTF);
#endif /* rtlinux and RT_FIFO */
          emcmotStatus->logOpen = 0;
          emcmotStatus->logStarted = 0;
          emcmotStatus->logSize = 0;
          emcmotStatus->logSkip = 0;
          emcmotStatus->logType = 0;
          break;

        case EMCMOT_DAC_OUT:
          /* write output to dacs directly */
          /* will only persist if amplifiers are disabled */
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }
          emcmotStatus->output[axis] = emcmotCommand->dacOut;
          break;

        case EMCMOT_HOME:
          /* home the specified axis */
          /* need to be in free mode, enable on */
          /* homing is basically a slow incremental jog to full range */
          axis = emcmotCommand->axis;
          if (GET_MOTION_COORD_FLAG() != 0 ||
              ! GET_MOTION_ENABLE_FLAG())
            {
              break;
            }
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }

          jogPos = emcmotStatus->pos;

          switch (axis)
            {
            case 0:
              if (GET_AXIS_HOMING_POLARITY(axis))
                {
                  jogPos.tran.x += 2.0 * XRANGE;
                }
              else
                {
                  jogPos.tran.x -= 2.0 * XRANGE;
                }
              jogPos.tran.y = 0.0;
              jogPos.tran.z = 0.0;
              break;

            case 1:
              if (GET_AXIS_HOMING_POLARITY(axis))
                {
                  jogPos.tran.y += 2.0 * YRANGE;
                }
              else
                {
                  jogPos.tran.y -= 2.0 * YRANGE;
                }
              jogPos.tran.z = 0.0;
              jogPos.tran.x = 0.0;
              break;

            case 2:
              if (GET_AXIS_HOMING_POLARITY(axis))
                {
                  jogPos.tran.z += 2.0 * ZRANGE;
                }
              else
                {
                  jogPos.tran.z -= 2.0 * ZRANGE;
                }
              jogPos.tran.x = 0.0;
              jogPos.tran.y = 0.0;
              break;

            default:
              break;
            }

          saveVmax = freeAxis[axis].vMax;
          tpSetVmax(&freeAxis[axis], emcmotStatus->homingVel[axis]);
          tpAddLine(&freeAxis[axis], jogPos);
          tpSetVmax(&freeAxis[axis], saveVmax);
          waitingForLatch[axis] = 1;
          SET_AXIS_HOMING_FLAG(axis, 1);
          SET_AXIS_HOMED_FLAG(axis, 0);
          break;

        case EMCMOT_ENABLE_WATCHDOG:
          wdEnable = 1;
          if (emcmotCommand->wdWait < 0)
            {
              wdWait = 0;
            }
          else
            {
              wdWait = emcmotCommand->wdWait;
            }
          wdCount = wdWait;
          break;

        case EMCMOT_DISABLE_WATCHDOG:
          wdEnable = 0;
          break;

        case EMCMOT_SET_POLARITY:
          axis = emcmotCommand->axis;
          if (axis < 0 ||
              axis >= EMCMOT_MAX_AXIS)
            {
              break;
            }
          if (emcmotCommand->level)
            {
              /* normal */
              emcmotStatus->axisPolarity[axis] |= emcmotCommand->axisFlag;
            }
          else
            {
              /* inverted */
              emcmotStatus->axisPolarity[axis] &= ~emcmotCommand->axisFlag;
            }
          break;

        default:
          reportError("Unrecognized command %d", emcmotCommand->command);
          emcmotStatus->commandStatus = EMCMOT_COMMAND_UNKNOWN_COMMAND;
          break;
        } /* end of command switch */
    } /* end of if-new-command */

#if defined (rtlinux) && defined (RT_FIFO)
    } /* end of while-msg loop for RT FIFO */

  if (err != 0)
    {
      return -1;
    }

#endif /* rtlinux and RT_FIFO */

  return 0;
}

#ifdef STEPPER_MOTORS

/*
  fpInUse is set by an RT task to signify that it is using floating point
  calculations. This is to protect against the failure of RT-Linux to
  save/restore FP registers between RT tasks.

  It's set by smController to bracket FP calls. It's read by emcmotController
  and if it's set the cycle is deferred via calling rt_task_wait()
  immediately.
*/
static unsigned char fpInUse = 0;

#endif

/*
  emcmotController() runs the trajectory and interpolation calculations
  each control cycle

  Inactive axes are still calculated, but the PIDs are inhibited and
  the amp enable/disable are inhibited
  */
static void emcmotController(int arg)
{
  double start, end, delta;     /* time stamping */
  int homeFlag;                 /* result of ext read to home switch */
  int axis;                     /* axis loop counter */
  int isLimit;                  /* result of ext read to limit sw */
  int whichCycle;               /* 0=none, 1=servo, 2=traj */
  int fault;
  double thisFerror[EMCMOT_MAX_AXIS];

#ifdef rtlinux
  for (;;)
    {
#endif /* rtlinux */

#ifdef STEPPER_MOTORS
      if (fpInUse)
        {
          rt_task_wait();
          continue;
        }
#endif /* rtlinux */

#ifdef rtlinux
      /* toggle sound, parallel port watchdogs */
      if (wdEnable &&
          wdCount-- <= 0)
        {
          soundByte = inb(SOUND_PORT);
          soundByte &= SOUND_MASK;
          toggle = !toggle;
          if (toggle)
            soundByte |= ~SOUND_MASK;
          outb(soundByte, SOUND_PORT);
          wdCount = wdWait;
        }
#endif /* rtlinux */

      /* increment head count */
      emcmotStatus->head++;

      /* record start time */
      start = etime();

      /* READ COMMAND: */

#ifndef RT_FIFO
      emcmotCommandHandler();

#endif /* not RT_FIFO */

      /* SAVE LAST CYCLE'S VALUES */
      for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
        {
          oldInput[axis] = emcmotStatus->input[axis];
          oldJointPos[axis] = jointPos[axis];
        }

      /* READ INPUTS: */

      /* latch all encoder feedback into raw input array */
#ifdef STEPPER_MOTORS
      for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
        {
          rawInput[axis] = (double) smStepsAccum[axis];
        }
#else
      extEncoderReadAll(EMCMOT_MAX_AXIS, rawInput);
#endif

      /* process input read limit switches */
      for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
        {
          /* FIXME-- testing */

          /* scale according to scaled = scale (raw - offset) */
          emcmotStatus->input[axis] =
            (rawInput[axis] - emcmotStatus->inputOffset[axis]) /
            emcmotStatus->inputScale[axis];

#ifndef STEPPER_MOTORS
          /* debounce bad feedback */
          if (fabs(emcmotStatus->input[axis] - lastInput[axis]) /
              emcmotStatus->servoCycleTime > bigVel) {
            /* bad input value-- debounce with the last value */
            emcmotStatus->input[axis] = lastInput[axis];
          }
#endif
          lastInput[axis] = emcmotStatus->input[axis];

          /* reset limit flags initially */
          SET_AXIS_PHL_FLAG(axis, 0);
          SET_AXIS_NHL_FLAG(axis, 0);

          /* read switches from external interface */
          extMaxLimitSwitchRead(axis, &isLimit);

          /* set flags if on max limit */
          if (isLimit == GET_AXIS_PHL_POLARITY(axis))
            {
              if (++maxLimitSwitchCount[axis] > LIMIT_SWITCH_DEBOUNCE)
                {
                  /* set max limit bit */
                  SET_AXIS_PHL_FLAG(axis, 1);
                  maxLimitSwitchCount[axis] = LIMIT_SWITCH_DEBOUNCE;
                }
            }
          else
            {
              maxLimitSwitchCount[axis] = 0;
            }

          /* read switches from external interface */
          extMinLimitSwitchRead(axis, &isLimit);

          /* set flags if on min limit */
          if (isLimit == GET_AXIS_NHL_POLARITY(axis))
            {
              if (++minLimitSwitchCount[axis] > LIMIT_SWITCH_DEBOUNCE)
                {
                  /* set min limit bit */
                  SET_AXIS_NHL_FLAG(axis, 1);
                  minLimitSwitchCount[axis] = LIMIT_SWITCH_DEBOUNCE;
                }
            }
          else
            {
              minLimitSwitchCount[axis] = 0;
            }

          /* read home switch */
          extHomeSwitchRead(axis, &homeFlag);
          SET_AXIS_HOME_SWITCH_FLAG(axis, homeFlag == GET_AXIS_HOME_SWITCH_POLARITY(axis));

          /* read amp fault */
          extAmpFault(axis, &fault);
          SET_AXIS_FAULT_FLAG(axis, fault == GET_AXIS_FAULT_POLARITY(axis));
          /* FIXME-- do something with amp fault, for active axes */
        }

      /* RUN STATE LOGIC: */

      /* check for disabling */
      if (! enabling &&
          GET_MOTION_ENABLE_FLAG())
        {
          /* clear out the motion queue and interpolators */
          sqClearQueue(&queue);
          for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
            {
              tpClear(&freeAxis[axis]);
              cubicDrain(&cubic[axis]);
              if (GET_AXIS_ACTIVE_FLAG(axis))
                {
                  extAmpEnable(axis,
                               ! GET_AXIS_ENABLE_POLARITY(axis));
                  SET_AXIS_ENABLE_FLAG(axis, 0);
                  SET_AXIS_HOMING_FLAG(axis, 0);
                  emcmotStatus->output[axis] = 0.0;
                }
            }

          /* reset state flags */
          SET_MOTION_ENABLE_FLAG(0);
        }

      /* check for enabling */
      if (enabling &&
          ! GET_MOTION_ENABLE_FLAG())
        {
          sqSetPos(&queue, emcmotStatus->pos);
          for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
            {
              tpSetPos(&freeAxis[axis], emcmotStatus->pos);
              pidReset(&emcmotStatus->pid[axis]);
              if (GET_AXIS_ACTIVE_FLAG(axis))
                {
                  extAmpEnable(axis,
                               GET_AXIS_ENABLE_POLARITY(axis));
                  SET_AXIS_ENABLE_FLAG(axis, 1);
                }
              SET_AXIS_ERROR_FLAG(axis, 0);
            }

          /* reset state flags */
          SET_MOTION_ENABLE_FLAG(1);
          SET_MOTION_ERROR_FLAG(0);
        }

      /* check for entering coordinated mode */
      if (coordinating &&
          ! GET_MOTION_COORD_FLAG())
        {
          if (GET_MOTION_INPOS_FLAG())
            {
              /* update coordinated queue position */
          sqSetPos(&queue, emcmotStatus->pos);

              SET_MOTION_COORD_FLAG(1);
              SET_MOTION_ERROR_FLAG(0);
            }
          else
            {
              /* not in position-- don't honor mode change */
              coordinating = 0;
            }
        }

      /* check entering free space mode */
      if (! coordinating &&
          GET_MOTION_COORD_FLAG())
        {
          if (GET_MOTION_INPOS_FLAG())
            {
              /* update free planner positions */
              for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
                {
                  tpSetPos(&freeAxis[axis], emcmotStatus->pos);
                }
              SET_MOTION_COORD_FLAG(0);
            }
          else
            {
              /* not in position-- don't honor mode change */
              coordinating = 1;
            }
        }

      /* check for homing sequences */
      for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
        {
          if (GET_AXIS_HOMING_FLAG(axis))
            {
              if (tpIsDone(&freeAxis[axis]))
                {
                  /* set input offset to saved latch */
                  emcmotStatus->inputOffset[axis] = saveLatch[axis];

                  /* recompute inputs to match so we don't have a
                     momentary jump */
                  emcmotStatus->input[axis] =
                    (rawInput[axis] - emcmotStatus->inputOffset[axis]) /
                    emcmotStatus->inputScale[axis];
                  lastInput[axis] = emcmotStatus->input[axis];

                  /* set axis position to 0 upon homing */
                  if (axis == 0)
                    emcmotStatus->pos.tran.x = 0;
                  else if (axis == 1)
                    emcmotStatus->pos.tran.y = 0;
                  else if (axis == 2)
                    emcmotStatus->pos.tran.z = 0;

                  /* clear out the interpolator */
                  tpSetPos(&freeAxis[axis], emcmotStatus->pos);
                  cubicDrain(&cubic[0]);
                  cubicDrain(&cubic[1]);
                  cubicDrain(&cubic[2]);
                  SET_AXIS_HOMING_FLAG(axis, 0);
                  SET_AXIS_HOMED_FLAG(axis, 1);
                }
            }
        }

      /* RUN MOTION CALCULATIONS: */

      /* run axis interpolations and outputs, but only if we're
         enabled. This section is "suppressed" if we're not
         enabled, although the read/write of encoders/dacs is still
         done. */
      whichCycle = 0;
      if (GET_MOTION_ENABLE_FLAG())
        {
          /* set whichCycle to be at least a servo cycle */
          whichCycle = 1;
          /* pull next point(s) from traj */
          while (cubicNeedNextPoint(&cubic[0]))
            {
              /* set whichCycle to be a trajectory cycle */
              whichCycle = 2;

              /* save old values for vel, accel differences */
              oldPos = emcmotStatus->pos;

              /* get next point from free or coord planners */
              if (GET_MOTION_COORD_FLAG() == 1)
                {
                  /* run coordinated trajectory planning cycle */
                  if (-1 ==sqRunCycle(&queue))
                  {
                    reportError("Runcycle error");
                    sqAbort(&queue);
                  }
                  sqGetPosition(&queue,&emcmotStatus->pos);
                }
              else
                {
                  /* run free trajectory planning cycle */
                  tpRunCycle(&freeAxis[0]);
                  tpRunCycle(&freeAxis[1]);
                  tpRunCycle(&freeAxis[2]);
                  emcmotStatus->pos.tran.x = tpGetPos(&freeAxis[0]).tran.x;
                  emcmotStatus->pos.tran.y = tpGetPos(&freeAxis[1]).tran.y;
                  emcmotStatus->pos.tran.z = tpGetPos(&freeAxis[2]).tran.z;
                }

              /* calculate current vel, accel */
              /* FIXME-- need to handle rotational vel, accel */

              newVel.tran.x = emcmotStatus->pos.tran.x - oldPos.tran.x;
              newVel.tran.y = emcmotStatus->pos.tran.y - oldPos.tran.y;
              newVel.tran.z = emcmotStatus->pos.tran.z - oldPos.tran.z;
              oldPos = emcmotStatus->pos;

              newAcc.tran.x = newVel.tran.x - oldVel.tran.x;
              newAcc.tran.y = newVel.tran.y - oldVel.tran.y;
              newAcc.tran.z = newVel.tran.z - oldVel.tran.z;
              oldVel = newVel;

              /* log per-traj-cycle data if logging active */
              logIt = 0;
              if (emcmotStatus->logStarted &&
                  emcmotStatus->logSkip >= 0) {

                /* save the type with the log item */
                ls.type = emcmotStatus->logType;

                /* now log type-specific data */
                switch (emcmotStatus->logType) {

                case EMCMOT_LOG_TYPE_TRAJ_POS:
                  if (logSkip-- <= 0) {
                    ls.item.trajPos.time = start;
                    ls.item.trajPos.pos = emcmotStatus->pos.tran;
                    logSkip = emcmotStatus->logSkip;
                    logIt = 1;
                  }
                  break;

                case EMCMOT_LOG_TYPE_TRAJ_VEL:
                  if (logSkip-- <= 0) {
                    ls.item.trajVel.time = start;
                    ls.item.trajVel.vel = newVel.tran;
/*                  pmCartMag(newVel.tran, &ls.item.trajVel.mag);  */
                    ls.item.trajVel.mag = sqGetVel(&queue);
                    logSkip = emcmotStatus->logSkip;
                    logIt = 1;
                  }
                  break;

                case EMCMOT_LOG_TYPE_TRAJ_ACC:
                  if (logSkip-- <= 0) {
                    ls.item.trajAcc.time = start;
                    ls.item.trajAcc.acc = newAcc.tran;
                    pmCartMag(newAcc.tran, &ls.item.trajAcc.mag);
                    logSkip = emcmotStatus->logSkip;
                    logIt = 1;
                  }
                  break;

                default:
                  break;
                } /* end of switch on log type */

                /* now log it */
#if defined (rtlinux) && defined (RT_FIFO)
                if (logIt) {
                  rtf_put(EMCMOT_LOG_RTF, &ls, sizeof(EMCMOT_LOG_STRUCT));
                  /* FIXME-- update emcmotStatus->logPoints */
                  logIt = 0;
                }
#else
                if (logIt) {
                  emcmotLogAdd(emcmotLog, ls);
                  emcmotStatus->logPoints = emcmotLog->howmany;
                  logIt = 0;
                }
#endif
              } /* end of logging per-traj-cycle data */

              /* convert to joints and spline it up */
              inverseKinematics(emcmotStatus->pos, trajPos);
              for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
                {
                  cubicAddPoint(&cubic[axis], trajPos[axis]);
                }

              /* check for limits-- only do this on a trajectory cycle */
              for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
                {
                  /* reset soft limit flags initially */
                  SET_AXIS_NSL_FLAG(axis, 0);
                  SET_AXIS_PSL_FLAG(axis, 0);

                  /* check for soft or hard limits-- hard limits always,
                     soft limits only if we've been homed. If we're not
                     homed we ignore software limits since the position
                     values are meaningless */
                  if (GET_AXIS_NHL_FLAG(axis) ||
                      GET_AXIS_PHL_FLAG(axis) ||
                      (GET_AXIS_HOMED_FLAG(axis) &&
                       (trajPos[axis] > emcmotStatus->maxLimit[axis] ||
                        trajPos[axis] < emcmotStatus->minLimit[axis])) )
                    {
                      /* set axis error flag in any case */
                      SET_AXIS_ERROR_FLAG(axis, 1);

                      /* set appropriate flag */
                      if (GET_AXIS_HOMED_FLAG(axis) &&
                          trajPos[axis] < emcmotStatus->minLimit[axis])
                        {
                          SET_AXIS_NSL_FLAG(axis, 1);
                        }
                      else if (GET_AXIS_HOMED_FLAG(axis) &&
                               trajPos[axis] > emcmotStatus->maxLimit[axis])
                        {
                          SET_AXIS_PSL_FLAG(axis, 1);
                        }

                      /* signal for abort */
                      if (! wasOnLimit[axis])
                        {
                          if (GET_MOTION_COORD_FLAG() == 1)
                            {
                              sqAbort(&queue);
                            }
                          else if (GET_MOTION_COORD_FLAG() == 0)
                            {
                              tpAbort(&freeAxis[axis]);
                            }
                          wasOnLimit[axis] = 1;
                        }
                    }
                  else
                    {
                      wasOnLimit[axis] = 0;
                    }
                } /* for (axis = 0, ...) */
            } /* while (cubicNeedNextPoint()) */

          /* run interpolation and compensation */
          for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
            {
              /* interpolate */
              jointPos[axis] = cubicInterpolate(&cubic[axis], 0, 0, 0, 0);

              /* run output calculations */
              if (GET_AXIS_ACTIVE_FLAG(axis))
                {
#ifndef STEPPER_MOTORS
                  /* COMPENSATE: */

                  /*
                    compensation means compute output
                    'emcmotStatus->output[]' based on desired position
                    'jointPos[]' and input 'emcmotStatus->input[]'.

                    Currently the source calls for PID compensation.
                    FIXME-- add wrapper for compensator, with ptr to
                    emcmotStatus struct, with semantics that ->output[]
                    needs to be filled.
                   */

                  /* here is PID compensation */
                  emcmotStatus->output[axis] =
                    pidRunCycle(&emcmotStatus->pid[axis],
                                emcmotStatus->input[axis],
                                jointPos[axis]);
#endif

                  /* COMPUTE FOLLOWING ERROR: */

                  /*
                    compute (signed) following error = commanded - actual,
                    abort if necessary, and set or clear following error
                    axis flag
                  */
                  thisFerror[axis] = jointPos[axis] - emcmotStatus->input[axis];
                  /* record the max ferror for this axis */
                  if (emcmotStatus->ferror[axis] < fabs(thisFerror[axis]))
                    {
                      emcmotStatus->ferror[axis] = fabs(thisFerror[axis]);
                    }

                  if (thisFerror[axis] > emcmotStatus->maxFerror[axis])
                    {
                      /* abort! abort! following error exceeded */
                      SET_AXIS_ERROR_FLAG(axis, 1);
                      SET_AXIS_FERROR_FLAG(axis, 1);
                      if (enabling)
                        {
                          // report the following error just this once
                          reportError("Axis %d following error", axis);
                        }
                      enabling = 0;
                    }
                  else
                    {
                      SET_AXIS_FERROR_FLAG(axis, 0);
                    }
                }
              else
                {
                  /* axis is not active-- leave the pid output where it is--
                     if axis is not active one can still write to the dac */
                }

#ifndef STEPPER_MOTORS
              /* CLAMP OUTPUT: */

              /*
                clamp output means take 'emcmotStatus->output[]' and
                limit to range
                'emcmotStatus->minOutput[] .. emcmotStatus->maxOutput[]'
              */
              if (emcmotStatus->output[axis] < emcmotStatus->minOutput[axis])
                {
                  emcmotStatus->output[axis] = emcmotStatus->minOutput[axis];
                }
              else if (emcmotStatus->output[axis] > emcmotStatus->maxOutput[axis])
                {
                  emcmotStatus->output[axis] = emcmotStatus->maxOutput[axis];
                }
#endif

              /* CHECK FOR LATCH CONDITION: */

              /*
                check for latch condition means if we're waiting for a
                latched index pulse, and we see the pulse and the home
                switch, we read the raw input and abort. The offset is set
                above in the homing section by noting that if we're homing,
                and waitingForLatch[] is cleared, we latched.

                This presumes an encoder index pulse.
                FIXME-- remove explicit calls to encoder index pulse, to
                allow for open-loop control latching via switches only.
              */
              if (waitingForLatch[axis])
                {
                  if (GET_AXIS_HOME_SWITCH_FLAG(axis))
                    {
                      /* read encoder index */
#ifdef STEPPER_MOTORS
                      /* force an "encoder latch" */
                      latchFlag[axis] = 1;
#else
                      extEncoderResetIndex(axis);
                      extEncoderReadLatch(axis, &latchFlag[axis]);
#endif
                      if (latchFlag[axis])
                        {
                          /* get latched position in RAW UNITS */
                          saveLatch[axis] = rawInput[axis];
                          waitingForLatch[axis] = 0;
                          /* call for an abort-- when it's finished, code
                             above sets inputOffset[] to saveLatch[] */
                          tpAbort(&freeAxis[axis]);
                        }
                    }
                }

            } /* end of axis for-loop */

        } /* end of if-enabled section */

      else                      /* not enabled */
        {
          /* set jointPos scaled input */
          for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
            {
              jointPos[axis] = emcmotStatus->input[axis];
            }

          /* set actual pos to the Cartesian forward of the joint pos */
          forwardKinematics(jointPos, &emcmotStatus->pos);

        } /* end of if-not-enabled section */

#ifndef STEPPER_MOTORS
      /* SCALE OUTPUTS: */

      for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
        {
          rawOutput[axis] =
            (emcmotStatus->output[axis] - emcmotStatus->outputOffset[axis]) /
            emcmotStatus->outputScale[axis];
        }

      /* WRITE OUTPUTS: */

      /* write DACs-- note that this is done even when not
         enabled, although in this case the pidOutputs are
         all zero unless manually overridden. They will not
         be set by any calculations if we're not enabled. */
      extDacWriteAll(EMCMOT_MAX_AXIS, rawOutput);
#endif

      /* UPDATE THE REST OF THE DYNAMIC STATUS: */

      /* run actual input position at servo rate through forward kins
         to get actual Cartesian pos */
      forwardKinematics(emcmotStatus->input, &emcmotStatus->actualPos);

      /* health heartbeat */
      emcmotStatus->heartbeat++;

      /* motion queue status */
      emcmotStatus->depth = sqGetDepth(&queue);
      emcmotStatus->activeDepth = sqGetDepth(&queue);
      emcmotStatus->qVscale = queue.feedOverrideFactor;
      emcmotStatus->id= sqGetID(&queue);
      emcmotStatus->queueFull = sqIsQueueFull(&queue);



      SET_MOTION_INPOS_FLAG(0);
      if (sqIsDone(&queue))
        {
          SET_MOTION_INPOS_FLAG(1);
        }

      /* axis status */
      for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
        {
          emcmotStatus->axVscale[axis] = freeAxis[axis].vScale;

          SET_AXIS_INPOS_FLAG(axis, 0);
          if (tpIsDone(&freeAxis[axis]))
            {
              SET_AXIS_INPOS_FLAG(axis, 1);
            }
        }

      /* check to see if we should pause in order to implement
         single stepping. This is only necessary to check
         when in free mode */
      if (!coordinating && stepping && idForStep != emcmotStatus->id)
        {
          for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
            {
              tpPause(&freeAxis[axis]);
            }
/*        tpPause(&queue); */
          stepping = 0;
          emcmotStatus->paused = 1;
        }

      /* calculate elapsed time and min/max/avg */
      end = etime();
      delta = end - start;
      emcmotStatus->computeTime = delta;
      if (2 == whichCycle)
        {
          /* traj calcs done this cycle-- use tMin,Max,Avg */
          mmxavgAdd(&tMmxavg, delta);
          emcmotStatus->tMin = mmxavgMin(&tMmxavg);
          emcmotStatus->tMax = mmxavgMax(&tMmxavg);
          emcmotStatus->tAvg = mmxavgAvg(&tMmxavg);
        }
      else if (1 == whichCycle)
        {
          /* servo calcs only this cycle-- use sMin,Max,Avg */
          mmxavgAdd(&sMmxavg, delta);
          emcmotStatus->sMin = mmxavgMin(&sMmxavg);
          emcmotStatus->sMax = mmxavgMax(&sMmxavg);
          emcmotStatus->sAvg = mmxavgAvg(&sMmxavg);
        }
      else
        {
          /* calcs disabled this cycle-- use nMin,Max,Avg */
          mmxavgAdd(&nMmxavg, delta);
          emcmotStatus->nMin = mmxavgMin(&nMmxavg);
          emcmotStatus->nMax = mmxavgMax(&nMmxavg);
          emcmotStatus->nAvg = mmxavgAvg(&nMmxavg);
        }

      /* log per-servo-cycle data if logging active */
      logIt = 0;
      if (emcmotStatus->logStarted &&
          emcmotStatus->logSkip >= 0) {

        /* record type here, since all will set this */
        ls.type = emcmotStatus->logType;

        /* now log type-specific data */
        switch (ls.type) {

        case EMCMOT_LOG_TYPE_AXIS_POS:
          if (logSkip-- <= 0) {
            ls.item.axisPos.time = end;
            ls.item.axisPos.input = emcmotStatus->input[loggingAxis];
            ls.item.axisPos.output = jointPos[loggingAxis];
            logSkip = emcmotStatus->logSkip;
            logIt = 1;
          }
          break;

        case EMCMOT_LOG_TYPE_ALL_INPOS:
          if (logSkip-- <= 0) {
            ls.item.allInpos.time = end;
            for (axis = 0;
                 axis < EMCMOT_LOG_NUM_AXES &&
                   axis < EMCMOT_MAX_AXIS;
                 axis++) {
              ls.item.allInpos.input[axis] = emcmotStatus->input[axis];
            }
            logSkip = emcmotStatus->logSkip;
            logIt = 1;
          }
          break;

        case EMCMOT_LOG_TYPE_ALL_OUTPOS:
          if (logSkip-- <= 0) {
            ls.item.allOutpos.time = end;
            for (axis = 0;
                 axis < EMCMOT_LOG_NUM_AXES &&
                   axis < EMCMOT_MAX_AXIS;
                 axis++) {
              ls.item.allOutpos.output[axis] = jointPos[axis];
            }
            logSkip = emcmotStatus->logSkip;
            logIt = 1;
          }
          break;

        case EMCMOT_LOG_TYPE_AXIS_VEL:
          if (logSkip-- <= 0) {
            ls.item.axisVel.time = end;
            ls.item.axisVel.cmdVel = jointPos[loggingAxis] -
              oldJointPos[loggingAxis];
            ls.item.axisVel.actVel = emcmotStatus->input[loggingAxis] -
              oldInput[loggingAxis];
            logSkip = emcmotStatus->logSkip;
            logIt = 1;
          }
          break;

        case EMCMOT_LOG_TYPE_ALL_FERROR:
          if (logSkip-- <= 0) {
            ls.item.allFerror.time = end;
            for (axis = 0;
                 axis < EMCMOT_LOG_NUM_AXES &&
                   axis < EMCMOT_MAX_AXIS;
                 axis++) {
              ls.item.allFerror.ferror[axis] = thisFerror[axis];
            }
            logSkip = emcmotStatus->logSkip;
            logIt = 1;
          }
          break;

        default:
          break;
        } /* end of switch on log type */

        /* now log it */
#if defined (rtlinux) && defined (RT_FIFO)
        if (logIt) {
          rtf_put(EMCMOT_LOG_RTF, &ls, sizeof(EMCMOT_LOG_STRUCT));
          /* FIXME-- update emcmotStatus->logPoints */
          logIt = 0;
        }
#else
        if (logIt) {
          emcmotLogAdd(emcmotLog, ls);
          emcmotStatus->logPoints = emcmotLog->howmany;
          logIt = 0;
        }
#endif
      } /* end of if logging */

      /* set tail to head, which has already been incremented */
      emcmotStatus->tail = emcmotStatus->head;

      /* WRITE STATUS DATA */

#if defined (rtlinux) && defined (RT_FIFO)
      /* write to rtlinux FIFO */
      rtf_put(EMCMOT_STATUS_RTF, &emcmotStatusBuffer, sizeof(EMCMOT_STATUS));

#endif /* rtlinux and RT_FIFO */

#ifdef rtlinux
      /* wait for next cycle */
      rt_task_wait();
    }
#endif /* rtlinux */
}

#ifdef STEPPER_MOTORS

static void smController(int arg)
{
  int axis;
  int steps;
  double dsteps;
  unsigned char smByte = 0;

  for (;;)
    {
      for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
        {
          if (smClkPhase[axis])
            {
              /* clock phase is high, so we need to reset it, 1-3-5 */
              smByte &= ~(0x02 << (axis << 1)); /* clk */
              /* ret it low so it can go high next time, if needed */
              smClkPhase[axis] = 0;
            }
          else
            {
              /* clock phase is low, so we can output a step if needed,
                 and set it, 1-3-5 */
              fpInUse = 1;
              dsteps = jointPos[axis] * smStepsPerUnit[axis] +
                emcmotStatus->inputOffset[axis];
              if (dsteps < 0.0)
                steps = (int) (dsteps - 0.5);
              else
                steps = (int) (dsteps + 0.5);
              fpInUse = 0;
              if (steps > smStepsAccum[axis])
                {
                  smStepsAccum[axis]++;
                  smByte |= (0x01 << (axis << 1)); /* dir */
                  smByte |= (0x02 << (axis << 1)); /* clk */
                  /* set it high so it will go low next time, regardless */
                  smClkPhase[axis] = 1;
                }
              else if (steps < smStepsAccum[axis])
                {
                  smStepsAccum[axis]--;
                  smByte &= ~(0x01 << (axis << 1)); /* dir */
                  smByte |= (0x02 << (axis << 1)); /* clk */
                  /* set it high so it will go low next time, regardless */
                  smClkPhase[axis] = 1;
                }
            }
        }

      pptDioByteWrite(0, smByte);

      rt_task_wait();
    }
}

#endif /* ifdef STEPPER_MOTORS */

int init_module(void)
{
  int axis;
  int t;
#ifdef rtlinux
  RTIME now;
#endif
  PID_STRUCT pid;

#ifdef rtlinux

#ifdef RT_FIFO
  rtf_create(EMCMOT_COMMAND_RTF, sizeof(EMCMOT_COMMAND) + 1);
  rtf_create(EMCMOT_STATUS_RTF, sizeof(EMCMOT_STATUS) + 1);
  rtf_create(EMCMOT_ERROR_RTF, EMCMOT_ERROR_NUM * EMCMOT_ERROR_LEN + 1);
  /* log is created, deleted dynamically */

  /* fifo puts and gets work on buffer structs, so point to these */
  emcmotCommand = &emcmotCommandBuffer;
  emcmotStatus = &emcmotStatusBuffer;

#else  /* not RT_FIFO */
  /* set upper memory pointers */
  emcmotStruct = (EMCMOT_STRUCT *) SHMEM_BASE_ADDRESS;

#endif /* else not RT_FIFO */

#else  /* not rtlinux */
  /* get command shmem */
  shmem = rcs_shm_open(SHMEM_KEY, sizeof(EMCMOT_STRUCT), 1, 0666);
  if (NULL == shmem ||
      NULL == shmem->addr)
    {
      diagnostics("can't get shared memory\n");
      return -1;
    }

  memset(shmem->addr, 0, sizeof(EMCMOT_STRUCT));
  /* map shmem area into local address space */
  emcmotStruct = (EMCMOT_STRUCT *) shmem->addr;

#endif /* else not rtlinux */

#ifndef RT_FIFO
  /* we'll reference emcmotStruct directly */
  emcmotCommand = (EMCMOT_COMMAND *) &emcmotStruct->command;
  emcmotStatus = (EMCMOT_STATUS *) &emcmotStruct->status;
  emcmotError = (EMCMOT_ERROR *) &emcmotStruct->error;
  emcmotLog = (EMCMOT_LOG *) &emcmotStruct->log;

  /* zero shared memory */
  for (t = 0; t < sizeof(EMCMOT_STRUCT); t++)
    {
      ((char *) emcmotStruct)[t] = 0;
    }
#endif /* not RT_FIFO */

  /* init locals */
  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
    {
      maxLimitSwitchCount[axis] = 0;
      minLimitSwitchCount[axis] = 0;
      /* STEPPER MOTORS */
      smStepsPerUnit[axis] = 1.0;
      smStepsAccum[axis] = 0;
      smClkPhase[axis] = 0;
      /* END STEPPER MOTORS */
    }

#ifndef RT_FIFO
  /* init error struct */
  emcmotErrorInit(emcmotError);
#endif /* not RT_FIFO */

  /* init command struct */
  emcmotCommand->head = 0;
  emcmotCommand->command = 0;
  emcmotCommand->commandNum = 0;
  emcmotCommand->tail = 0;

  /* init status struct */

  emcmotStatus->head = 0;

  emcmotStatus->motionFlag = 0;
  SET_MOTION_ERROR_FLAG(0);
  SET_MOTION_COORD_FLAG(0);
  emcmotStatus->split = 0;
  emcmotStatus->commandEcho = 0;
  emcmotStatus->commandNumEcho = 0;
  emcmotStatus->commandStatus = 0;
  emcmotStatus->heartbeat = 0;
  emcmotStatus->computeTime = 0.0;
  emcmotStatus->numAxes = EMCMOT_MAX_AXIS;
  emcmotStatus->trajCycleTime = TRAJ_CYCLE_TIME;
  emcmotStatus->servoCycleTime = SERVO_CYCLE_TIME;
  emcmotStatus->pos.tran.x = 0.0;
  emcmotStatus->pos.tran.y = 0.0;
  emcmotStatus->pos.tran.z = 0.0;
  emcmotStatus->actualPos.tran.x = 0.0;
  emcmotStatus->actualPos.tran.y = 0.0;
  emcmotStatus->actualPos.tran.z = 0.0;
  emcmotStatus->vel = VELOCITY;
  emcmotStatus->limitVel = VELOCITY;
  emcmotStatus->acc = ACCELERATION;
  emcmotStatus->qVscale = 1.0;
  emcmotStatus->id = 0;
  emcmotStatus->depth = 0;
  emcmotStatus->activeDepth = 0;
  emcmotStatus->paused = 0;
  SET_MOTION_INPOS_FLAG(1);
  emcmotStatus->logOpen = 0;
  emcmotStatus->logStarted = 0;
  emcmotStatus->logSize = 0;
  emcmotStatus->logSkip = 0;
  emcmotStatus->logPoints = 0;
  SET_MOTION_ENABLE_FLAG(0);

  oldPos = emcmotStatus->pos;
  oldVel.tran.x = 0.0;
  oldVel.tran.y = 0.0;
  oldVel.tran.z = 0.0;

  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
    {
      emcmotStatus->homingVel[axis] = VELOCITY;
      emcmotStatus->axisFlag[axis] = 0;
      emcmotStatus->maxLimit[axis] = MAX_LIMIT;
      emcmotStatus->minLimit[axis] = MIN_LIMIT;
      emcmotStatus->maxOutput[axis] = MAX_OUTPUT;
      emcmotStatus->minOutput[axis] = MIN_OUTPUT;
      emcmotStatus->maxFerror[axis] = MAX_FERROR;
      emcmotStatus->ferror[axis] = 0.0;
      emcmotStatus->outputScale[axis] = OUTPUT_SCALE;
      emcmotStatus->outputOffset[axis] = OUTPUT_OFFSET;
      emcmotStatus->inputScale[axis] = INPUT_SCALE;
      emcmotStatus->inputOffset[axis] = INPUT_OFFSET;
      emcmotStatus->axVscale[axis] = 1.0;
      SET_AXIS_ENABLE_FLAG(axis, 0);
      SET_AXIS_ACTIVE_FLAG(axis, 1); /* default is use it */
      SET_AXIS_NSL_FLAG(axis, 0);
      SET_AXIS_PSL_FLAG(axis, 0);
      SET_AXIS_NHL_FLAG(axis, 0);
      SET_AXIS_PHL_FLAG(axis, 0);
      SET_AXIS_INPOS_FLAG(axis, 1);
      SET_AXIS_HOMING_FLAG(axis, 0);
      SET_AXIS_HOMED_FLAG(axis, 0);
      SET_AXIS_FERROR_FLAG(axis, 0);
      SET_AXIS_FAULT_FLAG(axis, 0);
      SET_AXIS_ERROR_FLAG(axis, 0);
      emcmotStatus->axisPolarity[axis] = (EMCMOT_AXIS_FLAG) 0xFFFFFFFF;
      /* will read encoders directly, so don't set them here */
    }

  /* FIXME-- add emcmotError */

  /* init min-max-avg stats */
  mmxavgInit(&tMmxavg, tMmxavgSpace, MMXAVG_SIZE);
  mmxavgInit(&sMmxavg, sMmxavgSpace, MMXAVG_SIZE);
  mmxavgInit(&nMmxavg, nMmxavgSpace, MMXAVG_SIZE);
  emcmotStatus->tMin = 0.0;
  emcmotStatus->tMax = 0.0;
  emcmotStatus->tAvg = 0.0;
  emcmotStatus->sMin = 0.0;
  emcmotStatus->sMax = 0.0;
  emcmotStatus->sAvg = 0.0;
  emcmotStatus->nMin = 0.0;
  emcmotStatus->nMax = 0.0;
  emcmotStatus->nAvg = 0.0;

  /* init motion queue */
#ifdef rtlinux
  if ( -1 == sqInitQueue(&queue,(SEGMENT *)(SHMEM_BASE_ADDRESS+sizeof(EMCMOT_STRUCT)), TC_QUEUE_SIZE))
#else
  sqMemPool = (SEGMENT *)malloc(TC_QUEUE_SIZE*sizeof(SEGMENT));
  if ( sqMemPool == NULL )
    {
      diagnostics("Error in init_module(): cannot allocate memory for segmentqueue\n");
      return -1;
    }
  if ( -1 == sqInitQueue(&queue,sqMemPool, TC_QUEUE_SIZE))
#endif
    {
      diagnostics("can't initialize motion queue\n");
      return -1;
    }
  sqSetCycleTime(&queue, emcmotStatus->trajCycleTime);
  sqSetPos(&queue, emcmotStatus->pos);
  sqSetVmax(&queue, emcmotStatus->vel);
          sqSetFeed(&queue, emcmotStatus->vel);
          /* FIX ME ^^^^ the feed and the absolute maximum velocity
             are set the same number */

  sqSetMaxAcc(&queue, emcmotStatus->acc);
  sqSetMaxFeedOverride(&queue, 1.0 );

  /* init the axis components */
  pid.p = P_GAIN;
  pid.i = I_GAIN;
  pid.d = D_GAIN;
  pid.ff0 = FF0_GAIN;
  pid.ff1 = FF1_GAIN;
  pid.ff2 = FF2_GAIN;
  pid.backlash = BACKLASH;
  pid.bias = BIAS;
  pid.maxError = MAX_ERROR;

  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
    {
      if (-1 == tpCreate(&freeAxis[axis], FREE_AXIS_QUEUE_SIZE, freeAxisTcSpace[axis]))
        {
          diagnostics("can't create axis queue %d\n", axis);
          return -1;
        }
      tpInit(&freeAxis[axis]);
      tpSetCycleTime(&freeAxis[axis], emcmotStatus->trajCycleTime);
      tpSetPos(&freeAxis[axis], emcmotStatus->pos);
      tpSetVmax(&freeAxis[axis], emcmotStatus->vel);
      tpSetAmax(&freeAxis[axis], emcmotStatus->acc);
      pidInit(&emcmotStatus->pid[axis]);
      pidSetGains(&emcmotStatus->pid[axis], pid);
      cubicInit(&cubic[axis]);
    }

  /* init the time and rate using functions to affect traj, the
     pids, and the cubics properly, since they're coupled */
  setTrajCycleTime(TRAJ_CYCLE_TIME);
  setServoCycleTime(SERVO_CYCLE_TIME);

  /* init board */
  if (-1 == extMotInit((const char *) 0))
    {
      diagnostics("can't initialize motion hardware\n");
      return -1;
    }
  if (-1 == extDioInit((const char *) 0))
    {
      diagnostics("can't initialize DIO hardware\n");
      return -1;
    }
  if (-1 == extAioInit((const char *) 0))
    {
      diagnostics("can't initialize AIO hardware\n");
      return -1;
    }

  extEncoderSetIndexModel(EXT_ENCODER_INDEX_MODEL_MANUAL);
  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
    {
      rawInput[axis] = 0.0;
      rawOutput[axis] = 0.0;
      trajPos[axis] = 0.0;
      jointPos[axis] = 0.0;
      oldJointPos[axis] = 0.0;
      oldInput[axis] = 0.0;

      waitingForLatch[axis] = 0;
      latchFlag[axis] = 0;
      saveLatch[axis] = 0.0;

      wasOnLimit[axis] = 0;
      emcmotStatus->input[axis] = 0.0;
      lastInput[axis] = 0.0;
      emcmotStatus->output[axis] = 0.0;

      extAmpEnable(axis,
                   ! GET_AXIS_ENABLE_POLARITY(axis));
    }

  emcmotStatus->tail = 0;

#ifdef rtlinux

  /* get current time as base for starting tasks */
  now = rt_get_time();

  /* init control task */
  rt_task_init(&emcmotTask,     /* RT_TASK * */
               emcmotController, /* task code */
               0,               /* startup arg to task code */
               RT_TASK_STACK_SIZE, /* task stack size */
               RT_TASK_PRIORITY); /* priority */

  /* make sure we save FP context */

#if defined(rtlinux_09J) || defined(rtlinux_1_0)
  rt_task_use_fp(&emcmotTask, 1);
#else
  rt_use_fp(1);
#endif

  /* schedule control task to run */
  rt_task_make_periodic(&emcmotTask,
                        now + SECS_TO_RTIME(0.010),
                        (RTIME) (RT_TICKS_PER_SEC *
                                 emcmotStatus->servoCycleTime));

#ifdef STEPPER_MOTORS

  /* init stepper motor task */
  rt_task_init(&smTask, /* RT_TASK * */
               smController, /* task code */
               0,               /* startup arg to task code */
               SM_TASK_STACK_SIZE, /* task stack size */
               SM_TASK_PRIORITY); /* priority */

  /* make sure we save FP context */
#if defined(rtlinux_09J) || defined(rtlinux_1_0)
  rt_task_use_fp(&smTask, 1);
#endif

  /* schedule stepper task to run at twice servo (axis) rate */
  rt_task_make_periodic(&smTask,
                        now + SECS_TO_RTIME(0.010),
                        ((RTIME) (RT_TICKS_PER_SEC *
                                 emcmotStatus->servoCycleTime)) >> 1);

#endif

  /* FIXME */
  diagnostics("inited emcmot\n");

#endif /* rtlinux */

#if defined (rtlinux) && defined (RT_FIFO)
  /* create user process handler on control channel */
  rtf_create_handler(EMCMOT_COMMAND_RTF, /* fifo number */
                     emcmotCommandHandler /* control message handler */
                     );

#endif /* rtlinux and RT_FIFO */
  return 0;
}

void cleanup_module(void)
{
  int axis;

  /* release traj planner space */
#if !defined(rtlinux)
  free(sqMemPool);
#endif
  sqTrashQueue(&queue);

  /* release axis planner spaces */
  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
    {
      tpDelete(&freeAxis[axis]);
    }

  /* disable amps */
  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
    {
      extAmpEnable(axis,
                   ! GET_AXIS_ENABLE_POLARITY(axis));
    }

  /* quit board */
  extAioQuit();
  extDioQuit();
  extMotQuit();

#ifdef rtlinux

#ifdef RT_FIFO
  rtf_destroy(EMCMOT_COMMAND_RTF);
  rtf_destroy(EMCMOT_STATUS_RTF);
  rtf_destroy(EMCMOT_ERROR_RTF);
#endif

  /* delete control task */
  rt_task_delete(&emcmotTask);

  /* FIXME */
  diagnostics("exited emcmot\n");

#ifdef STEPPER_MOTORS

  /* delete stepper motor task */
  rt_task_delete(&smTask);

#endif

#else

  /* detach from shmem */
  if (NULL != shmem)
    {
      rcs_shm_close(shmem);
      shmem = NULL;
    }

#endif
}

#ifndef EMCMOT_NO_MAIN

static int done = 0;
static void quit(int sig)
{
  done = 1;
}

#include <string.h>             /* strcmp() */
#include <signal.h>             /* signal(), SIGINT */

static int getArgs(int argc, char *argv[])
{
  int t;
  int len;

  for (t = 1; t < argc; t++)
    {
      /* try -ini */
      /* NOTE: motion controller cannot normally read an INI file.
         This is provided for main() simulation, which can read
         an INI file and needs it for the plant simulation parameters */
      if (!strcmp(argv[t], "-ini"))
        {
          if (t == argc - 1)
            {
              diagnostics("missing -ini parameter\n");
              return -1;
            }
          else
            {
              strcpy(EMCMOT_INIFILE, argv[t+1]);
              t++;              /* step over following arg */
              continue;
            }
        }

      /* try SHMEM_BASE_ADDRESS */
      len = strlen("SHMEM_BASE_ADDRESS");
      if (! strncmp(argv[t], "SHMEM_BASE_ADDRESS", len))
        {
          if (argv[t][len] != '=' ||
              1 != sscanf(&argv[t][len + 1], "%i", &SHMEM_BASE_ADDRESS))
            {
              diagnostics("syntax: %s SHMEM_BASE_ADDRESS=integer\n", argv[0]);
              return -1;
            }
          else
            {
              continue;
            }
        }

      /* try SHMEM_KEY */
      len = strlen("SHMEM_KEY");
      if (! strncmp(argv[t], "SHMEM_KEY", len))
        {
          if (argv[t][len] != '=' ||
              1 != sscanf(&argv[t][len + 1], "%i", &SHMEM_KEY))
            {
              diagnostics("syntax: %s SHMEM_KEY=integer\n", argv[0]);
              return -1;
            }
          else
            {
              continue;
            }
        }

      /* no match-- bad arg */
      diagnostics("bad argument to %s: %s\n", argv[0], argv[t]);
      return -1;
    }

  return 0;
}

/*
  syntax: a.out SHMEM_BASE_ADDRESS=int SHMEM_KEY=int STG_BASE_ADDRESS=int

  Syntax matches that used with insmod sym=value, for consistency
  */
int main(int argc, char *argv[])
{
  int t;
  int shm;                      /* shared mem key override */
  int base;                     /* shared mem base addr override */
  double delta;                 /* elapsed time */

  /* trap ^C */
  signal(SIGINT, quit);

  /* process command line args */
  if (0 != getArgs(argc, argv))
    {
      diagnostics("can't init module-- bad arguments\n");
      exit(1);
    }

  /* set up data structs */
  if (-1 == init_module())
    {
      diagnostics("can't init module-- execution permission problems?\n");
      exit(1);
    }

  while (! done)
    {
      delta = etime();
      emcmotController(0);
      delta = etime() - delta;
      delta = emcmotStatus->servoCycleTime - delta;

      if (delta > 0.0)
        {
          esleep(delta);
        }
    }

  cleanup_module();
  exit(0);
}

#endif /* not EMCMOT_NO_MAIN */

---