at_pid.c

CNC 的开放码,EMC2 V2.2.8版

/******************************************************************************
 *
 * Copyright (C) 2003 John Kasunich <jmkasunich AT users DOT sourceforge DOT net>
 * Copyright (C) 2007 Peter G. Vavaroutsos <pete AT vavaroutsos DOT com>
 *
 * $RCSfile: at_pid.c,v $
 * $Author: jepler $
 * $Locker:  $
 * $Revision: 1.9 $
 * $State: Exp $
 * $Date: 2007/06/28 13:03:23 $
 *
 ******************************************************************************
 *
 * This file, 'pid.c', is a HAL component that provides Proportional/
 * Integeral/Derivative control loops.  It is a realtime component.
 *
 * It supports a maximum of 16 PID loops, as set by the insmod parameter
 * 'num_chan'.
 *
 * In this documentation, it is assumed that we are discussing position
 * loops.  However this component can be used to implement other loops
 * such as speed loops, torch height control, and others.
 *
 * Each loop has a number of pins and parameters, whose names begin
 * with 'pid.x.', where 'x' is the channel number.  Channel numbers
 * start at zero.
 *
 * The three most important pins are 'command', 'feedback', and
 * 'output'.  For a position loop, 'command' and 'feedback' are
 * in position units.  For a linear axis, this could be inches,
 * mm, metres, or whatever is relavent.  Likewise, for a angular
 * axis, it could be degrees, radians, etc.  The units of the
 * 'output' pin represent the change needed to make the feedback
 * match the command.  As such, for a position loop 'Output' is
 * a velocity, in inches/sec, mm/sec, degrees/sec, etc.
 *
 * Each loop has several other pins as well.  'error' is equal to
 * 'command' minus 'feedback'.  'enable' is a bit that enables
 * the loop.  If 'enable' is false, all integrators are reset,
 * and the output is forced to zero.  If 'enable' is true, the
 * loop operates normally.
 *
 * The PID gains, limits, and other 'tunable' features of the
 * loop are implemented as parameters.  These are as follows:
 *
 * Pgain        Proportional gain
 * Igain        Integral gain
 * Dgain        Derivative gain
 * bias         Constant offset on output
 * FF0          Zeroth order Feedforward gain
 * FF1          First order Feedforward gain
 * FF2          Second order Feedforward gain
 * deadband     Amount of error that will be ignored
 * maxerror     Limit on error
 * maxerrorI    Limit on error integrator
 * maxerrorD    Limit on error differentiator
 * maxcmdD      Limit on command differentiator
 * maxcmdDD     Limit on command 2nd derivative
 * maxoutput    Limit on output value
 *
 * All of the limits (max____) are implemented such that if the
 * parameter value is zero, there is no limit.
 *
 * A number of internal values which may be usefull for testing
 * and tuning are also available as parameters.  To avoid cluttering
 * the parameter list, these are only exported if "debug=1" is
 * specified on the insmod command line.
 *
 * errorI       Integral of error
 * errorD       Derivative of error
 * commandD     Derivative of the command
 * commandDD    2nd derivative of the command
 *
 * The PID loop calculations are as follows (see the code for
 * all the nitty gritty details):
 *
 * error = command - feedback
 * if ( fabs(error) < deadband ) then error = 0
 * limit error to +/- maxerror
 * errorI += error * period
 * limit errorI to +/- maxerrorI
 * errorD = (error - previouserror) / period
 * limit errorD to +/- paxerrorD
 * commandD = (command - previouscommand) / period
 * limit commandD to +/- maxcmdD
 * commandDD = (commandD - previouscommandD) / period
 * limit commandDD to +/- maxcmdDD
 * output = bias + error * Pgain + errorI * Igain +
 *          errorD * Dgain + command * FF0 + commandD * FF1 +
 *          commandDD * FF2
 * limit output to +/- maxoutput
 *
 * This component exports one function called 'pid.x.do-pid-calcs'
 * for each PID loop.  This allows loops to be included in different
 * threads and execute at different rates.
 *
 ******************************************************************************
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of version 2 of the GNU General
 * Public License as published by the Free Software Foundation.
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111 USA
 *
 * THE AUTHORS OF THIS LIBRARY ACCEPT ABSOLUTELY NO LIABILITY FOR
 * ANY HARM OR LOSS RESULTING FROM ITS USE.  IT IS _EXTREMELY_ UNWISE
 * TO RELY ON SOFTWARE ALONE FOR SAFETY.  Any machinery capable of
 * harming persons must have provisions for completely removing power
 * from all motors, etc, before persons enter any danger area.  All
 * machinery must be designed to comply with local and national safety
 * codes, and the authors of this software can not, and do not, take
 * any responsibility for such compliance.
 *
 * This code was written as part of the EMC HAL project.  For more
 * information, go to www.linuxcnc.org.
 *
 ******************************************************************************/

#ifndef RTAPI
#error This is a realtime component only!
#endif

#include "rtapi.h"                      // RTAPI realtime OS API.
#include "rtapi_app.h"                  // RTAPI realtime module decls.
#include "rtapi_math.h"
#include "hal.h"                        // HAL public API decls.


// Module information.
MODULE_AUTHOR("Pete Vavaroutsos");
MODULE_DESCRIPTION("Auto-Tune PID Loop Component for EMC HAL");
MODULE_LICENSE("GPL");
static int num_chan = 3;                // Number of channels - default = 3.
RTAPI_MP_INT(num_chan, "number of channels");
static int debug = 0;                   // Flag to export optional params.
RTAPI_MP_INT(debug, "enables optional params");


#define PI                              3.141592653589


/******************************************************************************
 * PID OBJECT
 *
 * This structure contains the runtime data for a single PID loop.
 ******************************************************************************/

typedef enum {
    STATE_PID,
    STATE_TUNE_IDLE,
    STATE_TUNE_START,
    STATE_TUNE_POS,
    STATE_TUNE_NEG,
    STATE_TUNE_ABORT,
} State;


// Values for tune-type parameter.
typedef enum {
    TYPE_PID,
    TYPE_PI_FF1,
} Mode;


typedef struct {
    // Parameters.
    hal_float_t         deadband;
    hal_float_t         maxError;       // Limit for error.
    hal_float_t         maxErrorI;      // Limit for integrated error.
    hal_float_t         maxErrorD;      // Limit for differentiated error.
    hal_float_t         maxCmdD;        // Limit for differentiated cmd.
    hal_float_t         maxCmdDd;       // Limit for 2nd derivative of cmd.

    hal_float_t         bias;           // Steady state offset.
    hal_float_t         pGain;          // Proportional gain.
    hal_float_t         iGain;          // Integral gain.
    hal_float_t         dGain;          // Derivative gain.
    hal_float_t         ff0Gain;        // Feedforward proportional.
    hal_float_t         ff1Gain;        // Feedforward derivative.
    hal_float_t         ff2Gain;        // Feedforward 2nd derivative.
    hal_float_t         maxOutput;      // Limit for PID output.
    hal_float_t         tuneEffort;     // Control effort for limit cycle.
    hal_u32_t           tuneCycles;
    hal_u32_t           tuneType;

    hal_float_t         errorI;         // Integrated error.
    hal_float_t         errorD;         // Differentiated error.
    hal_float_t         cmdD;           // Differentiated command.
    hal_float_t         cmdDd;          // 2nd derivative of command.
    hal_float_t         ultimateGain;   // Calc by auto-tune from limit cycle.
    hal_float_t         ultimatePeriod; // Calc by auto-tune from limit cycle.

    // Pins.
    hal_bit_t           *pEnable;
    hal_float_t         *pCommand;      // Commanded value.
    hal_float_t         *pFeedback;     // Feedback value.
    hal_float_t         *pError;        // Command - feedback.
    hal_float_t         *pOutput;       // The output value.
    hal_bit_t           *pTuneMode;     // 0=PID, 1=tune.
    hal_bit_t           *pTuneStart;    // Set to 1 to start an auto-tune cycle.
                                        // Clears automatically when the cycle
                                        // has finished.

    // Private data.
    hal_float_t         prevError;      // previous error for differentiator.
    hal_float_t         prevCmd;        // previous command for differentiator.
    hal_float_t         limitState;     // +1 or -1 if in limit, else 0.
    State               state;
    hal_u32_t           cycleCount;
    hal_u32_t           cyclePeriod;
    hal_float_t         cycleAmplitude;
    hal_float_t         totalTime;
    hal_float_t         avgAmplitude;
} Pid;


// These methods are used for initialization.
static int Pid_Init(Pid *this);
static int Pid_Export(Pid *this, int compId, int id);
static void Pid_AutoTune(Pid *this, long period);
static void Pid_CycleEnd(Pid *this);

// These methods are exported to the HAL.
static void Pid_Refresh(void *this, long period);

/******************************************************************************
 * COMPONENT OBJECT
 *
 * This object contains all the data for this HAL component.
 *
 ******************************************************************************/

#define MAX_CHANNELS            16

typedef struct {
    int                         id;     // HAL component ID.
    Pid                         *pidTable;
} Component;

static Component                component;


/******************************************************************************
 * INIT AND EXIT CODE
 ******************************************************************************/

int
rtapi_app_main(void)
{
    int                         i;
    Pid                         *pComp;

    // Check number of channels.
    if((num_chan <= 0) || (num_chan > MAX_CHANNELS)){
        rtapi_print_msg(RTAPI_MSG_ERR,
                        "PID: ERROR: invalid num_chan: %d\n", num_chan);
        return(-1);
    }

    // Connect to the HAL.
    component.id = hal_init("at_pid");
    if(component.id < 0){
        rtapi_print_msg(RTAPI_MSG_ERR, "PID: ERROR: hal_init() failed\n");
        return(-1);
    }

    // Allocate memory for pid objects.
    component.pidTable = hal_malloc(num_chan * sizeof(*pComp));

    if(component.pidTable == NULL){
        rtapi_print_msg(RTAPI_MSG_ERR, "PID: ERROR: hal_malloc() failed\n");
        hal_exit(component.id);
        return(-1);
    }

    pComp = component.pidTable;
    for(i = 0; i < num_chan; i++, pComp++){
        // Initialize pid.
        if(Pid_Init(pComp)){
            hal_exit(component.id);
            return(-1);
        }

        // Export pins, parameters, and functions.
        if(Pid_Export(pComp, component.id, i)){
            hal_exit(component.id);
            return(-1);
        }
    }

    hal_ready(component.id);
    return(0);
}


void
rtapi_app_exit(void)
{
    Pid                         *pComp;

    hal_exit(component.id);

    if((pComp = component.pidTable) != NULL){
        // TODO: Free pid objects when HAL supports free.
//        hal_free(pComp);
    }
}


/******************************************************************************
 * PID OBJECT FUNCTION DEFINITIONS
 ******************************************************************************/

/*
 * LOCAL FUNCTIONS
 */

static int
Pid_Init(Pid *this)
{
    // Init all structure members.
    this->deadband = 0;
    this->maxError = 0;
    this->maxErrorI = 0;
    this->maxErrorD = 0;
    this->maxCmdD = 0;
    this->maxCmdDd = 0;
    this->errorI = 0;
    this->prevError = 0;
    this->errorD = 0;
    this->prevCmd = 0;
    this->limitState = 0;
    this->cmdD = 0;
    this->cmdDd = 0;
    this->bias = 0;
    this->pGain = 1;
    this->iGain = 0;
    this->dGain = 0;
    this->ff0Gain = 0;
    this->ff1Gain = 0;
    this->ff2Gain = 0;
    this->maxOutput = 0;
    this->state = STATE_PID;
    this->tuneCycles = 50;
    this->tuneEffort = 0.5;
    this->tuneType = TYPE_PID;

    return(0);
}


static int
Pid_Export(Pid *this, int compId, int id)
{
    int                         error, msg;
    char                        buf[HAL_NAME_LEN + 2];

    // This function exports a lot of stuff, which results in a lot of
    // logging if msg_level is at INFO or ALL. So we save the current value
    // of msg_level and restore it later.  If you actually need to log this
    // function's actions, change the second line below.
    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);

    // Export pins.
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.enable", id);
    error = hal_pin_bit_new(buf, HAL_IN, &(this->pEnable), compId);

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.command", id);
        error = hal_pin_float_new(buf, HAL_IN, &(this->pCommand), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.feedback", id);
        error = hal_pin_float_new(buf, HAL_IN, &(this->pFeedback), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.error", id);
        error = hal_pin_float_new(buf, HAL_OUT, &(this->pError), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.output", id);
        error = hal_pin_float_new(buf, HAL_OUT, &(this->pOutput), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.tune-mode", id);
        error = hal_pin_bit_new(buf, HAL_IN, &(this->pTuneMode), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.tune-start", id);
        error = hal_pin_bit_new(buf, HAL_IO, &(this->pTuneStart), compId);
    }

    // Export parameters.
    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.deadband", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->deadband), compId);
    }

    if(!error){
        rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.maxerror", id);
        error = hal_param_float_new(buf, HAL_RW, &(this->maxError), compId);
    }

    if(!error){