---

emcstepmot.c

Go to the documentation of this file.

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

/*
  emcstepmot.c

  Real-time motion controller for queued 3-axis motion planning, independent
  axis motion, cubic subinterpolation, and stepper or servo control.

  Modification history:

*FIX ME* extMotDout is throwing an "unresolved symbol" error for stepper
systems. Have included a dummy call as a temp fix.

  16-Jul-2001  FMP upped the task stack sizes for stepper and emcmot tasks
  to 4096 and 8192, resp., matching the fix that Will did in emcmot.c
  28-Jun-2001  FMP fixed WPS's mmxavg debug structs; added STEPPING_TYPE
  as a module parameter, just to be compatible with runs scripts that
  pass this to freqmod.
  31-May-2001  FMP took out EMCMOT_C conditional
  24-Oct-2000 terrylr added define EMCMOT_C to allow for the conditional
  declaration of init_module & cleanup_module in emcmot.h.
  21-Sep-2000  FMP added check against GET_AXIS_ENABLE_FLAG() before
  aborting amp faults. Changed AMP_FAULT_DEBOUNCE from 10 to 100. For
  500 usec servos, this is 50 msecs, and fixes the race condition problem
  between amp enable and a reset of amp fault, in systems where amp fault
  equals not amp enable.
  15-Aug-2000  FMP added alter to comping compatibility
  8-Aug-2000  FMP added EMCMOT_COMP stuff for compatibility, but nothing
  is done with it here; added probing stuff from emcmot.c
  31-Jul-2000  FMP put PERIOD definition in, to keep the .run scripts
  from failing when insmod set this variable. It's unused here, so it
  has no effect other than to make the .run script happy.
  21-Jul-2000  FMP put gain setting back in, so that gains don't get reset
  12-Apr-2000 WPS eliminated redundant second include string.h
  30-Mar-2000 WPS added ferrorCurrent and ferrorHighMark junk
  21-Mar-2000 WPS eliminated #includes of <rtl_fifo.h>, we don't use FIFO's
  anymore and it caused an anoying warning message.
  13-Mar-2000 WPS changed hrgettime to gethrtime and fixed the
  rt_make_task_periodic that was still compiled under RTL 2.0.
  13-Mar-2000 WPS added unused attribute to ident to avoid 'defined but
  not used' compiler warning, and added (void) to many functions with no
  arguments to avoid 'declaration is not a prototype'  compiler warning
  6-Mar-2000 WPS eliminated the #include <linux/rt_sched.h> and
  <linux/rt_time.h>, which appeared to be only needed for pre- rtlinux_09J
  platforms and those are unlikely to work anyway.
  3-Mar-2000  FMP changed filename from emcmot.c to emcstepmot.c, along
  with changing emcfreqmot.c to emcmot.c. This file has thus been demoted.
  28-Feb-2000  FMP changed static smStepsAccum[] to non-static stepperCount[]
  30-Jan-2000  FMP added quantization of computed position about input
  scale, using rounding
  22-Jan-2000  FMP added triggering of POS_VOLTAGE log with dac write
  13-Jan-2000  WPS updated for rtlinux2 including the following:
  -> replace rt_task_ functions with pthread_ functions (more portable?)
  -> replace lilo ioremap/vremap with mbuff calls. (no need for lilo append)
  -> replace rt_get_time with gethrtime() (high-resolution time)
  20-Dec-1999  FMP restored dual-cycle version of stepper code, from
  1.119 version, which ran best on Bridgeport.
  17-Dec-1999  FMP ran sm task at half, not quarter time, so the cycle
  time would be the same as everybody expects. Note that this is twice
  as fast as it need run, since we're at one full pulse per sm task.
  9-Dec-1999  FMP reversed priorities on motion and stepper tasks, so that
  stepper task had highest priority; only cleared clock bits at start of
  stepper task, leaving direction bits alone.
  7-Dec-1999  FMP changed smController to run at full pulse rate
  3-Dec-1999  FMP added smCyclesPerAxis as configuration variable, that isn't
  recalculated using floating point in smController each cycle. This was to
  speed it up.
  23-Nov-1999  FMP restored last output bytes in smController to preserve
  last values of bits not changed.
  16-Nov-1999  FMP changed the logic in the stepper code so that pulses
  are spread across the whole servo cycle, instead of bunched at the front,
  and the duty cycle is 50-50. Added later fix on smLastUp so that a group
  never finished on an up pulse.
  12-Nov-1999  FMP took negative axis in EMCMOT_OVERRIDE_LIMITS to mean
  don't override limits
  3-Nov-1999  FMP added EMCMOT_LOG_POS_VOLTAGE
  29-Oct-1999  FMP changed rtlinux_b11 to rtlinux2
  22-Oct-1999  FMP added MODULE_PARM() calls for globals
  21-Oct-1999  FMP added rtlinux_b11 port to 2.2 kernels; ioremap(),
  iounmap() calls for 2.2.
  21-Oct-1999  FMP added call to SET_AXIS_HOMED_FLAG(axis, 0) for steppers,
  to clear the homed flag if we went into the disabled state. This is to
  catch the case where steppers aren't on a full pulse when power is taken
  away, which could make the system off by pulse requiring homing again.
  18-Oct-1999  FMP added missing call to tpClear() for each axis' motion
  planner, to fix problem with axes jumping when overriding a limit.
  8-Oct-1999  FMP added isHoming() to check for any axis homing, to support
  homing off a limit switch tied into all axes.
  5-Oct-1999  FMP added limitFerror, the speed-dependent following error.
  Now following error depends linearly on speed, so for slower moves the
  fatal following error is lower.
  30-Sep-1999  FMP didn't flag an error or abort on hardware limits when
  homing, and used them as a home condition along with home switch.
  29-Sep-1999  FMP added homeOffset[] to status, EMCMOT_SET_HOME_OFFSET to cmd
  21-Sep-1999  WPS eliminate sscanf and printf calls not supported under CE.
  14-Sep-1999  FMP added inverseOutputScale[], like inverseInputScale[], to
  save a division at the servo cycle.
  17-Aug-1999  FMP fixed typo from yesterday
  16-Aug-1999  FMP allowed calculation of actualPos if not homed, if we
  have trivial kinematics
  9-Aug-1999  FMP added inverseInputScale[] = 1/inputScale[], to convert
  a division into a multiplication to save calc time at the servo rate.
  Added setting of home position(s) to be jointHome[], worldHome, from
  EMCMOT_SET_WORLD_HOME, EMCMOT_SET_JOINT_HOME. Revised calls to kinematics
  to use new naming convention "kinematicsInv/For/Home".
  4-Aug-1999  FMP fixed watchdog so that sound port bit is zeroed when
  watchdog is turned off; fixed newly-introduced bug in soft limit checking
  that gave a soft limit when not homed, for negative directions.
  3-Aug-1999  FMP bracketed checks for limit, fault, home switch with
  check that the joint is active; set default for axis to be deactivated
  2-Aug-1999  FMP split out Cartesian and joint motion more explicitly;
  added cubicOffset in place of cubicDrain when a joint homed. Reworked
  the way kinematics functions were called, and coord/free mode interacted.
  Added coarseJointPos[] array. Called forward kins at the simulated
  trajectory rate when disabled, via the interpolationCounter var. Fixed
  handling of axes >=4 in smController().
  26-Jul-1999  FMP put 6 axes' worth of bits into stepper motor task
  23-Jul-1999  FMP fixed bug in calculating smUnitsPerStep from 1/input scale,
  in which negative input scales gave negative units per step, which were
  always assumed to be positive in the backlash calcs. Converted backlash
  comp from step-per-cycle application to acc/dec application.
  Suppressed redundant writes of stepper motor byte in smController.
  Added smInhibit flag that causes smController to set command steps
  equal to accum steps, so we won't have stepper runaway.
  22-Jul-1999  FMP added backlash comp for steppers, in which backlash
  takeup was meted out one pulse per servo cycle; fixed homing so that
  there was a controlled move from the post-home decel back to home.
  Previously after the post-home decel the home position was given directly
  to the servos, so it screamed back the full decel distance. This was so
  small that no one noticed.
  14-Jul-1999  FMP set emcmotStatus->qVscale,axVscale[] only when
  EMCMOT_SCALE was read, not every cycle; put head/tail in command handler
  and moved down a bit in controller
  1-Jul-1999  FMP reduced calls to kinematicsForward()
  24-Jun-1999  FMP added vremap(), vfree() calls to RT Linux shared mem;
  changed EMCMOT_NO_MAIN to EMCMOT_MAIN
  16-Jun-1999  FMP added axisPos[] to status
  14-Jun-1999  FMP added #ifdef STEPPER_MOTORS bracketing around some more
  stepper data
  11-Jun-1999  FMP calculated smTask cycle time using T = 1/2 * 1/(V*P),
  where V is max vel, P is pulses per unit. Whenever these are changed,
  the task rate is changed also.
  10-Jun-1999  FMP added magFerror to solve problem that following error
  was only compared in the positive direction against the max
  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 emcmotConfig->limitVel, for upper velocity limit.
  Added EMCMOT_SET_VEL_LIMIT
  7-Jun-1999  FMP fixed logging of trajectory cycle points with the logIt
  flag, without which points were logged every servo cycle; #ifdef'ed out
  PID, output, and rawoutput sections in code if we're doing steppers
  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
  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
  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

#if (defined(rtlinux2) || defined(RTLINUX_V2)) && !defined(CONFIG_RTL_USE_V1_API)
#define USE_RTL2
#endif


#ifdef USE_RTL2
#include <rtl.h>
#ifdef RT_FIFO
#include <rtl_fifo.h>
#endif
#include <time.h>
#include <rtl_sched.h>
#include <rtl_sync.h>
#include <pthread.h>
#include <unistd.h>
#include <rtl_debug.h>
#include <errno.h>
#include "mbuff.h"
#endif

/*
  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>
#include <linux/mm.h>           /* vremap(), vfree() */

#ifndef USE_RTL2
#include <rtl_sched.h>
#include <rtl_fifo.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(), ... */
#if !defined(UNDER_CE) && !defined(NO_STDIO_SUPPORT)
#include <stdio.h>              /* printf() */
#endif
#include <stdlib.h>             /* sizeof() */
#include <math.h>
#include <stdarg.h>             /* va_start() */
#include <string.h>             /* memset() */

#endif /* else not rtlinux */

#include "posemath.h"           /* PmPose, pmCartMag() */
#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 "extintf.h"
#include "mmxavg.h"
#include "emcmotlog.h"

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

static char __attribute__((unused))  ident[] = "$Id: emcstepmot.c,v 1.18 2001/10/18 15:29:09 wshackle Exp $";

void extMotDout(unsigned char byte)     // P.C. Added this to fix a broken call
    {                                   // when the functioning call has been
    return;                             // included for steppermotors, this
    }                                   // can be deleted.
    
#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 */
#ifdef USE_RTL2
static pthread_t emcmotTask;
#else
static RT_TASK emcmotTask;
#endif

#define RT_TASK_PRIORITY 2      /* the highest is 1, the lowest is
                                   RT_LOWEST_PRIORITY */
#define RT_TASK_STACK_SIZE 8192

#ifdef STEPPER_MOTORS
#ifdef USE_RTL2
static pthread_t smTask;
#else
static RT_TASK smTask;
#endif
#define SM_TASK_PRIORITY 1
#define SM_TASK_STACK_SIZE 4096
#endif

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

#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.
  emcmotComp[] points to emcmotStruct->comp[].
 */

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

/* need to allocate command, status */
static EMCMOT_COMMAND emcmotCommandBuffer;
static EMCMOT_STATUS emcmotStatusBuffer;
static EMCMOT_CONFIG emcmotConfigBuffer;
static EMCMOT_DEBUG emcmotDebugBuffer;

#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;
static EMCMOT_CONFIG *emcmotConfig;
static EMCMOT_DEBUG *emcmotDebug;

#if defined (rtlinux) && defined (RT_FIFO)
#else
static EMCMOT_ERROR *emcmotError; /* unused for RT_FIFO */
static EMCMOT_LOG *emcmotLog;   /* unused for RT_FIFO */
static EMCMOT_COMP *emcmotComp[EMCMOT_MAX_AXIS]; /* 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 wdEnabling = 0;
static int wdEnabled = 0;
static int wdWait = 0;
static int wdCount = 0;
static unsigned char wdToggle = 0;

/* flag that all active axes are homed */
static unsigned char allHomed = 0;

/* values for joint home positions */
static double jointHome[EMCMOT_MAX_AXIS];

/* value for world home position */
static EmcPose worldHome = {{0.0, 0.0, 0.0},
                           0.0, 0.0, 0.0};

/* kinematics flags */
static KINEMATICS_FORWARD_FLAGS fflags = 0;
static KINEMATICS_INVERSE_FLAGS iflags = 0;

/* the type of kinematics, from emcmot.h */
static int kinType = 0;

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

/* diagnostics printing */
#ifdef rtlinux
#define diagnostics printk
#else
#ifdef UNDER_CE
#include "rcs_prnt.hh"          /* rcs_print() */
#define diagnostics rcs_print
#else
#define diagnostics printf
#endif
#endif

static inline void MARK_EMCMOT_CONFIG_CHANGE(void)
{
  if(emcmotConfig->head == emcmotConfig->tail) { 
    emcmotConfig->config_num++;
    emcmotStatus->config_num = emcmotConfig->config_num;
    emcmotConfig->head++;
  }
}

/* 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) (emcmotConfig->axisPolarity[ax] & EMCMOT_AXIS_ENABLE_BIT ? 1 : 0)

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

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

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

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

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

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

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

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

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

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

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

static TP_STRUCT queue;         /* coordinated mode planner */
/* the freeAxis TP_STRUCTs are used to store the single joint value,
   in tran.x, that enables us to use the TP_STRUCT functions for axis
   planning. This is overkill, and we need to create a simpler TP_STRUCT
   for single-joint motion planning. */
static TP_STRUCT freeAxis[EMCMOT_MAX_AXIS];
/* freePose is a EmcPose struct that is used to hold a joint value, in
   tran.x, so that the TP_STRUCT functions can be called for scalar joint
   planning */
static EmcPose freePose = {{0.0, 0.0, 0.0}, 0.0, 0.0, 0.0};
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 coarseJointPos[EMCMOT_MAX_AXIS];  /* trajectory point, in joints */
static double jointPos[EMCMOT_MAX_AXIS]; /* interpolated point, in joints */
static double jointVel[EMCMOT_MAX_AXIS]; /* joint velocity */
static double oldJointPos[EMCMOT_MAX_AXIS]; /* ones from last cycle, for vel */
static double oldInput[EMCMOT_MAX_AXIS]; /* ones for actual pos, last cycle */
/* inverseInputScale[] is 1/inputScale[], and lets us use a multiplication
   instead of a division each servo cycle */
static double inverseInputScale[EMCMOT_MAX_AXIS];
static double inverseOutputScale[EMCMOT_MAX_AXIS];
static EmcPose oldPos;           /* last position, used for vel differencing */
static EmcPose oldVel, newVel;   /* velocities, used for acc differencing */
static EmcPose newAcc;           /* differenced acc */

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

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 = 0;      /* non-zero means we already aborted
                                   everything due to a soft
                                   limit, and need not re-abort. It's
                                   cleared only when all limits are
                                   cleared. */
static int onLimit = 0;         /* non-zero means at least one axis is
                                   on a soft limit */

static int overriding = 0;      /* non-zero means we've initiated an axis
                                   move while overriding limits */

static int stepping = 0;
static int idForStep = 0;

#ifdef STEPPER_MOTORS

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

static char smInhibit = 0 ;     /* non-zero means zero stepper task */
static int smSteps[EMCMOT_MAX_AXIS];
static double smStepsPerUnit[EMCMOT_MAX_AXIS]; /* signed, from input scale */
static double smMagUnitsPerStep[EMCMOT_MAX_AXIS]; /* magnitude, always pos */
static double smMaxStepsPerUnit; /* inited to INPUT_SCALE in init_module() */
/* int stepperCount[EMCMOT_MAX_AXIS]; */
static double smVPmax;          /* 1/smVPmax is clock rate for stepper pulse */
static double smCycleTime;
static int smCyclesPerAxis = 1;
static int smNewCycle[EMCMOT_MAX_AXIS];

#endif

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

#ifdef rtlinux

static double etime(void)
{
#ifdef USE_RTL2
  hrtime_t now;

  now = gethrtime();            /* in HR_TICKS */

  return ((double) now) / ((double) HRTICKS_PER_SEC);
#else

  RTIME now;

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

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

#endif /* if rtlinux */

#ifdef UNDER_CE
#include "rcs_ce.h"             /* RCS_CE_VSPRINTF() */
#endif

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

  va_start(args, fmt);
#ifndef UNDER_CE
  vsprintf(error, fmt, args);
#else
  RCS_CE_VSPRINTF(error,fmt,args);
#endif
  va_end(args);

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

#else
  emcmotErrorPut(emcmotError, error);

#endif
}

/* isHoming() returns non-zero if any axes are homing, 0 if none are */
static int isHoming(void)
{
  int axis;

  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
    if (GET_AXIS_HOMING_FLAG(axis)) {
      return 1;
    }
  }

  /* got here, so none are homing */
  return 0;
}

/* axis lengths */
#define AXRANGE(axis) ((emcmotConfig->maxLimit[axis] - emcmotConfig->minLimit[axis]))

/* 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 */
  kinematicsInverse(&pos, joint, &iflags, &fflags);

  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] > emcmotConfig->maxLimit[axis] ||
        joint[axis] < emcmotConfig->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. Called on a linear and circular move. */
static int checkLimits(void)
{
  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 (emcmotStatus->overrideLimits) {
    return 1;                   /* okay to jog when limits overridden */
  }

  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;
}

/* counter that triggers computation of forward kinematics during
   disabled state, making them run at the trajectory rate instead
   of the servo rate. In the disabled state the interpolators are
   not queried so we don't know when a trajectory cycle would occur,
   so we use this counter. It's loaded with emcmotConfig->interpolationRate
   whenever it goes to zero, during the code executed in the disabled
   state. */
static int interpolationCounter = 0;

/* 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;
  }
  MARK_EMCMOT_CONFIG_CHANGE();


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

  /* set traj planner */
  tpSetCycleTime(&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], emcmotConfig->interpolationRate);
  }

  /* copy into status out */
  emcmotConfig->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;
  }

  MARK_EMCMOT_CONFIG_CHANGE();

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

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

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

#ifdef STEPPER_MOTORS
  smCyclesPerAxis = (int) (emcmotConfig->servoCycleTime / smCycleTime);
#endif
}

/*
  emcmotCommandHandler() is called each main cycle to read the
  shared memory buffer
  */
static int emcmotCommandHandler(void)
{
  int axis;                     /* loop counter */
#ifdef rtlinux
#ifdef RT_FIFO
  int err;
#endif
#endif
  int valid;
#ifdef STEPPER_MOTORS
  double mag;                   /* temporary value for magnitudes, to
                                   avoid calling fabs() all the time */
#endif

  /* 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) {
      emcmotDebug->split++;
      return 0;                 /* not really an error */
    }

    if (emcmotCommand->commandNum != emcmotStatus->commandNumEcho) {
      /* increment head count-- we'll be modifying emcmotStatus */
      emcmotStatus->head++;
      emcmotDebug->head++;

      /* 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);
        emcmotStatus->logPoints = emcmotLog->howmany;
#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_NUM_AXES:
        MARK_EMCMOT_CONFIG_CHANGE();
        /* set the global NUM_AXES, which must be between 1 and
           EMCMOT_MAX_AXIS, inclusive */
        axis = emcmotCommand->axis;
        /* note that this comparison differs from the check on the
           range of 'axis' in most other places, since those checks
           are for a value to be used as an index and here it's a value
           to be used as a counting number. The indenting is different
           here so as not to match macro editing on that other bunch. */
        if
          (
           axis <= 0 ||
           axis > EMCMOT_MAX_AXIS
           ) {
          break;
        }
        NUM_AXES = axis;
        break;

      case EMCMOT_SET_WORLD_HOME:
        worldHome = emcmotCommand->pos;
        break;

      case EMCMOT_SET_JOINT_HOME:
        axis = emcmotCommand->axis;
        if (axis < 0 ||
            axis >= EMCMOT_MAX_AXIS) {
          break;
        }
        jointHome[axis] = emcmotCommand->offset; /* FIXME-- use 'home' instead */
        break;

      case EMCMOT_SET_HOME_OFFSET:
        axis = emcmotCommand->axis;
        if (axis < 0 ||
            axis >= EMCMOT_MAX_AXIS) {
          break;
        }
        emcmotConfig->homeOffset[axis] = emcmotCommand->offset;
        break;

      case EMCMOT_OVERRIDE_LIMITS:
        if (emcmotCommand->axis < 0) {
          /* don't override limits */
          emcmotStatus->overrideLimits = 0;
        }
        else {
          emcmotStatus->overrideLimits = 1;
        }
        overriding = 0;
        for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
          SET_AXIS_ERROR_FLAG(axis, 0);
        }
        break;

      case EMCMOT_SET_TRAJ_CYCLE_TIME:
        MARK_EMCMOT_CONFIG_CHANGE();
        /* 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:
        MARK_EMCMOT_CONFIG_CHANGE();
        /* 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
#ifdef USE_RTL2
        pthread_make_periodic_np(emcmotTask,
                                 gethrtime(),
                                 (hrtime_t)  (HRTICKS_PER_SEC *
                                             emcmotConfig->servoCycleTime));
#else
        rt_task_make_periodic(&emcmotTask,
                              rt_get_time(),
                              (RTIME) (RT_TICKS_PER_SEC *
                                       emcmotConfig->servoCycleTime));
#endif
#endif /* rtlinux */
        break;

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

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

        emcmotConfig->minOutput[axis] = emcmotCommand->minLimit;
        emcmotConfig->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;
        inverseOutputScale[axis] = 1.0 / emcmotStatus->outputScale[axis];
        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.0) {
          break;
        }

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

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

#ifdef STEPPER_MOTORS
        /* change the units on the stepper counts; note that this
           is a signed value. De-sign it if you want to consider
           just its magnitude. */
        smStepsPerUnit[axis] = emcmotCommand->scale;
        mag = fabs(smStepsPerUnit[axis]);
        smMagUnitsPerStep[axis] = 1.0 / mag;
        /* update smVPmax so that stepper task runs at proper rate */
        if (mag > smMaxStepsPerUnit) {
          smVPmax = smVPmax * mag / smMaxStepsPerUnit;
          smMaxStepsPerUnit = mag;
        }
        /* set the period to twice the max pulse rate. It need only
           be the same as the pulse rate, but this makes it the same
           as before */
        smCycleTime = 0.5/smVPmax;
        smCyclesPerAxis = (int) (emcmotConfig->servoCycleTime / smCycleTime);
#ifdef USE_RTL2
        pthread_make_periodic_np(smTask,
                              gethrtime(),
                              ((hrtime_t) (HRTICKS_PER_SEC * smCycleTime)));
#else
        rt_task_make_periodic(&smTask,
                              rt_get_time(),
                              ((RTIME) (RT_TICKS_PER_SEC * smCycleTime)));
#endif
#endif
        break;

        /*
          Max and min ferror work like this:
          limiting ferror is determined by slope of ferror line,
          = maxFerror/limitVel
          -> limiting ferror = maxFerror/limitVel * vel.
          If ferror < minFerror then OK else
          if ferror < limiting ferror then OK else ERROR
         */

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

      case EMCMOT_SET_MIN_FERROR:
        MARK_EMCMOT_CONFIG_CHANGE();
        axis = emcmotCommand->axis;
        if (axis < 0 ||
            axis >= EMCMOT_MAX_AXIS ||
            emcmotCommand->minFerror < 0.0) {
          break;
        }
        emcmotConfig->minFerror[axis] = emcmotCommand->minFerror;
        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() ||
            ! 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;
        }

        if (emcmotCommand->vel > 0.0) {
          if (GET_AXIS_HOMED_FLAG(axis)) {
            freePose.tran.x = emcmotConfig->maxLimit[axis];
          }
          else {
            freePose.tran.x = AXRANGE(axis);
          }
        }
        else {
          if (GET_AXIS_HOMED_FLAG(axis)) {
            freePose.tran.x = emcmotConfig->minLimit[axis];
          }
          else {
            freePose.tran.x = - AXRANGE(axis);
          }
        }

        tpSetVmax(&freeAxis[axis], fabs(emcmotCommand->vel));
        tpAddLine(&freeAxis[axis], freePose);
        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() ||
            ! 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;
        }

        if (emcmotCommand->vel > 0.0) {
          freePose.tran.x = jointPos[axis] + emcmotCommand->offset; /* FIXME-- use 'goal' instead */
          if (GET_AXIS_HOMED_FLAG(axis)) {
            if (freePose.tran.x > emcmotConfig->maxLimit[axis]) {
              freePose.tran.x = emcmotConfig->maxLimit[axis];
            }
          }
        }
        else {
          freePose.tran.x = jointPos[axis] - emcmotCommand->offset; /* FIXME-- use 'goal' instead */
          if (GET_AXIS_HOMED_FLAG(axis)) {
            if (freePose.tran.x < emcmotConfig->minLimit[axis]) {
              freePose.tran.x = emcmotConfig->minLimit[axis];
            }
          }
        }

        tpSetVmax(&freeAxis[axis], fabs(emcmotCommand->vel));
        tpAddLine(&freeAxis[axis], freePose);
        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() ||
            ! 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;
        }

        freePose.tran.x = emcmotCommand->offset; /* FIXME-- use 'goal' instead */
        if (GET_AXIS_HOMED_FLAG(axis)) {
          if (freePose.tran.x > emcmotConfig->maxLimit[axis]) {
            freePose.tran.x = emcmotConfig->maxLimit[axis];
          }
          else if (freePose.tran.x < emcmotConfig->minLimit[axis]) {
            freePose.tran.x = emcmotConfig->minLimit[axis];
          }
        }

        tpSetVmax(&freeAxis[axis], fabs(emcmotCommand->vel));
        tpAddLine(&freeAxis[axis], freePose);
        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;
          tpAbort(&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;
          tpAbort(&queue);
          SET_MOTION_ERROR_FLAG(1);
          break;
        }

        /* append it to the queue */
        tpSetId(&queue, emcmotCommand->id);
        if (-1 == tpAddLine(&queue, emcmotCommand->pos)) {
          reportError("Can't add linear move");
          emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
          tpAbort(&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;
          tpAbort(&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;
          tpAbort(&queue);
          SET_MOTION_ERROR_FLAG(1);
          break;
        }

        /* append it to the queue */
        tpSetId(&queue, emcmotCommand->id);
        if (-1 ==
            tpAddCircle(&queue, emcmotCommand->pos,
                        emcmotCommand->center, emcmotCommand->normal,
                        emcmotCommand->turn)) {
          reportError("Can't add circular move");
          emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
          tpAbort(&queue);
          SET_MOTION_ERROR_FLAG(1);
          break;
        }
        else {
          SET_MOTION_ERROR_FLAG(0);
        }
        break;

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


      case EMCMOT_SET_AXIS_VEL_LIMIT:
        /* check axis range */
        axis = emcmotCommand->axis;
        if (axis < 0 ||
            axis >= EMCMOT_MAX_AXIS) {
          break;
        }
        tpSetVlimit(&freeAxis[axis], emcmotCommand->vel);
        emcmotConfig->axisLimitVel[axis] = emcmotCommand->vel;
        // bigVel[axis = 10 * emcmotCommand->vel;
        break;
        
      case EMCMOT_SET_VEL_LIMIT:
        /* set the absolute max velocity for all subsequent moves */
        /* can do it at any time */
#ifdef STEPPER_MOTORS
        /* update smVPmax so that stepper task runs at proper rate,
           before we lose emcmotStatus->vel from overwrite by new value */
        smVPmax = smVPmax * emcmotCommand->vel / emcmotConfig->limitVel;
        /* set the period to twice the max pulse rate. It need only
           be the same as the pulse rate, but this makes it the same
           as before */
        smCycleTime = 0.5/smVPmax;
        smCyclesPerAxis = (int) (emcmotConfig->servoCycleTime / smCycleTime);
#ifdef USE_RTL2
        pthread_make_periodic_np(smTask,
                              gethrtime(),
                              ((hrtime_t) (HRTICKS_PER_SEC * smCycleTime)));
#else
        rt_task_make_periodic(&smTask,
                              rt_get_time(),
                              ((RTIME) (RT_TICKS_PER_SEC * smCycleTime)));
#endif
#endif
        emcmotConfig->limitVel = emcmotCommand->vel;
        for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
          tpSetVlimit(&freeAxis[axis], emcmotConfig->limitVel);
        }
        tpSetVlimit(&queue, emcmotConfig->limitVel);
        /* set the debounce vel */
        bigVel = 10.0 * emcmotConfig->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) {
          emcmotConfig->homingVel[axis] = - emcmotCommand->vel;
          SET_AXIS_HOMING_POLARITY(axis, 0);
        }
        else {
          emcmotConfig->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);
        }
        tpSetAmax(&queue, emcmotStatus->acc);
        break;

      case EMCMOT_PAUSE:
        /* pause the motion */
        /* can happen at any time */
        for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
          tpPause(&freeAxis[axis]);
        }
        tpPause(&queue);
        emcmotStatus->paused = 1;
        break;

      case EMCMOT_RESUME:
        /* resume paused motion */
        /* can happen at any time */
        stepping = 0;
        for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
          tpResume(&freeAxis[axis]);
        }
        tpResume(&queue);
        emcmotStatus->paused = 0;
        break;

      case EMCMOT_STEP:
        /* resume paused motion until id changes */
        /* can happen at any time */
        idForStep = emcmotStatus->id;
        stepping = 1;
        for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
          tpResume(&freeAxis[axis]);
        }
        tpResume(&queue);
        emcmotStatus->paused = 0;
        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);
          emcmotStatus->axVscale[axis] = emcmotCommand->scale;
        }
        tpSetVscale(&queue, emcmotCommand->scale);
        emcmotStatus->qVscale = emcmotCommand->scale;
        break;

      case EMCMOT_ABORT:
        /* abort motion */
        /* can happen at any time */
        /* check for coord or free space motion active */
        if (GET_MOTION_COORD_FLAG()) {
          tpAbort(&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;
        }
        /* force it, instead of callind pidSetGains(), which would require
           that we link in pid.o, which we don't really need to do */
        emcmotConfig->pid[axis].p = emcmotCommand->pid.p;
        emcmotConfig->pid[axis].i = emcmotCommand->pid.i;
        emcmotConfig->pid[axis].d = emcmotCommand->pid.d;
        emcmotConfig->pid[axis].ff0 = emcmotCommand->pid.ff0;
        emcmotConfig->pid[axis].ff1 = emcmotCommand->pid.ff1;
        emcmotConfig->pid[axis].ff2 = emcmotCommand->pid.ff2;
        emcmotConfig->pid[axis].backlash = emcmotCommand->pid.backlash;
        emcmotConfig->pid[axis].bias = emcmotCommand->pid.bias;
        emcmotConfig->pid[axis].maxError = emcmotCommand->pid.maxError;
        emcmotConfig->pid[axis].deadband = emcmotCommand->pid.deadband;
        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:
          case EMCMOT_LOG_TYPE_AXIS_VEL:
          case EMCMOT_LOG_TYPE_POS_VOLTAGE:
            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 */
        /* first ignore triggered log types */
        if (emcmotStatus->logType == EMCMOT_LOG_TYPE_POS_VOLTAGE) {
          break;
        }
        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;
        }
        /* trigger log, if active */
        if (emcmotStatus->logType == EMCMOT_LOG_TYPE_POS_VOLTAGE &&
            loggingAxis == axis &&
            emcmotStatus->logOpen != 0) {
          emcmotStatus->logStarted = 1;
          logSkip = 0;
        }
        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 (axis < 0 ||
            axis >= EMCMOT_MAX_AXIS) {
          break;
        }
        if (GET_MOTION_COORD_FLAG() ||
            ! GET_MOTION_ENABLE_FLAG()) {
          break;
        }

        if (GET_AXIS_HOMING_POLARITY(axis)) {
          freePose.tran.x = + 2.0 * AXRANGE(axis);
        }
        else {
          freePose.tran.x = - 2.0 * AXRANGE(axis);
        }

        tpSetVmax(&freeAxis[axis], emcmotConfig->homingVel[axis]);
        tpAddLine(&freeAxis[axis], freePose);
        waitingForLatch[axis] = 1;
        SET_AXIS_HOMING_FLAG(axis, 1);
        SET_AXIS_HOMED_FLAG(axis, 0);
        break;

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

      case EMCMOT_DISABLE_WATCHDOG:
        wdEnabling = 0;
        break;

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

#ifdef ENABLE_PROBING
      case EMCMOT_SET_PROBE_INDEX:
        emcmotConfig->probeIndex = emcmotCommand->probeIndex;
        break;

      case EMCMOT_SET_PROBE_POLARITY:
        emcmotConfig->probePolarity = emcmotCommand->level;
        break;

      case EMCMOT_CLEAR_PROBE_FLAGS:
        emcmotStatus->probeTripped = 0;
        emcmotStatus->probing = 1;
        break;

      case EMCMOT_PROBE:
        /* most of this is taken from 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;
          tpAbort(&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;
          tpAbort(&queue);
          SET_MOTION_ERROR_FLAG(1);
          break;
        }

        /* append it to the queue */
        tpSetId(&queue, emcmotCommand->id);
        if (-1 == tpAddLine(&queue, emcmotCommand->pos)) {
          reportError("Can't add linear move");
          emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
          tpAbort(&queue);
          SET_MOTION_ERROR_FLAG(1);
          break;
        }
        else {
          emcmotStatus->probeTripped = 0;
          emcmotStatus->probing = 1;
          SET_MOTION_ERROR_FLAG(0);
        }
        break;
#endif /* ENABLE_PROBING */

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

      /* synch tail count */
      emcmotStatus->tail = emcmotStatus->head;
      emcmotConfig->tail = emcmotConfig->head;
      emcmotDebug->tail = emcmotDebug->head;

    } /* 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;
}

/* FIXME */
int EMCMOT_NO_FORWARD_KINEMATICS = 0;

/*
  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
  */
#ifdef USE_RTL2
static void *emcmotController(void *arg)
#else
static void emcmotController(int arg)
#endif
{
  double start, end, delta;     /* time stamping */
  int homeFlag;                 /* result of ext read to home switch */
  int axis;                     /* axis loop counter */
  int t;                        /* loop counter if we're in axis loop */
  int isLimit;                  /* result of ext read to limit sw */
  int whichCycle;               /* 0=none, 1=servo, 2=traj */
  int fault;
  double numres;
  double thisFerror[EMCMOT_MAX_AXIS];
  double limitFerror;           /* current scaled ferror */
  double magFerror;
#ifdef STEPPER_MOTORS
  double bcomp[EMCMOT_MAX_AXIS]; /* backlash comp value */
  char bcompdir[EMCMOT_MAX_AXIS]; /* 0=none, 1=pos, -1=neg */
  double bcompincr[EMCMOT_MAX_AXIS]; /* incremental backlash comp value */
  double dsteps;                /* steps value (double) for stepper task */
  double oldbcomp;
  /* various parameters for acc/dec backlash comp */
  char bac_done[EMCMOT_MAX_AXIS];
  double bac_d[EMCMOT_MAX_AXIS];
  double bac_di[EMCMOT_MAX_AXIS];
  double bac_D[EMCMOT_MAX_AXIS];
  double bac_halfD[EMCMOT_MAX_AXIS];
  double bac_incrincr[EMCMOT_MAX_AXIS];
  double bac_incr[EMCMOT_MAX_AXIS];
#endif
  char finalHome[EMCMOT_MAX_AXIS];

  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
#ifdef STEPPER_MOTORS
    bcomp[axis] = 0.0;
    bcompdir[axis] = 0;
    bcompincr[axis] = 0.0;
    bac_done[axis] = 1;
    bac_d[axis] = 0.0;
    bac_di[axis] = 0.0;
    bac_D[axis] = 0.0;
    bac_halfD[axis] = 0.0;
    bac_incrincr[axis] = 0.0;
    bac_incr[axis] = 0.0;
#endif
    finalHome[axis] = 0;
  }

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

#ifdef rtlinux
    /* toggle sound port watchdog */

    if (wdEnabling && ! wdEnabled) {
      /* just turned WD on */
      wdCount = wdWait;
      wdToggle = 0;
      wdEnabled = 1;
    }
    if (! wdEnabling && wdEnabled) {
      /* just turned WD off */
      /* clear the bit */
      soundByte = inb(SOUND_PORT);
      soundByte &= ~SOUND_MASK;
      outb(soundByte, SOUND_PORT);
      wdEnabled = 0;
    }

    if (wdEnabled && wdCount-- <= 0) {
      soundByte = inb(SOUND_PORT);
      wdToggle = ! wdToggle;
      if (wdToggle) {
        soundByte |= SOUND_MASK;
      }
      else {
        soundByte &= ~SOUND_MASK;
      }
      outb(soundByte, SOUND_PORT);
      wdCount = wdWait;
    }
#endif /* rtlinux */

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

    /* READ COMMAND: */

#ifndef RT_FIFO
    emcmotCommandHandler();

#endif /* not RT_FIFO */

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

    /* READ INPUTS: */

#ifndef STEPPER_MOTORS
    /* latch all encoder feedback into raw input array, done outside
       of for-loop on joints below, since it's a single call for all
       joints */
    extEncoderReadAll(EMCMOT_MAX_AXIS, rawInput);
#endif

    /* process input and read limit switches */
    for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
#ifdef STEPPER_MOTORS
      /* READ INPUTS, deferred from above */
      rawInput[axis] = (double) emcmotDebug->stepperCount[axis] -
        bcompincr[axis] * smStepsPerUnit[axis];
#endif

      /* save old cycle's values */
      oldInput[axis] = emcmotStatus->input[axis];
      /* set input, scaled according to input = (raw - offset) / scale */
      emcmotStatus->input[axis] =
        (rawInput[axis] - emcmotStatus->inputOffset[axis]) *
        inverseInputScale[axis];

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

      /* read limit switches and amp fault from external
         interface, and set 'enabling' to zero if tripped
         to cause immediate stop */

      /* handle limits */
      if (GET_AXIS_ACTIVE_FLAG(axis)) {
        extMaxLimitSwitchRead(axis, &isLimit);
        if (isLimit == GET_AXIS_PHL_POLARITY(axis)) {
          if (++maxLimitSwitchCount[axis] > LIMIT_SWITCH_DEBOUNCE) {
            SET_AXIS_PHL_FLAG(axis, 1);
            maxLimitSwitchCount[axis] = LIMIT_SWITCH_DEBOUNCE;
            if (emcmotStatus->overrideLimits ||
                isHoming()) {
            }
            else {
              SET_AXIS_ERROR_FLAG(axis, 1);
              enabling = 0;
            }
          }
        }
        else {
          SET_AXIS_PHL_FLAG(axis, 0);
          maxLimitSwitchCount[axis] = 0;
        }
      }
      if (GET_AXIS_ACTIVE_FLAG(axis)) {
        extMinLimitSwitchRead(axis, &isLimit);
        if (isLimit == GET_AXIS_NHL_POLARITY(axis)) {
          if (++minLimitSwitchCount[axis] > LIMIT_SWITCH_DEBOUNCE) {
            SET_AXIS_NHL_FLAG(axis, 1);
            minLimitSwitchCount[axis] = LIMIT_SWITCH_DEBOUNCE;
            if (emcmotStatus->overrideLimits ||
                isHoming()) {
            }
            else {
              SET_AXIS_ERROR_FLAG(axis, 1);
              enabling = 0;
            }
          }
        }
        else {
          SET_AXIS_NHL_FLAG(axis, 0);
          minLimitSwitchCount[axis] = 0;
        }
      }

      if (GET_AXIS_ACTIVE_FLAG(axis) &&
          GET_AXIS_ENABLE_FLAG(axis)) {
        extAmpFault(axis, &fault);
        if (fault == GET_AXIS_FAULT_POLARITY(axis)) {
          if (++ampFaultCount[axis] > AMP_FAULT_DEBOUNCE) {
            SET_AXIS_ERROR_FLAG(axis, 1);
            SET_AXIS_FAULT_FLAG(axis, 1);
            ampFaultCount[axis] = AMP_FAULT_DEBOUNCE;
            enabling = 0;
          }
        }
        else {
          SET_AXIS_FAULT_FLAG(axis, 0);
          ampFaultCount[axis] = 0;
        }
      }

      /* read home switch and set flag if tripped. Handling of home
         sequence is done later. */
      if (GET_AXIS_ACTIVE_FLAG(axis)) {
        extHomeSwitchRead(axis, &homeFlag);
        if (homeFlag == GET_AXIS_HOME_SWITCH_POLARITY(axis)) {
          SET_AXIS_HOME_SWITCH_FLAG(axis, 1);
        }
        else {
          SET_AXIS_HOME_SWITCH_FLAG(axis, 0);
        }
      }

    } /* end of: loop on axes, for reading inputs, setting limit and
           home switch flags */

    /* now we're outside the axis loop, having just read input, scaled it,
         read the limit and home switches and amp faults. We need to abort
         all motion if we're on limits, handle homing sequence, and handle
         mode and state transitions. */

    /* RUN STATE LOGIC: */

    /* check for disabling */
    if (! enabling && GET_MOTION_ENABLE_FLAG()) {
      /* clear out the motion queue and interpolators */
      tpClear(&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;
        }
        /* don't clear the axis error flag, since that may signify why
           we just went into disabled state */
      }
      /* reset the trajectory interpolation counter, so that the kinematics
         functions called during the disabled state run at the nominal
         trajectory rate rather than the servo rate. It's loaded with
         emcmotConfig->interpolationRate when it goes to zero. */
      interpolationCounter = 0;
#ifdef STEPPER_MOTORS
      smInhibit = 1;
#endif
      SET_MOTION_ENABLE_FLAG(0);
      /* don't clear the motion error flag, since that may signify why
         we just went into disabled state */
    }

    /* check for enabling */
    if (enabling && ! GET_MOTION_ENABLE_FLAG()) {
      tpSetPos(&queue, emcmotStatus->pos);
      for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
        freePose.tran.x = jointPos[axis];
        tpSetPos(&freeAxis[axis], freePose);
#ifndef STEPPER_MOTORS
        pidReset(&emcmotConfig->pid[axis]);
#endif
        if (GET_AXIS_ACTIVE_FLAG(axis)) {
          extAmpEnable(axis, GET_AXIS_ENABLE_POLARITY(axis));
          SET_AXIS_ENABLE_FLAG(axis, 1);
          SET_AXIS_HOMING_FLAG(axis, 0);
        }
        /* clear any outstanding axis errors when going into enabled state */
        SET_AXIS_ERROR_FLAG(axis, 0);
      }
#ifdef STEPPER_MOTORS
      smInhibit = 0;
#endif
      SET_MOTION_ENABLE_FLAG(1);
      /* clear any outstanding motion errors when going into enabled state */
      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 */
        tpSetPos(&queue, emcmotStatus->pos);
        /* drain the cubics so they'll synch up */
        for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
          cubicDrain(&cubic[axis]);
        }
        /* clear the override limits flags */
        overriding = 0;
        emcmotStatus->overrideLimits = 0;
        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()) {
        for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
          /* update free planner positions */
          freePose.tran.x = jointPos[axis];
          tpSetPos(&freeAxis[axis], freePose);
          /* drain the cubics so they'll synch up */
          cubicDrain(&cubic[axis]);
        }
        SET_MOTION_COORD_FLAG(0);
        SET_MOTION_ERROR_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])) {
          /* check for decel or final move */
          if (finalHome[axis]) {
            /* reset flag-- we're back at home */
            finalHome[axis] = 0;

            /* rework the home kinematics values-- this could be done
               after each message to set world or joint kinematics, but
               we'll defer it to here so it happens just prior to when
               it's needed. Note that the nominal value of these may be
               changed, if the kinematics need to. */
            kinematicsHome(&worldHome, jointHome, &fflags, &iflags);

            /* set input offset to value such that resulting input
               is the jointHome[] value, with:
               input = (raw - offset) / scale ->
               offset = raw - input * scale ->
               offset = raw - jointHome * scale */

            emcmotStatus->inputOffset[axis] = rawInput[axis] -
              jointHome[axis] * emcmotStatus->inputScale[axis];

            /* recompute inputs to match so we don't have a
               momentary jump */
            emcmotStatus->input[axis] = jointHome[axis];
            oldInput[axis] = emcmotStatus->input[axis];

            /* reset interpolator so that it doesn't jump */
            cubicOffset(&cubic[axis], jointHome[axis] - coarseJointPos[axis]);

            /* set axis position to jointHome upon homing */
            jointPos[axis] = jointHome[axis];
            freePose.tran.x = jointHome[axis];
            tpSetPos(&freeAxis[axis], freePose);

            SET_AXIS_HOMING_FLAG(axis, 0);
            SET_AXIS_HOMED_FLAG(axis, 1);

            /* set allHomed flag if all active axes are homed; this
                 will signify that kinematics functions can be called */
            allHomed = 1;
            for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
              if (GET_AXIS_ACTIVE_FLAG(t) &&
                  ! GET_AXIS_HOMED_FLAG(t)) {
                /* one active axis is not homed */
                allHomed = 0;
                break;
              }
            }
            /* FIXME-- this only works with no forward kins */
            if (allHomed) {
              emcmotStatus->pos = worldHome;
              emcmotStatus->actualPos = worldHome;
            }
          }
          else {
            /* just finished decel, now we'll do final home */
            finalHome[axis] = 1;
            /* add move back to latched position + backoff */
            freePose.tran.x =
              (saveLatch[axis] - emcmotStatus->inputOffset[axis]) *
              inverseInputScale[axis] +
              emcmotConfig->homeOffset[axis];
            tpAddLine(&freeAxis[axis], freePose);
          }
        } /* end of: if axis is done either decel or final home */
      } /* end of: if axis is homing */
    } /* end of: axis loop that checks for homing */

    /* 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, for calc time logging */
      whichCycle = 1;

      /* check to see if the interpolators are empty */
      while (cubicNeedNextPoint(&cubic[0])) {
        /* they're empty, so pull next point(s) off Cartesian or
           joint planner, depending upon coord or free mode. */

        /* check to see whether we're in coordinated or free mode, to
           decide which motion planner to call */
        if (GET_MOTION_COORD_FLAG()) {
          /* we're in coordinated mode-- pull a pose off the Cartesian
             trajectory planner, run it through the inverse kinematics,
             and spline up the joint points for interpolation in
             servo cycles. */

          /* set whichCycle to be a Cartesian trajectory cycle,
             for calc time logging */
          whichCycle = 2;

          /* run coordinated trajectory planning cycle */
          tpRunCycle(&queue);

          /* set new commanded traj pos */
          emcmotStatus->pos = tpGetPos(&queue);

          /* convert to joints */
          kinematicsInverse(&emcmotStatus->pos, coarseJointPos, &iflags, &fflags);

          /* spline joints up-- note that we may be adding points that
             fail soft limits, but we'll abort at the end of this cycle
             so it doesn't really matter */
          for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
            cubicAddPoint(&cubic[axis], coarseJointPos[axis]);
          }

          /* call forward kinematics on input points for actual pos,
             at trajectory rate to save bandwidth */
          /* FIXME */
          if (EMCMOT_NO_FORWARD_KINEMATICS) {
            /* fake it by setting actual pos to commanded pos */
            emcmotStatus->actualPos = emcmotStatus->pos;
          }
          else {
            kinematicsForward(emcmotStatus->input, &emcmotStatus->actualPos, &fflags, &iflags);
          }

          /* now emcmotStatus->actualPos, emcmotStatus->pos, and
             coarseJointPos[] are set */

        } /* end of: coord mode */
        else {
          /* we're in free mode-- run joint planning cycles */
          for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
            /* set whichCycle to be a joint trajectory cycle,
               for calc time logging */
            /* note that this may include one or more joint trajectory
               cycles, so calc time may be inherently variable */
            whichCycle = 2;

            /* run joint trajectory planning cycle */
            tpRunCycle(&freeAxis[axis]);

            /* set new coarse joint position. FIXME-- this uses only
               the tran.x field of the TP_STRUCT, which is overkill.
               We need a TP_STRUCT with a single scalar element. */
            coarseJointPos[axis] = tpGetPos(&freeAxis[axis]).tran.x;

            /* spline joint up-- note that we may be adding a point that
               fails soft limit, but we'll abort at the end of this cycle
               so it doesn't really matter */
            cubicAddPoint(&cubic[axis], coarseJointPos[axis]);

          } /* end of: axis for loop for joint planning cycle */

          if (EMCMOT_NO_FORWARD_KINEMATICS) {
            /* can't do anything */
          }
          else {
            /* call forward kinematics on input points for actual pos,
               at trajectory rate to save bandwidth */
            kinematicsForward(emcmotStatus->input, &emcmotStatus->actualPos, &fflags, &iflags);

            /* push coarseJointPos[] through the forward kins to
             set emcmotStatus->pos, which are not computed in free
             mode as they are in coord mode */
          /* Note: in systems where the forward kins are costly to compute,
             emcmotStatus->pos can be set to emcmotStatus->actualPos to
             save one call to the forward kins */
            kinematicsForward(coarseJointPos, &emcmotStatus->pos, &fflags, &iflags);
          }

          /* now emcmotStatus->actualPos, emcmotStatus->pos, and
             coarseJointPos[] are set */

        } /* end of: free mode trajectory planning */
      } /* end of: while (cubicNeedNextPoint(0)) */

      /* we're still in motion enabled section. For coordinated mode,
         the Cartesian trajectory cycle has been computed, if necessary,
         run through the inverse kinematics, and the joints have been
         splined up for interpolation. For free mode, the joint trajectory
         cycles have been computed, if necessary, and the joints have
         been splined up for interpolation. We still need to push the
         actual input through the forward kinematics, for actual pos.

         Effects:

         For coord mode, emcmotStatus->pos contains the commanded
         Cartesian pose, coarseJointPos[] contains the results of the
         inverse kinematics at the coarse (trajectory) rate, and the
         interpolators are not empty.

         For free mode, emcmotStatus->pos is unchanged, and needs to
         be updated via the forward kinematics. FIXME-- make sure this
         happens, and note where in this comment. coarseJointPos[]
         contains the results of the joint trajectory calculations
         at the coarse (trajectory) rate, and the interpolators are
         not empty.
      */

      /* check for soft joint limits. If so, abort all motion. The
         interpolators will pick this up further down and begin
         planning abort and stop. */
      onLimit = 0;
      for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
        SET_AXIS_PSL_FLAG(axis, 0);
        SET_AXIS_NSL_FLAG(axis, 0);
        if (GET_AXIS_HOMED_FLAG(axis)) {
          if (coarseJointPos[axis] > emcmotConfig->maxLimit[axis]) {
            SET_AXIS_ERROR_FLAG(axis, 1);
            SET_AXIS_PSL_FLAG(axis, 1);
            onLimit = 1;
          }
          else if (coarseJointPos[axis] < emcmotConfig->minLimit[axis]) {
            SET_AXIS_ERROR_FLAG(axis, 1);
            SET_AXIS_NSL_FLAG(axis, 1);
          }
        }
      }

      /* reset wasOnLimit flag iff all joints are free of
       soft limits, as seen in the flag bits set last cycle.
       No need to do this for hard limits, since wasOnLimit
       only prevents flurry of aborts while on a soft limit
       and hard limits don't abort, they disable. */
      if (onLimit) {
        if (! wasOnLimit) {
          /* abort everything, regardless of coord or free mode */
          tpAbort(&queue);
          for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
            tpAbort(&freeAxis[axis]);
            wasOnLimit = 1;
          }
        }
        /* else we were on a limit, so inhibit firing of aborts */
      }
      else {
        /* not on a limit, so clear wasOnLimit so aborts fire next
           time we are on a limit */
        wasOnLimit = 0;
      }

      if (whichCycle == 2) {
        /* we're on a trajectory cycle, either Cartesian or joint planning, so
           log per-traj-cycle data if logging active */
        logIt = 0;

        if (emcmotStatus->logStarted &&
            emcmotStatus->logSkip >= 0) {

          /* calculate current vel, accel, for logging */
          newVel.tran.x = (emcmotStatus->pos.tran.x - oldPos.tran.x) /
            emcmotConfig->trajCycleTime;
          newVel.tran.y = (emcmotStatus->pos.tran.y - oldPos.tran.y) /
            emcmotConfig->trajCycleTime;
          newVel.tran.z = (emcmotStatus->pos.tran.z - oldPos.tran.z) /
            emcmotConfig->trajCycleTime;
          oldPos = emcmotStatus->pos;
          newAcc.tran.x = (newVel.tran.x - oldVel.tran.x) /
            emcmotConfig->trajCycleTime;
          newAcc.tran.y = (newVel.tran.y - oldVel.tran.y) /
            emcmotConfig->trajCycleTime;
          newAcc.tran.z = (newVel.tran.z - oldVel.tran.z) /
            emcmotConfig->trajCycleTime;
          oldVel = newVel;

          /* 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);
              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: if (emcmotStatus->logStarted &&
                         emcmotStatus->logSkip >= 0) */
      } /* end of: if (whichCycle == 2), for trajectory cycle logging */

      /* run interpolation and compensation */
      for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
        /* interpolate */
        oldJointPos[axis] = jointPos[axis];
        jointPos[axis] = cubicInterpolate(&cubic[axis], 0, 0, 0, 0);
        /* round position to input resolution */
        numres = jointPos[axis] * emcmotStatus->inputScale[axis];
        if (numres < 0.0) {
          jointPos[axis] = inverseInputScale[axis] * ((int) (numres - 0.5));
        }
        else {
          jointPos[axis] = inverseInputScale[axis] * ((int) (numres + 0.5));
        }
        jointVel[axis] = (jointPos[axis] - oldJointPos[axis]) /
          emcmotConfig->servoCycleTime;

        /* run output calculations */
        if (GET_AXIS_ACTIVE_FLAG(axis)) {
#ifdef STEPPER_MOTORS
          /* backlash compensation for steppers*/
          /* This is pretty screwed up, since backlash was originally
             part of the PID structure. We need to duplicate the
             correction method here, and use the backlash value from
             the PID structure since that's where it ends up.
             FIXME-- make backlash part of the EMC status proper, not
             the PID structure */
          if (jointPos[axis] - oldJointPos[axis] > 0.0) {
            oldbcomp = bcomp[axis];
            bcomp[axis] = + emcmotConfig->pid[axis].backlash / 2.0;
            if (bcompdir[axis] != + 1) {
              bac_done[axis] = 0;
              bac_di[axis] = bcompincr[axis];
              bac_d[axis] = 0;
              bac_D[axis] = bcomp[axis] - oldbcomp;
              bac_halfD[axis] = 0.5 * bac_D[axis];
              bac_incrincr[axis] = emcmotStatus->acc *
                emcmotConfig->servoCycleTime * emcmotConfig->servoCycleTime;
              bac_incr[axis] = - 0.5 * bac_incrincr[axis];
              bcompdir[axis] = + 1;
            }
          }
          else if (jointPos[axis] - oldJointPos[axis] < 0.0) {
            oldbcomp = bcomp[axis];
            bcomp[axis] = - emcmotConfig->pid[axis].backlash / 2.0;
            if (bcompdir[axis] != - 1) {
              bac_done[axis] = 0;
              bac_di[axis] = bcompincr[axis];
              bac_d[axis] = 0;
              bac_D[axis] = bcomp[axis] - oldbcomp;
              bac_halfD[axis] = 0.5 * bac_D[axis];
              bac_incrincr[axis] = emcmotStatus->acc *
                emcmotConfig->servoCycleTime * emcmotConfig->servoCycleTime;
              bac_incr[axis] = - 0.5 * bac_incrincr[axis];
              bcompdir[axis] = - 1;
            }
          }
          /* else no motion, so leave bcomp what it was */

          /* mete out the backlash according to an acc/dec profile */
          if (! bac_done[axis]) {
            if (bac_D[axis] > 0.0) {
              bcompincr[axis] = bac_di[axis] + bac_d[axis];
              if (bac_d[axis] < bac_halfD[axis]) {
                bac_incr[axis] += bac_incrincr[axis];
                bac_d[axis] += bac_incr[axis];
              }
              else if (bac_d[axis] < bac_D[axis]) {
                bac_incr[axis] -= bac_incrincr[axis];
                bac_d[axis] += bac_incr[axis];
              }
              if (bac_d[axis] >= bac_D[axis] || bac_incr[axis] <= 0.0) {
                bac_done[axis] = 1;
              }
            }
            else {
              bcompincr[axis] = bac_di[axis] + bac_d[axis];
              if (bac_d[axis] > bac_halfD[axis]) {
                bac_incr[axis] += bac_incrincr[axis];
                bac_d[axis] -= bac_incr[axis];
              }
              else if (bac_d[axis] > bac_D[axis]) {
                bac_incr[axis] -= bac_incrincr[axis];
                bac_d[axis] -= bac_incr[axis];
              }
              if (bac_d[axis] <= bac_D[axis] || bac_incr[axis] <= 0.0) {
                bac_done[axis] = 1;
              }
            }
          } /* end of: if not bac_done[axis] */

          /* fill in global smSteps[] for stepper task */
          /* first get steps, as a floating point */
          dsteps = (jointPos[axis] + bcompincr[axis]) *
            smStepsPerUnit[axis] + emcmotStatus->inputOffset[axis];
          /* round to nearest integer */
          if (dsteps < 0.0) {
            smSteps[axis] = (int) (dsteps - 0.5);
          }
          else {
            smSteps[axis] = (int) (dsteps + 0.5);
          }

          /* flag this as the start of a new chunk for the stepper task */
          smNewCycle[axis] = 1;
#else
          /* 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(&emcmotConfig->pid[axis],
                        emcmotStatus->input[axis],
                        jointPos[axis]);
#endif

          /* COMPUTE FOLLOWING ERROR: */

          /* compute signed following error and magnitude */
          thisFerror[axis] = jointPos[axis] - emcmotStatus->input[axis];
          emcmotDebug->ferrorCurrent[axis] = thisFerror[axis];
          magFerror = fabs(thisFerror[axis]);

          /* record the max ferror for this axis */
          if (emcmotDebug->ferrorHighMark[axis] < magFerror) {
            emcmotDebug->ferrorHighMark[axis] = magFerror;
          }

          /* compute the scaled ferror for a move of this speed */
          limitFerror = emcmotConfig->maxFerror[axis] /
            emcmotConfig->limitVel *
            jointVel[axis];
          if (limitFerror < emcmotConfig->minFerror[axis]) {
            limitFerror = emcmotConfig->minFerror[axis];
          }

          /* abort if this ferror is greater than the scaled ferror */
          if (magFerror > limitFerror) {
            /* 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);
          }
        } /* end of: if (GET_AXIS_ACTIVE_FLAG(axis)) */
        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
          'emcmotConfig->minOutput[] .. emcmotConfig->maxOutput[]'
        */
        if (emcmotStatus->output[axis] < emcmotConfig->minOutput[axis]) {
          emcmotStatus->output[axis] = emcmotConfig->minOutput[axis];
        }
        else if (emcmotStatus->output[axis] > emcmotConfig->maxOutput[axis]) {
          emcmotStatus->output[axis] = emcmotConfig->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) ||
              GET_AXIS_PHL_FLAG(axis) ||
              GET_AXIS_NHL_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[] */
              finalHome[axis] = 0;
              tpAbort(&freeAxis[axis]);
            }
          } /* end of: if (GET_AXIS_HOME_SWITCH_FLAG(axis)) */
        }/* end of: if (waitingForLatch[axis]) */
      } /* end of: for (axis = 0; ...) */
    } /* end of: if (GET_MOTION_ENABLE_FLAG()) */
    else {
      /* we're not enabled, so no motion planning or interpolation has
         been done. jointPos[] is set to emcmotStatus->input[], and
         likewise with coarseJointPos[], which is normally updated at
         the traj rate but it's convenient to do them here at the same
         time at the servo rate. emcmotStatus->pos, ->actualPos need to
         be run through forward kinematics.
         Note that we are running at the servo rate, so we may need
         to slow down by the interpolation factor to avoid soaking
         the CPU. If we were enabled, ->pos was set by calcs (coord mode)
         or forward kins (free mode), and ->actualPos was set by forward
         kins on ->input[], all at the trajectory rate. */
      for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
        coarseJointPos[axis] = emcmotStatus->input[axis];
        jointPos[axis] = coarseJointPos[axis];
      }
      /* synthesize the trajectory interpolation, via a counter that
         decrements from the interpolation rate. This causes the statements
         to execute at the trajectory rate instead of the servo rate at which
         this enclosing code is called. */
      if (--interpolationCounter <= 0) {
        if (! EMCMOT_NO_FORWARD_KINEMATICS) {
          /* call the forward kinematics, at the effective trajectory rate */
          if (allHomed ||
              kinType == KINEMATICS_IDENTITY) {
            kinematicsForward(emcmotStatus->input, &emcmotStatus->actualPos, &fflags, &iflags);
          }
          emcmotStatus->pos = emcmotStatus->actualPos;
        }
        /* reload the interpolation counter that simulates the interpolation
           done when enabled */
        interpolationCounter = emcmotConfig->interpolationRate;
      }
    }

#ifdef ENABLE_PROBING
    extDioRead(emcmotConfig->probeIndex, &emcmotStatus->probeval);
    if (emcmotStatus->probing && emcmotStatus->probeTripped) {
      tpClear(&queue);
      emcmotStatus->probing = 0;
    }
    else if (emcmotStatus->probing && GET_MOTION_INPOS_FLAG() &&
             tpQueueDepth(&queue) == 0) {
      emcmotStatus->probing = 0;
    }
    else if (emcmotStatus->probing) {
      if (emcmotStatus->probeval == emcmotConfig->probePolarity) {
        emcmotStatus->probeTripped = 1;
        emcmotStatus->probedPos = emcmotStatus->actualPos;
        if (GET_MOTION_COORD_FLAG()) {
          tpAbort(&queue);
          SET_MOTION_ERROR_FLAG(0);
        }
        else {
          /* check axis range */
          for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
            tpAbort(&freeAxis[axis]);
            SET_AXIS_HOMING_FLAG(axis, 0);
            SET_AXIS_ERROR_FLAG(axis, 0);
          }
        }
      }
    }
#endif /* ENABLE_PROBING */

#ifndef STEPPER_MOTORS

    /* SCALE OUTPUTS: */

    for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
      rawOutput[axis] =
        (emcmotStatus->output[axis] - emcmotStatus->outputOffset[axis]) *
        inverseOutputScale[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: */

    /* copy computed axis positions to status. Note that if motion is
       disabled, this is the same as ->input[]. */
    for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
      emcmotStatus->axisPos[axis] = jointPos[axis];
    }

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

    /* motion queue status */
    emcmotStatus->depth = tpQueueDepth(&queue);
    emcmotStatus->activeDepth = tpActiveDepth(&queue);
    emcmotStatus->id = tpGetExecId(&queue);
    emcmotStatus->queueFull = tcqFull(&queue.queue);
    SET_MOTION_INPOS_FLAG(0);
    if (tpIsDone(&queue)) {
      SET_MOTION_INPOS_FLAG(1);
    }

    /* axis status */
    for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
      SET_AXIS_INPOS_FLAG(axis, 0);
      if (tpIsDone(&freeAxis[axis])) {
        SET_AXIS_INPOS_FLAG(axis, 1);
      }
      else {
        /* this axis, at least, is moving, so set overriding flag */
        if (emcmotStatus->overrideLimits) {
          overriding = 1;
        }
      }
    }

    /* reset overrideLimits flag if we have started a move and now
       are in position */
    for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
      if (! GET_AXIS_INPOS_FLAG(axis)) {
        break;
      }
    }
    if (axis == EMCMOT_MAX_AXIS) {
      /* ran through all axes, and all are in position */
      if (overriding) {
        overriding = 0;
        emcmotStatus->overrideLimits = 0;
      }
    }

    /* check to see if we should pause in order to implement
       single stepping */
    if (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(&emcmotDebug->tMmxavg, delta);
      emcmotDebug->tMin = mmxavgMin(&emcmotDebug->tMmxavg);
      emcmotDebug->tMax = mmxavgMax(&emcmotDebug->tMmxavg);
      emcmotDebug->tAvg = mmxavgAvg(&emcmotDebug->tMmxavg);
    }
    else if (1 == whichCycle) {
      /* servo calcs only this cycle-- use sMin,Max,Avg */
      mmxavgAdd(&emcmotDebug->sMmxavg, delta);
      emcmotDebug->sMin = mmxavgMin(&emcmotDebug->sMmxavg);
      emcmotDebug->sMax = mmxavgMax(&emcmotDebug->sMmxavg);
      emcmotDebug->sAvg = mmxavgAvg(&emcmotDebug->sMmxavg);
    }
    else {
      /* calcs disabled this cycle-- use nMin,Max,Avg */
      mmxavgAdd(&emcmotDebug->nMmxavg, delta);
      emcmotDebug->nMin = mmxavgMin(&emcmotDebug->nMmxavg);
      emcmotDebug->nMax = mmxavgMax(&emcmotDebug->nMmxavg);
      emcmotDebug->nAvg = mmxavgAvg(&emcmotDebug->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;

      case EMCMOT_LOG_TYPE_POS_VOLTAGE:
        /* don't do a circular log-- suppress if full */
        if (emcmotLog->howmany >= emcmotStatus->logSize) {
          emcmotStatus->output[loggingAxis] = 0.0; /* drop the DAC to zero */
          emcmotStatus->logStarted = 0; /* stop logging */
          logIt = 0;            /* should still be zero, reset anyway */
          break;
        }
        if (logSkip-- <= 0) {
          ls.item.posVoltage.time = end;
          for (axis = 0;
               axis < EMCMOT_LOG_NUM_AXES &&
                 axis < EMCMOT_MAX_AXIS;
               axis++) {
            ls.item.posVoltage.pos = emcmotStatus->input[loggingAxis];
            ls.item.posVoltage.voltage = rawOutput[loggingAxis];
          }
          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;
    emcmotDebug->tail = emcmotDebug->head;
    emcmotConfig->tail = emcmotConfig->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 */
#ifdef USE_RTL2
    pthread_wait_np();
#else
    rt_task_wait();
#endif
  } /* end of: forever loop for RT task */
#endif /* rtlinux */
#ifdef USE_RTL2
  return(NULL);
#endif
} /* end of: emcmotController() function */

#ifdef STEPPER_MOTORS

/*
  smController runs the stepper motor pulse task calculations at a
  relatively high frequency. The time interval between calls to this
  task is governed by the maximum speed ([TRAJ] MAX_VELOCITY) and the
  max pulses-per-unit among all motors ([AXIS_#] INPUT_SCALE). The task
  runs at twice this time since it must issue both down pulses and up
  pulses, and can only do one per motor per call.

  For each motor, the task looks at the commanded position, in integer
  counts, as stored in the global smSteps[]. This is set by the
  emcmotController task. If the accumulated number of steps is greater
  (or less) than half a pulse, a pulse is sent in the proper direction and
  the accumulated count is incremented (or decremented).

  The timing of this task does not vary with the speed of each motor.

  The parallel port is used for outputs, using the pptDioByteWrite()
  function. Four axes (step and direction) are put on the low byte, and
  the remaining two axes are put on the high byte.
*/

#define _TESTING
#ifdef USE_RTL2
static void *smController(void *arg)
#else
static void smController(int arg)
#endif
{
  static unsigned char oldSmLoByte = 0, oldSmHiByte = 0;
  static int smCount[EMCMOT_MAX_AXIS];
  static int up[EMCMOT_MAX_AXIS];
  static int down[EMCMOT_MAX_AXIS];
  static int direction[EMCMOT_MAX_AXIS];
#ifdef TESTING
  static char toggle = 0;
#endif
  int axis;
  unsigned char smLoByte = 0, smHiByte = 0;
  int delta;
  int period;

#ifndef STANDALONE
  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
    smCount[axis] = 0;
    up[axis] = 0;
    down[axis] = 0;
    direction[axis] = 1;
  }

  for (;;) {
#endif
    smLoByte = oldSmLoByte;
    smHiByte = oldSmHiByte;

    for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {

      if (smNewCycle[axis]) {
        smCount[axis] = smCyclesPerAxis;
        smNewCycle[axis] = 0;
      }

      if (up[axis] > 0) {
        up[axis]--;
        if (direction[axis]) {
          smLoByte |= (0x01 << (axis << 1)); /* dir */
          smLoByte |= (0x02 << (axis << 1)); /* clk */
        }
        else {
          smLoByte &= ~(0x01 << (axis << 1)); /* dir */
          smLoByte |= (0x02 << (axis << 1)); /* clk */
        }
      }
      else if (down[axis] > 0) {
        down[axis]--;
        smLoByte &= ~(0x02 << (axis << 1)); /* clk */
      }
      else {
        delta = smSteps[axis] - emcmotDebug->stepperCount[axis];
        if (delta > 0) {
          direction[axis] = 1;
          period = smCount[axis] / +delta;
          up[axis] = period / 2;
          if (up[axis] <= 0) up[axis] = 1;
          down[axis] = period - up[axis];
          if (down[axis] <= 0) down[axis] = 1;
          smLoByte |= (0x01 << (axis << 1)); /* dir */
          smLoByte |= (0x02 << (axis << 1)); /* clk */
          emcmotDebug->stepperCount[axis]++;
          up[axis]--;
        }
        else if (delta < 0) {
          direction[axis] = 0;
          period = smCount[axis] / -delta;
          up[axis] = period / 2;
          if (up[axis] <= 0) up[axis] = 1;
          down[axis] = period - up[axis];
          if (down[axis] <= 0) down[axis] = 1;
          smLoByte &= ~(0x01 << (axis << 1)); /* dir */
          smLoByte |= (0x02 << (axis << 1)); /* clk */
          emcmotDebug->stepperCount[axis]--;
          up[axis]--;
        }
        else {
          /* no delta, so output a down */
          smLoByte &= ~(0x02 << (axis << 1)); /* clk */
        }
      }

      smCount[axis]--;
    }

#ifndef STANDALONE
#ifdef TESTING
    /* channel 2 is sm task trigger */
    if (toggle) {
      smLoByte |= 0x0C;
    }
    else {
      smLoByte &= ~0x0C;
    }
    toggle = ! toggle;
#endif
    /* suppress redundant writes */
    if (smLoByte != oldSmLoByte) {
      pptDioByteWrite(0, smLoByte);
      oldSmLoByte = smLoByte;
    }
    if (smHiByte != oldSmHiByte) {
      pptDioByteWrite(1, smHiByte);
      oldSmHiByte = smHiByte;
    }
#else
    pptDioByteWrite(0, smLoByte);
#endif

#ifndef STANDALONE
#ifdef USE_RTL2
    pthread_wait_np();
#else
    rt_task_wait();
#endif
  }
#endif
#ifdef USE_RTL2
  return(NULL);
#endif

}

#endif /* ifdef STEPPER_MOTORS */

/* fix for scripts that pass PERIOD to insmod; we don't use it here but it
   will keep insmod from complaining */
#ifndef __GNUC__
#ifndef __attribute__
#define __attribute__(x)
#endif
#endif

static int __attribute__((unused)) PERIOD = 1;

/* STEPPING_TYPE is unused here, but provided for compatibility with
   scripts that run this code and pass this as a parameter */
static int __attribute__((unused)) STEPPING_TYPE = 0;

#if defined(rtlinux2)
/* register symbols to be modified by insmod */
MODULE_PARM(PERIOD, "i");
MODULE_PARM(SHMEM_BASE_ADDRESS, "l");
MODULE_PARM(STG_BASE_ADDRESS, "h");
MODULE_PARM(PARPORT_IO_ADDRESS, "l");
MODULE_PARM(EMCMOT_NO_FORWARD_KINEMATICS, "i");
MODULE_PARM(STEPPING_TYPE, "i");
#endif

int init_module(void)
{
  int axis;
  int t;
#ifdef rtlinux
#ifdef USE_RTL2
  hrtime_t now;
  pthread_attr_t attr;
  struct sched_param sched_param;
  clockid_t local_clock;
#else
  RTIME now;
#endif
#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;
  emcmotConfig = &emcmotConfigBuffer;
  emcmotDebug = &emcmotDebugBuffer;

#else  /* not RT_FIFO */


#ifdef USE_RTL2
  /* set shared memory pointer using mbuff which does not require that memory be resevered in lilo.conf */
  printk(KERN_INFO "calling mbuff_alloc(%s,%d)\n",
         "emcmotStruct",
         (int) sizeof(EMCMOT_STRUCT));
  emcmotStruct = (EMCMOT_STRUCT *) mbuff_alloc("emcmotStruct",sizeof(EMCMOT_STRUCT));
  printk(KERN_INFO "mbuff_alloc returned %d\n",((int) emcmotStruct));

#else
#ifdef rtlinux2
  /* set shared memory pointer using ioremap for 2.1+ kernels */
  emcmotStruct = (EMCMOT_STRUCT *) ioremap(SHMEM_BASE_ADDRESS, sizeof(EMCMOT_STRUCT));

#else
  /* set shared memory pointer */
  emcmotStruct = (EMCMOT_STRUCT *) vremap(SHMEM_BASE_ADDRESS, sizeof(EMCMOT_STRUCT));
#endif
#endif

#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;
  emcmotConfig = (EMCMOT_CONFIG *) &emcmotStruct->config;
  emcmotDebug = (EMCMOT_DEBUG *) &emcmotStruct->debug;
  emcmotError = (EMCMOT_ERROR *) &emcmotStruct->error;
  emcmotLog = (EMCMOT_LOG *) &emcmotStruct->log;
  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
    emcmotComp[axis] = (EMCMOT_COMP *) &emcmotStruct->comp[axis];
  }

  /* 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;
    ampFaultCount[axis] = 0;
#ifdef STEPPER_MOTORS
    smStepsPerUnit[axis] = INPUT_SCALE;
    smMagUnitsPerStep[axis] = fabs(1.0 / INPUT_SCALE);
    emcmotDebug->stepperCount[axis] = 0;
    smNewCycle[axis] = 1;
#endif
  }

#ifndef RT_FIFO
  /* init compensation struct */
  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
    emcmotComp[axis]->total = 0;
    emcmotComp[axis]->alter = 0.0;
    /* leave out the avgint, nominal, forward, and reverse, since
       these can't be zero and the total flag prevents their use anyway */
  }

  /* 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;
  emcmotConfig->head = 0;
  emcmotDebug->head = 0;

  emcmotStatus->motionFlag = 0;
  SET_MOTION_ERROR_FLAG(0);
  SET_MOTION_COORD_FLAG(0);
  emcmotDebug->split = 0;
  emcmotStatus->commandEcho = 0;
  emcmotStatus->commandNumEcho = 0;
  emcmotStatus->commandStatus = 0;
  emcmotStatus->heartbeat = 0;
  emcmotStatus->computeTime = 0.0;
  emcmotConfig->numAxes = EMCMOT_MAX_AXIS;
  emcmotConfig->trajCycleTime = TRAJ_CYCLE_TIME;
  emcmotConfig->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;
  emcmotConfig->limitVel = VELOCITY;
  emcmotStatus->acc = ACCELERATION;
  emcmotStatus->qVscale = 1.0;
  emcmotStatus->id = 0;
  emcmotStatus->depth = 0;
  emcmotStatus->activeDepth = 0;
  emcmotStatus->paused = 0;
  emcmotStatus->overrideLimits = 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++) {
    emcmotConfig->homingVel[axis] = VELOCITY;
    emcmotConfig->homeOffset[axis] = 0.0;
    emcmotStatus->axisFlag[axis] = 0;
    emcmotConfig->maxLimit[axis] = MAX_LIMIT;
    emcmotConfig->minLimit[axis] = MIN_LIMIT;
    emcmotConfig->maxOutput[axis] = MAX_OUTPUT;
    emcmotConfig->minOutput[axis] = MIN_OUTPUT;
    emcmotConfig->minFerror[axis] = 0.0; /* gives a true linear ferror */
    emcmotConfig->maxFerror[axis] = MAX_FERROR;
    emcmotDebug->ferrorCurrent[axis] = 0.0;
    emcmotDebug->ferrorHighMark[axis] = 0.0;
    emcmotStatus->outputScale[axis] = OUTPUT_SCALE;
    emcmotStatus->outputOffset[axis] = OUTPUT_OFFSET;
    emcmotStatus->inputScale[axis] = INPUT_SCALE;
    inverseInputScale[axis] = 1.0 / INPUT_SCALE;
    emcmotStatus->inputOffset[axis] = INPUT_OFFSET;
    inverseOutputScale[axis] = 1.0 / OUTPUT_SCALE;
    emcmotStatus->axVscale[axis] = 1.0;
    SET_AXIS_ENABLE_FLAG(axis, 0);
    SET_AXIS_ACTIVE_FLAG(axis, 0); /* default is not to use it; need an
                                      explicit activate */
    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);
    emcmotConfig->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(&emcmotDebug->tMmxavg, emcmotDebug->tMmxavgSpace, MMXAVG_SIZE);
  mmxavgInit(&emcmotDebug->sMmxavg, emcmotDebug->sMmxavgSpace, MMXAVG_SIZE);
  mmxavgInit(&emcmotDebug->nMmxavg, emcmotDebug->nMmxavgSpace, MMXAVG_SIZE);
  emcmotDebug->tMin = 0.0;
  emcmotDebug->tMax = 0.0;
  emcmotDebug->tAvg = 0.0;
  emcmotDebug->sMin = 0.0;
  emcmotDebug->sMax = 0.0;
  emcmotDebug->sAvg = 0.0;
  emcmotDebug->nMin = 0.0;
  emcmotDebug->nMax = 0.0;
  emcmotDebug->nAvg = 0.0;

  /* init motion queue */
  if (-1 == tpCreate(&queue, DEFAULT_TC_QUEUE_SIZE, queueTcSpace)) {
    diagnostics("can't create motion queue\n");
    return -1;
  }
  tpInit(&queue);
  tpSetCycleTime(&queue, emcmotConfig->trajCycleTime);
  tpSetPos(&queue, emcmotStatus->pos);
  tpSetVmax(&queue, emcmotStatus->vel);
  tpSetAmax(&queue, emcmotStatus->acc);

  /* 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], emcmotConfig->trajCycleTime);
    /* freePose is inited to 0's in decl */
    tpSetPos(&freeAxis[axis], freePose);
    tpSetVmax(&freeAxis[axis], emcmotStatus->vel);
    tpSetAmax(&freeAxis[axis], emcmotStatus->acc);
#ifndef STEPPER_MOTORS
    pidInit(&emcmotConfig->pid[axis]);
    pidSetGains(&emcmotConfig->pid[axis], pid);
#endif
    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;
  }

  /* record the kinematics type of the machine */
  kinType = kinematicsType();

  extEncoderSetIndexModel(EXT_ENCODER_INDEX_MODEL_MANUAL);
  for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
    rawInput[axis] = 0.0;
    rawOutput[axis] = 0.0;
    coarseJointPos[axis] = 0.0;
    jointPos[axis] = 0.0;
    jointVel[axis] = 0.0;
    emcmotStatus->axisPos[axis] = 0.0;
    oldJointPos[axis] = 0.0;

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

    emcmotStatus->input[axis] = 0.0;
    oldInput[axis] = 0.0;
    emcmotStatus->output[axis] = 0.0;

    jointHome[axis] = 0.0;

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

  emcmotStatus->tail = 0;

#ifdef rtlinux


#ifdef USE_RTL2
  local_clock = rtl_getschedclock();

#if 0
  if(!rtl_setclockmode(local_clock, RTL_CLOCK_MODE_PERIODIC, 20000))
    {
    diagnostics("can't set periodic mode\n");
  }
#else
  rtl_setclockmode(local_clock, RTL_CLOCK_MODE_ONESHOT,0);
#endif

#else // RTL before version 2
#if 0
  if (RT_TIME_END == rtl_set_periodic_mode(20)) {
    diagnostics("can't set periodic mode\n");
  }
#else
  rtl_set_oneshot_mode();
#endif

#endif

#ifdef USE_RTL2
  pthread_attr_init (&attr);
  sched_param.sched_priority = RT_TASK_PRIORITY;
  pthread_attr_setschedparam (&attr, &sched_param);
  attr.use_fp = 1;
  attr.stack_size = RT_TASK_STACK_SIZE;
  pthread_create( &emcmotTask, &attr, emcmotController, (void *) 1);

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

  pthread_setfp_np(emcmotTask,1);
  pthread_make_periodic_np(emcmotTask,
                           now + 0.010*HRTICKS_PER_SEC,
                           (hrtime_t) (HRTICKS_PER_SEC *
                                       emcmotConfig->servoCycleTime));
#else

  /* 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 */
  rt_task_use_fp(&emcmotTask, 1);

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

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


#endif



#ifdef STEPPER_MOTORS


  /* schedule stepper task to run at 1/(V*P), where V is max velocity
     and P is the max stepper pulses per unit among all axes, i.e.,
     inputScale. Note that this is also done whenever limit vel or input
     scales are changed */
  smVPmax = VELOCITY * INPUT_SCALE; /* use defaults at first */
  smMaxStepsPerUnit = INPUT_SCALE;
  /* set the period to twice the max pulse rate. It need only
     be the same as the pulse rate, but this makes it the same
     as before */
  smCycleTime = 0.5/smVPmax;
  smCyclesPerAxis = (int) (emcmotConfig->servoCycleTime / smCycleTime);


#ifdef USE_RTL2
  pthread_attr_init (&attr);
  sched_param.sched_priority = SM_TASK_PRIORITY;
  pthread_attr_setschedparam (&attr, &sched_param);
  attr.use_fp = 1;
  attr.stack_size = SM_TASK_STACK_SIZE;
  pthread_create( &smTask, &attr, smController, NULL);

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

  pthread_setfp_np(emcmotTask,1);
  pthread_make_periodic_np(emcmotTask,
                           now + 0.010*HRTICKS_PER_SEC,
                           (hrtime_t) (HRTICKS_PER_SEC *
                                       smCycleTime));

#else

  /* 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 */
  rt_task_use_fp(&smTask, 1);

  rt_task_make_periodic(&smTask,
                        now + SECS_TO_RTIME(0.010),
                        ((RTIME) (RT_TICKS_PER_SEC * smCycleTime)));

#endif
#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 */
  tpDelete(&queue);

  /* 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 */
#ifdef USE_RTL2
  pthread_delete_np(emcmotTask);
#else
  rt_task_delete(&emcmotTask);
#endif

#ifdef STEPPER_MOTORS

  /* delete stepper motor task */
#ifdef USE_RTL2
  pthread_delete_np(smTask);
#else
  rt_task_delete(&smTask);
#endif

#endif

  /* free shared memory */
#ifdef USE_RTL2
  mbuff_free("emcmotStruct",emcmotStruct);
#else
#ifdef rtlinux2
  iounmap(emcmotStruct);
#else
  vfree(emcmotStruct);
#endif
#endif


  /* FIXME */
  diagnostics("exited emcmot\n");
#ifdef STEPPER_MOTORS
  diagnostics("sm cycle time %d\n", (int) (smCycleTime * 1000000.0));
  diagnostics("sm cycles per axis %d\n", smCyclesPerAxis);
#endif

#else

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

#endif
}

/* Build a standalone executable with main() if not rtlinux */

#ifndef rtlinux
int emcmot_done = 0;
static void emcmot_quit(int sig)
{
  emcmot_done = 1;
}

#ifndef UNDER_CE
#include <signal.h>             /* signal(), SIGINT */
#endif

#ifndef UNDER_CE
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;
      }
    }

    /* FIXME */
    /* try -noforward */
    if (!strcmp(argv[t], "-noforward")) {
      EMCMOT_NO_FORWARD_KINEMATICS = 1;
      continue;
    }

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

  return 0;
}
#endif


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

  Syntax matches that used with insmod sym=value, for consistency
  */
#ifndef UNDER_CE
int main(int argc, char *argv[])
#else
int emcmotsim()
#endif
{
  double delta;                 /* elapsed time */

#ifndef UNDER_CE
  /* trap ^C */
  signal(SIGINT, emcmot_quit);

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

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

  while (! emcmot_done) {
    delta = etime();
    emcmotController(0);
    delta = etime() - delta;
    delta = emcmotConfig->servoCycleTime - delta;

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

  cleanup_module();
#ifndef UNDER_CE
  exit(0);
#else
  return 0;
#endif
}

#endif /* not rtlinux */

---