motion.h

Source code for an Numeric Cmputer

/********************************************************************
* Description: motion.h
*   Data structures used throughout emc2.
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
* Last change:
* $Revision: 1.48.2.1 $
* $Author: jmkasunich $
* $Date: 2006/05/18 17:41:09 $
********************************************************************/

/* jmk says: This file is a mess! */

/*

Misc ramblings:

The terms axis and joint are used inconsistently throughout EMC.
For all new code, the usages are as follows:

    axis - one of the six degrees of freedom, x, y, z, a, b, c
	these refer to axes in Cartesian space, which may or
	may not match up with joints (see below). On Cartesian
	machines they do match up, but for hexapods, robots, and
	other non-Cartesian machines they don't.
    joint - one of the physical degrees of freedom of the machine
	these might be linear (leadscrews) or rotary (rotary
	tables, robot arm joints).  There can be any number of
	joints.  The kinematics code is responsible for translating
	from axis space to joint space and back.

There are three main kinds of data needed by the motion controller

1) data shared with higher level stuff - commands, status, etc.
2) data that is local to the motion controller
3) data shared with lower level stuff - hal pins

In addition, some internal data (2) should be shared for trouble
shooting purposes, even though it is "internal" to the motion
controller.  Depending on the type of data, it can either be
treated as type (1), and made available to the higher level
code, or it can be treated as type (3), and made available to
the hal, so that halscope can monitor it.

This file should ONLY contain structures and declarations for
type (1) items - those that are shared with higher level code.

Type (2) items should be declared in mot_priv.h, along
with type (3) items.

In the interest of retaining my sanity, I'm not gonna attempt
to move everything to its proper location yet....

However, all new items will be defined in the proper place,
and some existing items may be moved from one struct definition
to another.

*/

/* the following line can be used to control where some of the
   "internal" motion controller data is stored.  By default,
   it is stored in staticlly allocated kernel memory.  However,
   if STRUCTS_IN_SHMEM is defined, it will be stored in the
   emcmotStruct shared memory area, for debugging purposes.
*/

// #define STRUCTS_IN_SHMEM



#ifndef MOTION_H
#define MOTION_H

#include "posemath.h"		/* PmCartesian, PmPose, pmCartMag() */
#include "emcpos.h"		/* EmcPose */
#include "cubic.h"		/* CUBIC_STRUCT, CUBIC_COEFF */
#include "emcmotcfg.h"		/* EMCMOT_MAX_AXIS */
#include "tp.h"			/* TP_STRUCT */
#include "tc.h"			/* TC_STRUCT, TC_QUEUE_STRUCT */
#include "mmxavg.h"		/* MMXAVG_STRUCT */
#include "kinematics.h"

#ifdef __cplusplus
extern "C" {
#endif

    typedef struct _EMC_TELEOP_DATA {
	EmcPose currentVel;
	EmcPose currentAccell;
	EmcPose desiredVel;
	EmcPose desiredAccell;
    } EMC_TELEOP_DATA;

/* This enum lists all the possible commands */

    typedef enum {
	EMCMOT_ABORT = 1,	/* abort all motion */
	EMCMOT_AXIS_ABORT,	/* abort one axis */
	EMCMOT_ENABLE,		/* enable servos for active axes */
	EMCMOT_DISABLE,		/* disable servos for active axes */
	EMCMOT_ENABLE_AMPLIFIER,	/* enable amp outputs */
	EMCMOT_DISABLE_AMPLIFIER,	/* disable amp outputs */
	EMCMOT_ENABLE_WATCHDOG,	/* enable watchdog sound, parport */
	EMCMOT_DISABLE_WATCHDOG,	/* enable watchdog sound, parport */
	EMCMOT_ACTIVATE_JOINT,	/* make axis active */
	EMCMOT_DEACTIVATE_JOINT,	/* make axis inactive */

	EMCMOT_PAUSE,		/* pause motion */
	EMCMOT_RESUME,		/* resume motion */
	EMCMOT_STEP,		/* resume motion until id encountered */
	EMCMOT_FREE,		/* set mode to free (joint) motion */
	EMCMOT_COORD,		/* set mode to coordinated motion */
	EMCMOT_TELEOP,		/* set mode to teleop */

	EMCMOT_SCALE,		/* scale the speed */
	EMCMOT_OVERRIDE_LIMITS,	/* temporarily ignore limits until jog done */

	EMCMOT_HOME,		/* home an axis */
	EMCMOT_JOG_CONT,	/* continuous jog */
	EMCMOT_JOG_INCR,	/* incremental jog */
	EMCMOT_JOG_ABS,		/* absolute jog */
	EMCMOT_SET_LINE,	/* queue up a linear move */
	EMCMOT_SET_CIRCLE,	/* queue up a circular move */
	EMCMOT_SET_TELEOP_VECTOR,	/* Move at a given velocity but in
					   world cartesian coordinates, not
					   in joint space like EMCMOT_JOG_* */

	EMCMOT_CLEAR_PROBE_FLAGS,	/* clears probeTripped flag */
	EMCMOT_PROBE,		/* go to pos, stop if probe trips, record
				   trip pos */

	EMCMOT_SET_POSITION_LIMITS,	/* set the axis position +/- limits */
	EMCMOT_SET_BACKLASH,	/* set the axis backlash */
	EMCMOT_SET_MIN_FERROR,	/* minimum following error, input units */
	EMCMOT_SET_MAX_FERROR,	/* maximum following error, input units */
	EMCMOT_SET_VEL,		/* set the velocity for subsequent moves */
	EMCMOT_SET_VEL_LIMIT,	/* set the max vel for all moves (tooltip) */
	EMCMOT_SET_JOINT_VEL_LIMIT,	/* set the max axis vel */
	EMCMOT_SET_JOINT_ACC_LIMIT,	/* set the max axis accel */
	EMCMOT_SET_ACC,		/* set the max accel for moves (tooltip) */
	EMCMOT_SET_TERM_COND,	/* set termination condition (stop, blend) */
	EMCMOT_SET_NUM_AXES,	/* set the number of axes */
	EMCMOT_SET_WORLD_HOME,	/* set pose for world home */
	EMCMOT_SET_HOMING_PARAMS,	/* sets axis homing parameters */
	EMCMOT_SET_DEBUG,       /* sets the debug level */
	EMCMOT_SET_DOUT,        /* sets or unsets a DIO, this can be imediate or synched with motion */
	EMCMOT_SET_AOUT,	/* sets or unsets a AIO, this can be imediate or synched with motion */
        EMCMOT_SET_SPINDLESYNC, /* syncronize motion to spindle encoder */
    } cmd_code_t;

/* this enum lists the possible results of a command */

    typedef enum {
	EMCMOT_COMMAND_OK = 0,	/* cmd honored */
	EMCMOT_COMMAND_UNKNOWN_COMMAND,	/* cmd not understood */
	EMCMOT_COMMAND_INVALID_COMMAND,	/* cmd can't be handled now */
	EMCMOT_COMMAND_INVALID_PARAMS,	/* bad cmd params */
	EMCMOT_COMMAND_BAD_EXEC	/* error trying to initiate */
    } cmd_status_t;

/* termination conditions for queued motions */
#define EMCMOT_TERM_COND_STOP 1
#define EMCMOT_TERM_COND_BLEND 2

/*********************************
       COMMAND STRUCTURE
*********************************/

/* This is the command structure.  There is one of these in shared
   memory, and all commands from higher level code come thru it.
*/
    typedef struct {
	unsigned char head;	/* flag count for mutex detect */
	cmd_code_t command;	/* command code (enum) */
	int commandNum;		/* increment this for new command */
	double maxLimit;	/* pos value for position limit, output */
	double minLimit;	/* neg value for position limit, output */
	EmcPose pos;		/* line/circle endpt, or teleop vector */
	PmCartesian center;	/* center for circle */
	PmCartesian normal;	/* normal vec for circle */
	int turn;		/* turns for circle */
	double vel;		/* max velocity */
        double ini_maxvel;      /* max velocity allowed by machine
                                   constraints (the ini file) */
        int motion_type;        /* this move is because of traverse, feed, arc, or toolchange */
        double spindlesync;     /* user units per spindle revolution, 0 = no sync */
	double acc;		/* max acceleration */
	double backlash;	/* amount of backlash */
	int id;			/* id for motion */
	int termCond;		/* termination condition */
	double tolerance;	/* tolerance for path deviation in CONTINUOUS mode */
	int axis;		/* which index to use for below */
	double scale;		/* velocity scale arg */
	double offset;		/* input, output, or home offset arg */
	double home;		/* joint home position */
	double search_vel;	/* home search velocity */
	double latch_vel;	/* home latch velocity */
	int flags;		/* homing config flags */
	double minFerror;	/* min following error */
	double maxFerror;	/* max following error */
	int wdWait;		/* cycle to wait before toggling wd */
	int debug;		/* debug level, from DEBUG in .ini file */
	unsigned char now, out, start, end;	/* these are related to synched AOUT/DOUT. now=wether now or synched, out = which gets set, start=start value, end=end value */
	unsigned char tail;	/* flag count for mutex detect */
    } emcmot_command_t;

/*! \todo FIXME - these packed bits might be replaced with chars
   memory is cheap, and being able to access them without those
   damn macros would be nice
*/

/* motion flag type */
    typedef unsigned short EMCMOT_MOTION_FLAG;

/*
  motion status flag structure-- looks like:

  MSB                             LSB
  v---------------v------------------v
  |   |   |   | T | CE | C | IP | EN |
  ^---------------^------------------^

  where:

  EN is 1 if calculations are enabled, 0 if not
  IP is 1 if all axes in position, 0 if not
  C is 1 if coordinated mode, 0 if in free mode
  CE is 1 if coordinated mode error, 0 if not
  T is 1 if we are in teleop mode.
  */

/* bit masks */
#define EMCMOT_MOTION_ENABLE_BIT      0x0001
#define EMCMOT_MOTION_INPOS_BIT       0x0002
#define EMCMOT_MOTION_COORD_BIT       0x0004
#define EMCMOT_MOTION_ERROR_BIT       0x0008
#define EMCMOT_MOTION_TELEOP_BIT      0x0010

/* axis flag type */
    typedef unsigned short EMCMOT_AXIS_FLAG;
/*
  axis status flag structure-- looks like:

  MSB                                                          LSB
  ----------v-----------------v-----------------------v-------------------v
  | AF | FE | AH | HD | H | HS | NHL | PHL | NSL | PSL | ER | IP | AC | EN |
  ----------^-----------------^-----------------------^-------------------^
               

  x = unused

  where:

  EN  is 1 if axis amplifier is enabled, 0 if not
  AC  is 1 if axis is active for calculations, 0 if not
  IP  is 1 if axis is in position, 0 if not (free mode only)
  ER  is 1 if axis has an error, 0 if not

  PSL is 1 if axis is on maximum software limit, 0 if not
  NSL is 1 if axis is on minimum software limit, 0 if not
  PHL is 1 if axis is on maximum hardware limit, 0 if not
  NHL is 1 if axis is on minimum hardware limit, 0 if not

  HS  is 1 if axis home switch is tripped, 0 if not
  H   is 1 if axis is homing, 0 if not
  HD  is 1 if axis has been homed, 0 if not
  AH  is 1 if axis is at home position, 0 if not

  FE  is 1 if axis exceeded following error, 0 if not
  AF  is 1 if amplifier is faulted, 0 if not

Suggestion: Split this in to an Error and a Status flag register..
             Then a simple test on each of the two flags can be performed
             rather than testing each bit... Saving on a global per axis
             fault and ready status flag.
  */

/* bit masks */
#define EMCMOT_AXIS_ENABLE_BIT         0x0001
#define EMCMOT_AXIS_ACTIVE_BIT         0x0002
#define EMCMOT_AXIS_INPOS_BIT          0x0004
#define EMCMOT_AXIS_ERROR_BIT          0x0008

#define EMCMOT_AXIS_MAX_SOFT_LIMIT_BIT 0x0010
#define EMCMOT_AXIS_MIN_SOFT_LIMIT_BIT 0x0020
#define EMCMOT_AXIS_MAX_HARD_LIMIT_BIT 0x0040
#define EMCMOT_AXIS_MIN_HARD_LIMIT_BIT 0x0080

#define EMCMOT_AXIS_HOME_SWITCH_BIT    0x0100
#define EMCMOT_AXIS_HOMING_BIT         0x0200
#define EMCMOT_AXIS_HOMED_BIT          0x0400

/*! \todo FIXME - I'm not sure AT_HOME is being reported correctly.
   AT_HOME is cleared when you jog in free mode, but not if
   you do a coordinated move... perhaps that is the intended
   behavior.
*/
#define EMCMOT_AXIS_AT_HOME_BIT        0x0800

#define EMCMOT_AXIS_FERROR_BIT         0x1000
#define EMCMOT_AXIS_FAULT_BIT          0x2000

/*! \todo FIXME - the terms "teleop", "coord", and "free" are poorly
   documented.  This is my feeble attempt to understand exactly
   what they mean.

   According to Fred, teleop is never used with machine tools,
   although that may not be true for machines with non-trivial
   kinematics.

   "coord", or coordinated mode, means that all the axis are
   synchronized, and move together as commanded by the higher
   level code.  It is the normal mode when machining.  In
   coordinated mode, commands are assumed to be in the cartesean
   reference frame, and if the machine is non-cartesean, the
   commands are translated by the kinematics to drive each
   axis in joint space as needed.

   "free" mode means commands are interpreted in joint space.
   It is used for jogging individual axes (joints), although
   it does not preclude multiple axes moving at once (I think).
   Homing is also done in free mode, in fact machines with
   non-trivial kinematics must be homed before they can go
   into either coord or teleop mode.

   'teleop' is what you probably want if you are 'jogging'
   a hexapod.  The jog commands as implemented by the motion
   controller are joint jogs, which work in free mode.  But
   if you want to jog a hexapod or similar machine along
   one particular cartesean axis, you need to operate more
   than one joint.  That's what 'teleop' is for.

*/

/* compensation structure */
#define EMCMOT_COMP_SIZE 256
    typedef struct {
	int total;		/* how many comp points */
	double avgint;		/* average interval between points */
	double nominal[EMCMOT_COMP_SIZE];	/* nominal points */
	double forward[EMCMOT_COMP_SIZE];	/* forward comp points */
	double reverse[EMCMOT_COMP_SIZE];	/* reverse comp points */
	double alter;		/* additive dynamic compensation */
    } emcmot_comp_t;

/* motion controller states */

    typedef enum {
	EMCMOT_MOTION_DISABLED = 0,
	EMCMOT_MOTION_FREE,
	EMCMOT_MOTION_TELEOP,
	EMCMOT_MOTION_COORD
    } motion_state_t;

/* states for homing */
    typedef enum {
	HOME_IDLE = 0,
	HOME_START,		// 1
	HOME_INITIAL_BACKOFF_START,	// 2
	HOME_INITIAL_BACKOFF_WAIT,	// 3
	HOME_INITIAL_SEARCH_START,	// 4
	HOME_INITIAL_SEARCH_WAIT,	// 5
	HOME_SET_COARSE_POSITION,	// 6
	HOME_FINAL_BACKOFF_START,	// 7
	HOME_FINAL_BACKOFF_WAIT,	// 8
	HOME_RISE_SEARCH_START,	// 9
	HOME_RISE_SEARCH_WAIT,	// 10
	HOME_FALL_SEARCH_START,	// 11
	HOME_FALL_SEARCH_WAIT,	// 12
	HOME_INDEX_SEARCH_WAIT,	// 13
	HOME_SET_FINAL_POSITION,	// 14
	HOME_FINAL_MOVE_START,	// 15
	HOME_FINAL_MOVE_WAIT,	// 16
	HOME_FINISHED,		// 17
	HOME_ABORT		// 18
    } home_state_t;

/* flags for homing */
#define HOME_IGNORE_LIMITS	1
#define HOME_USE_INDEX		2

/* flags for switch config */
#define SWITCHES_LATCH_LIMITS	16

/* This structure contains all of the data associated with
   a single joint.  Note that this structure does not need
   to be in shared memory (but it can, if desired for debugging