control.c

Source code for an Numeric Cmputer

/********************************************************************
* Description: control.c
*   emcmotController() is the main loop running at the servo cycle
*   rate. All state logic and trajectory calcs are called from here.
*
*   Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* Created on:
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
********************************************************************/

#include "posemath.h"
#include "rtapi.h"
#include "hal.h"
#include "emcmotglb.h"
#include "motion.h"
#include "mot_priv.h"

/***********************************************************************
*                  LOCAL VARIABLE DECLARATIONS                         *
************************************************************************/

/*! \todo FIXME - this is a leftover global, it will eventually go away */
int rehomeAll;

/* these variables have the servo cycle time and 1/cycle time */
static double servo_period;
static double servo_freq;


/* debugging function - prints a cartesean pose (multplies the floating
   point numbers by 1 million since kernel printf doesn't handle floats
*/

void print_pose ( EmcPose *pos )
{
    rtapi_print(" (%d, %d, %d):(%d, %d, %d) ",
	(int)(pos->tran.x*1000000.0),
	(int)(pos->tran.y*1000000.0),
	(int)(pos->tran.z*1000000.0),
	(int)(pos->a*1000000.0),
	(int)(pos->b*1000000.0),
	(int)(pos->c*1000000.0) );
}


/*! \todo FIXME - debugging - uncomment the following line to log changes in
   AXIS_FLAG and MOTION_FLAG */
// #define WATCH_FLAGS 1


/* debugging function - it watches a particular variable and
   prints a message when the value changes.  Right now there are
   calls to this scattered throughout this and other files.
   To disable them, comment out the following define:
*/
// #define ENABLE_CHECK_STUFF

#ifdef ENABLE_CHECK_STUFF
void check_stuff(char *location)
{
   static char *target, old = 0xFF;
/*! \todo Another #if 0 */
#if 0
/* kludge to look at emcmotDebug->enabling and emcmotStatus->motionFlag
   at the same time - we simply use a high bit of the flags to
   hold "enabling" */
   short tmp;
   if ( emcmotDebug->enabling )
     tmp = 0x1000;
   else
     tmp = 0x0;
   tmp |= emcmotStatus->motionFlag;
   target = &tmp;
/* end of kluge */
#endif

    target = (emcmot_hal_data->enable);
    if ( old != *target ) {
	rtapi_print ( "%d: watch value %02X (%s)\n", emcmotStatus->heartbeat, *target, location );
	old = *target;
    }
}
#else /* make it disappear */
void check_stuff(char *location)
{
/* do nothing (I wonder if gcc is smart
   enough to optimize the calls away?) */
}
#endif /* ENABLE_CHECK_STUFF */


/***********************************************************************
*                      LOCAL FUNCTION PROTOTYPES                       *
************************************************************************/

/* the following functions are called (in this order) by the main
   controller function.  They are an attempt to break the huge
   function (originally 1600 lines) into something a little easier
   to understand.
*/

/* 'process_inputs()' is responsible for reading hardware input
   signals (from the HAL) and doing basic processing on them.  In
   the case of position feedback, that means removing backlash comp
   and calculating the following error.  For switches, it means
   debouncing them and setting flags in the emcmotStatus structure.
*/
static void process_inputs(void);

/* 'do forward kins()' takes the position feedback in joint coords
   and applies the forward kinematics to it to generate feedback
   in Cartesean coordinates.  It has code to handle machines that
   don't have forward kins, and other special cases, such as when
   the joints have not been homed.
*/
static void do_forward_kins(void);

/* 'check_soft_limits()' checks the position of active axes against
   their soft limits, and sets flags accordingly.  It does not check
   limits on axes that have not been homed, since their position is
   unknown.  It only sets flags, it does not take any action.
*/
static void check_soft_limits(void);

/* 'check_for_faults()' is responsible for detecting fault conditions
   such as limit switches, amp faults, following error, etc.  It only
   checks active axes.  It is also responsible for generating an error
   message.  (Later, once I understand the cmd/status/error interface
   better, it will probably generate error codes that can be passed
   up the architecture toward the GUI - printing error messages
   directly seems a little messy)
*/
static void check_for_faults(void);

/* 'set_operating_mode()' handles transitions between the operating
   modes, which are free, coordinated, and teleop.  This stuff needs
   to be better documented.  It is basically a state machine, with
   a current state, a desired state, and rules determining when the
   state can change.  It should be rewritten as such, but for now
   it consists of code copied exactly from emc1.
*/
static void set_operating_mode(void);

/* 'do_homing()' looks at the home_state field of each joint struct
    to decide what, if anything, needs to be done related to homing
    the joint.  Homing is implemented as a state machine, the exact
    sequence of states depends on the machine configuration.  It
    can be as simple as immediately setting the current position to
    zero, or a it can be a multi-step process (find switch, set
    approximate zero, back off switch, find index, set final zero,
    rapid to home position), or anywhere in between.
*/
static void do_homing(void);

/* 'get_pos_cmds()' generates the position setpoints.  This includes
   calling the trajectory planner and interpolating it's outputs.
   The teleop and coord mode code is copied directly from emc1 for
   now, the free mode code has been re-written.
*/

static void get_pos_cmds(void);

/* 'compute_backlash()' is responsible for calculating backlash and
   lead screw error compensation.  (Leadscrew error compensation is
   a more sophisticated version that includes backlash comp.)  It uses
   the velocity in emcmotStatus->joint_vel_cmd to determine which way
   each axis is moving, and the position in emcmotStatus->joint_pos_cmd
   to determine where the axis is at.  That information is used to
   create the compensation value that is added to the joint_pos_cmd
   to create motor_pos_cmd, and is subtracted from motor_pos_fb to
   get joint_pos_fb.  (This function does not add or subtract the
   compensation value, it only computes it.)  The basic compensation
   value is in backlash_corr, however has makes step changes when
   the direction reverses.  backlash_filt is a ramped version, and
   that is the one that is later added/subtracted from the position.
*/
static void compute_backlash(void);

/* 'output_to_hal()' writes the handles the final stages of the
   control function.  It applies backlash comp and writes the
   final motor position to the HAL (which routes it to the PID
   loop).  It also drives other HAL outputs, and it writes a
   number of internal variables to HAL parameters so they can
   be observed with halscope and halmeter.
*/
static void output_to_hal(void);

/* 'update_status()' copies assorted status information to shared
   memory (the emcmotStatus structure) so that it is available to
   higher level code.
*/
static void update_status(void);


/***********************************************************************
*                        PUBLIC FUNCTION CODE                          *
************************************************************************/

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

  This function gets called at regular intervals - therefore it does NOT
  have a loop within it!

  Inactive axes are still calculated, but the PIDs are inhibited and
  the amp enable/disable are inhibited
  */

void emcmotController(void *arg, long period)
{

    /* calculate servo period as a double - period is in integer nsec */
    servo_period = period * 0.000000001;
    /* calculate servo frequency for calcs like vel = Dpos / period */
    /* it's faster to do vel = Dpos * freq */
    servo_freq = 1.0 / servo_period;
    /* increment head count to indicate work in progress */
    emcmotStatus->head++;
    /* here begins the core of the controller */

check_stuff ( "before process_inputs()" );
    process_inputs();
check_stuff ( "after process_inputs()" );
    do_forward_kins();
check_stuff ( "after do_forward_kins()" );
    check_soft_limits();
check_stuff ( "after check_soft_limits()" );
    check_for_faults();
check_stuff ( "after check_for_faults()" );
    set_operating_mode();
check_stuff ( "after set_operating_mode()" );
    do_homing();
check_stuff ( "after do_homing()" );
    get_pos_cmds();
check_stuff ( "after get_pos_cmds()" );
    compute_backlash();
check_stuff ( "after compute_backlash()" );
    output_to_hal();
check_stuff ( "after output_to_hal()" );
    update_status();
check_stuff ( "after update_status()" );
    /* here ends the core of the controller */
    emcmotStatus->heartbeat++;
    /* set tail to head, to indicate work complete */
    emcmotStatus->tail = emcmotStatus->head;

/* end of controller function */
}

/***********************************************************************
*                         LOCAL FUNCTION CODE                          *
************************************************************************/

/* The protoypes and documentation for these functions are located
   at the top of the file in the section called "local function
   prototypes"
*/

static void process_inputs(void)
{
    int joint_num, tmp;
    double abs_ferror;
    axis_hal_t *axis_data;
    emcmot_joint_t *joint;

    /* read probe input */
    if (*(emcmot_hal_data->probe_input)) {
	emcmotStatus->probeVal = 1;
    } else {
	emcmotStatus->probeVal = 0;
    }

    emcmotStatus->spindleRevs = *emcmot_hal_data->spindle_revs;

    /* read and process per-joint inputs */
    for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
	/* point to axis HAL data */
	axis_data = &(emcmot_hal_data->axis[joint_num]);
	/* point to joint data */
	joint = &joints[joint_num];
	/* copy data from HAL to joint structure */
	joint->motor_pos_fb = *(axis_data->motor_pos_fb);

	/* subtract backlash comp and motor offset */
	joint->pos_fb =
	    joint->motor_pos_fb - (joint->backlash_filt +
	    joint->motor_offset);
	/* calculate following error */
	joint->ferror = joint->pos_cmd - joint->pos_fb;
	abs_ferror = fabs(joint->ferror);
	/* update maximum ferror if needed */
	if (abs_ferror > joint->ferror_high_mark) {
	    joint->ferror_high_mark = abs_ferror;
	}

	/* calculate following error limit */
	if (joint->vel_limit > 0.0) {
	    joint->ferror_limit =
		joint->max_ferror * fabs(joint->vel_cmd) / joint->vel_limit;
	} else {
	    joint->ferror_limit = 0;
	}
	if (joint->ferror_limit < joint->min_ferror) {
	    joint->ferror_limit = joint->min_ferror;
	}
	/* update following error flag */
	if (abs_ferror > joint->ferror_limit) {
	    SET_JOINT_FERROR_FLAG(joint, 1);
	} else {
	    SET_JOINT_FERROR_FLAG(joint, 0);
	}

	/* read limit switches */
	if (*(axis_data->pos_lim_sw)) {
	    SET_JOINT_PHL_FLAG(joint, 1);
	} else {
	    SET_JOINT_PHL_FLAG(joint, 0);
	}
	if (*(axis_data->neg_lim_sw)) {
	    SET_JOINT_NHL_FLAG(joint, 1);
	} else {
	    SET_JOINT_NHL_FLAG(joint, 0);
	}
	/* Some machines can coast past the limit switches - this makes EMC
	   think the machine is no longer on the limit. To avoid this
	   problem, we latch the position when the limit first trips, and we
	   won't release the limit unless the current position is inside the
	   latched position. We use motor position because joint position
	   makes step changes during homing */
	/* latching of limit switches is optional - it should not be used if
	   pos_limit and neg_limit are tied together */
	if (joint->switch_flags & SWITCHES_LATCH_LIMITS) {
	    if (GET_JOINT_PHL_FLAG(joint)) {
		if (!joint->pos_limit_latch) {
		    /* on switch and not latched */
		    joint->pos_limit_latch = 1;
		    joint->pos_limit_pos = joint->motor_pos_fb;
		}
	    } else {
		if (joint->pos_limit_latch &&
		    (joint->motor_pos_fb < joint->pos_limit_pos)) {
		    /* off switch and inside switch position */
		    joint->pos_limit_latch = 0;
		}
	    }
	    if (GET_JOINT_NHL_FLAG(joint)) {
		if (!joint->neg_limit_latch) {
		    /* on switch and not latched */
		    joint->neg_limit_latch = 1;
		    joint->neg_limit_pos = joint->motor_pos_fb;
		}
	    } else {
		if (joint->neg_limit_latch &&
		    (joint->motor_pos_fb > joint->neg_limit_pos)) {
		    /* off switch and inside switch position */
		    joint->neg_limit_latch = 0;
		}
	    }
	} else {
	    /* don't latch, just copy flags */
	    joint->pos_limit_latch = GET_JOINT_PHL_FLAG(joint);
	    joint->neg_limit_latch = GET_JOINT_NHL_FLAG(joint);
	}
	/* read amp fault input */