control.c

CNC 的开放码,EMC2 V2.2.8版

/********************************************************************
* 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.
*
* Last change:
* $Revision: 1.130.2.2 $
* $Author: jepler $
* $Date: 2007/12/06 13:56:38 $
*
********************************************************************/

#include "posemath.h"
#include "rtapi.h"
#include "hal.h"
#include "emcmotglb.h"
#include "motion.h"
#include "mot_priv.h"
#include "rtapi_math.h"
#include "tp.h"
#include "tc.h"
#include "motion_debug.h"
#include "config.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),(%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),
                (int)(pos->u*1000000.0),
                (int)(pos->v*1000000.0),
                (int)(pos->w*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 or
   screw error 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_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);

/* 'handle_jogwheels()' reads jogwheels, decides if they should be
   enabled, and if so, changes the free mode planner's target position
   when the jogwheel(s) turn.
*/
static void handle_jogwheels(void);

/* 'do_homing_sequence()' looks at emcmotStatus->homingSequenceState 
   to decide what, if anything, needs to be done related to multi-joint
   homing.
*/

static void do_homing_sequence(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 its outputs.
*/

static void get_pos_cmds(long period);

/* 'compute_screw_comp()' 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_screw_comp(void);

/* 'output_to_hal()' writes the handles the final stages of the
   control function.  It applies screw 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)
{
#ifdef RTAPI
    // - overrun detection -
    // maintain some records of how long it's been between calls.  The
    // first time we see a delay that's much longer than the records show
    // is normal, report an error.  This might detect bogus realtime 
    // performance caused by ACPI, onboard video, etc.  It can be reliably
    // triggered by maximizing glxgears on my nvidia system, which also
    // causes the rtai latency test to show overruns.

    // check below if you set this under 5
#define CYCLE_HISTORY 5

    static long int cycles[CYCLE_HISTORY];
    static long long int last = 0;

    static int index = 0, priming = 1;

    long long int now = rtapi_get_clocks();
    long int this_run = (long int)(now - last);
    emcmot_hal_data->last_period = this_run;
#ifdef HAVE_CPU_KHZ
    emcmot_hal_data->last_period_ns = this_run * 1e6 / cpu_khz;
#endif

#ifndef RTAPI_SIM
    if(!priming) {
        // we have CYCLE_HISTORY samples, so check for this call being 
        // anomolously late
        int i;

        for(i=0; i<CYCLE_HISTORY; i++) {
            if (this_run > 1.2 * cycles[i]) {
                emcmot_hal_data->overruns++;
                // print message on first overrun only
                if(emcmot_hal_data->overruns == 1) {
                    int saved_level = rtapi_get_msg_level();
                    rtapi_set_msg_level(RTAPI_MSG_ALL);
                    reportError("Unexpected realtime delay: check dmesg for details.");
                    rtapi_print_msg(RTAPI_MSG_WARN,
                        "\nIn recent history there were\n"
                        "%ld, %ld, %ld, %ld, and %ld\n"
                        "elapsed clocks between calls to the motion controller.\n",
                        cycles[0], cycles[1], cycles[2], cycles[3], cycles[4]);
                    rtapi_print_msg(RTAPI_MSG_WARN,
                        "This time, there were %ld which is so anomalously\n"
                        "large that it probably signifies a problem with your\n"
                        "realtime configuration.  For the rest of this run of\n"
                        "EMC, this message will be suppressed.\n\n",
                        this_run);
                    rtapi_set_msg_level(saved_level);
                }

		break;
	    }
        }
    }
#endif
    if(last) {
        cycles[index++] = this_run;
    }
    if(index == CYCLE_HISTORY) {
        // wrap around to the start of the array
        index = 0;
        // we now have CYCLE_HISTORY good samples, so start checking times
        priming = 0;
    }
    // we need this for next time
    last = now;

    // end of overrun detection
#endif

    /* 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_for_faults();
check_stuff ( "after check_for_faults()" );
    set_operating_mode();
check_stuff ( "after set_operating_mode()" );
    handle_jogwheels();
check_stuff ( "after handle_jogwheels()" );
    do_homing_sequence();
check_stuff ( "after do_homing_sequence()" );
    do_homing();
check_stuff ( "after do_homing()" );
    get_pos_cmds(period);
check_stuff ( "after get_pos_cmds()" );
    compute_screw_comp();
check_stuff ( "after compute_screw_comp()" );
    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;
    /* clear init flag */
    first_pass = 0;

/* 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;
    double abs_ferror, tmp, scale;
    axis_hal_t *axis_data;
    emcmot_joint_t *joint;
    unsigned char enables;

    /* read probe input */
    emcmotStatus->probeVal = *(emcmot_hal_data->probe_input);
    if (emcmotStatus->probing) {
        /* check if the probe has been tripped */
        if (emcmotStatus->probeVal) {
            /* remember the current position */
            emcmotStatus->probedPos = emcmotStatus->carte_pos_fb; 
            /* stop! */
            tpAbort(&emcmotDebug->queue);
            emcmotStatus->probing=0;
        /* check if the probe hasn't tripped, but the move finished */
        } else if (GET_MOTION_INPOS_FLAG() && tpQueueDepth(&emcmotDebug->queue) == 0) {
            /* we are already stopped, but we need to remember the current