/********************************************************************
 * Description:  pid.comp
 *               This file, 'pid.comp', is a HAL component that provides
 *               Proportional/Integeral/Derivative control loops.
 *               Ported from the 'pid.c' component.
 *
 *  Author: John Kasunich 2003
 *  Ported to icomp by Alexander Roessler 2015
 *
 *  Copyright (c) 2003-2015 All rights reserved.
 *
 ********************************************************************/
component pidv2 "HAL component that provides Proportional \
    Integeral/Derivative control loops.  It is a realtime component.";
description """
    HAL component that provides Proportional/
    Integeral/Derivative control loops.  It is a realtime component.

    The number of pid components is set by the module parameter 'num_chan='
    when the component is insmod'ed.  Alternatively, use the
    names= specifier and a list of unique names separated by commas.
    The names= and num_chan= specifiers are mutually exclusive.

    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 ( abs(error) < deadband ) then error = 0
    limit error to +/- maxerror
    errorI += error * period
    limit errorI to +/- maxerrorI
    errorD = (error - previouserror) / period
    limit errorD to +/- maxerrorD
    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.
""";

pin_ptr in bit enable = false "Enable/disabled the PID loop";
pin_ptr in float command = 0.0 "Commanded value";
pin_ptr in float command_deriv = 0.0 "Derivative command input";
pin_ptr in float feedback = 0.0 "Feedback input";
pin_ptr in float feedback_deriv = 0.0 "Derivative feedback input";
pin_ptr out float error "Current error";
pin_ptr out float output "Ouput value";
pin_ptr out bit saturated "If the PID loop is saturated";
pin_ptr out float saturated_s "Saturated time";
pin_ptr out s32 saturated_count "How often the PID loop was saturated";
// parameters
pin_ptr in float Pgain = 1.0 "Proportional gain";
pin_ptr in float Igain = 0.0 "Integral gain";
pin_ptr in float Dgain = 0.0 "Derivative gain";
pin_ptr in float bias = 0.0 "Constant offset on output";
pin_ptr in float FF0 = 0.0 "Zeroth order Feedforward gain";
pin_ptr in float FF1 = 0.0 "First order Feedforward gain";
pin_ptr in float FF2 = 0.0 "Second order Feedforward gain";
pin_ptr in float deadband = 0.0 "Amount of error that will be ignored";
pin_ptr in float maxerror = 0.0 "Limit on error";
pin_ptr in float maxerrorI = 0.0 "Limit on error integrator";
pin_ptr in float maxerrorD = 0.0 "Limit on error differentiator";
pin_ptr in float maxcmdD = 0.0 "Limit on command differentiator";
pin_ptr in float maxcmdDD = 0.0 "Limit on command 2nd derivative";
pin_ptr in float maxoutput = 0.0 "Limit on output value";
pin_ptr in bit index_enable = false "Index enable";
pin_ptr in bit error_previous_target = false "Error previous target";
// debug
pin_ptr out float errorI "Integral of error";
pin_ptr out float errorD "Derivative of error";
pin_ptr out float commandD "Derivative of the command";
pin_ptr out float commandDD "2nd derivative of the command";
// variables
variable hal_float_t prev_error = 0.0;
variable hal_float_t prev_cmd = 0.0;
variable hal_float_t prev_fb = 0.0;
variable hal_float_t limit_state = 0.0;
variable hal_bit_t prev_ie;
// function
function do_pid_calcs fp;
// options
// option debug 0;
// misc
license "GPL v2";
author "John Kasunich";
;;

FUNCTION(do_pid_calcs) {
    hal_float_t tmp1, tmp2;
    hal_float_t periodfp, periodrecip;

    /* precalculate some timing constants */
    periodfp = (hal_float_t)period * 0.000000001;
    periodrecip = 1.0 / periodfp;
    /* calculate the error */
    if((!(prev_ie && !gb(index_enable))) && gb(error_previous_target)) {
        // the user requests ferror against prev_cmd, and we can honor
        // that request because we haven't just had an index reset that
        // screwed it up.  Otherwise, if we did just have an index
        // reset, we will present an unwanted ferror proportional to
        // velocity for this period, but velocity is usually very small
        // during index search.
        tmp1 = prev_cmd - gf(feedback);
    } else {
        tmp1 = gf(command) - gf(feedback);
    }
    /* store error to error pin */
    sf(error, tmp1);
    /* apply error limits */
    if (gf(maxerror) != 0.0) {
        if (tmp1 > gf(maxerror)) {
            tmp1 = gf(maxerror);
        } else if (tmp1 < -gf(maxerror)) {
            tmp1 = -gf(maxerror);
        }
    }
    /* apply the deadband */
    if (tmp1 > gf(deadband)) {
        tmp1 -= gf(deadband);
    } else if (tmp1 < -gf(deadband)) {
        tmp1 += gf(deadband);
    } else {
        tmp1 = 0.0;
    }
    /* do integrator calcs only if enabled */
    if (gb(enable)) {
        /* if output is in limit, don't let integrator wind up */
        if ( ( tmp1 * limit_state ) <= 0.0 ) {
            /* compute integral term */
            sf(errorI, gf(errorI) + tmp1 * periodfp);
        }
        /* apply integrator limits */
        if (gf(maxerrorI) != 0.0) {
            if (gf(errorI) > gf(maxerrorI)) {
                sf(errorI, gf(maxerrorI));
            } else if (gf(errorI) < -gf(maxerrorI)) {
                sf(errorI, -gf(maxerrorI));
            }
        }
    } else {
        /* not enabled, reset integrator */
        sf(errorI, 0.0);
    }
#if 0
    /* DEBUG: compute command and feedback derivatives to dummysigs */
    if(!(prev_ie && !gb(index_enable))) {
        command_derivds = (gf(command) - prev_cmd) * periodrecip;
        feedback_derivds = (gf(feedback) - prev_fb) * periodrecip;
    }
#endif
    /* and calculate derivative term as difference of derivatives */
    sf(errorD, gf(command_deriv) - gf(feedback_deriv));
    prev_error = tmp1;
    /* apply derivative limits */
    if (gf(maxerrorD) != 0.0) {
        if (gf(errorD) > gf(maxerrorD)) {
            sf(errorD, gf(maxerrorD));
        } else if (gf(errorD) < -gf(maxerrorD)) {
            sf(errorD, -gf(maxerrorD));
        }
    }
    /* calculate derivative of command */
    /* save old value for 2nd derivative calc later */
    tmp2 = gf(commandD);
    if(!(prev_ie && !gb(index_enable))) {
        // not falling edge of index_enable: the normal case
        sf(commandD, (gf(command) - prev_cmd) * periodrecip);
    }
    // else: leave commandD alone and use last period's.  prev_cmd
    // shouldn't be trusted because index homing has caused us to have
    // a step in position.  Using the previous period's derivative is
    // probably a decent approximation since index search is usually a
    // slow steady speed.

    // save ie for next time
    prev_ie = gb(index_enable);

    prev_cmd = gf(command);
    prev_fb = gf(feedback);

    /* apply derivative limits */
    if (gf(maxcmdD) != 0.0) {
        if (gf(commandD) > gf(maxcmdD)) {
            sf(commandD, gf(maxcmdD));
        } else if (gf(commandD) < -gf(maxcmdD)) {
            sf(commandD, gf(maxcmdD));
        }
    }
    /* calculate 2nd derivative of command */
    sf(commandDD, (gf(commandD) - tmp2) * periodrecip);
    /* apply 2nd derivative limits */
    if (gf(maxcmdDD) != 0.0) {
        if (gf(commandDD) > gf(maxcmdDD)) {
            sf(commandDD, gf(maxcmdDD));
        } else if (gf(commandDD) < -gf(maxcmdDD)) {
            sf(commandDD, -gf(maxcmdDD));
        }
    }
    /* do output calcs only if enabled */
    if (gb(enable)) {
    /* calculate the output value */
    tmp1 =
        gf(bias) + gf(Pgain) * tmp1 + gf(Igain) * gf(errorI) + gf(Dgain) * gf(errorD);
    tmp1 += gf(command) * gf(FF0) + gf(commandD) * gf(FF1) + gf(commandDD) * gf(FF2);
    /* apply output limits */
    if (gf(maxoutput) != 0.0) {
        if (tmp1 > gf(maxoutput)) {
            tmp1 = gf(maxoutput);
            limit_state = 1.0;
        } else if (tmp1 < -gf(maxoutput)) {
            tmp1 = -gf(maxoutput);
            limit_state = -1.0;
        } else {
            limit_state = 0.0;
        }
    }
    } else {
        /* not enabled, force output to zero */
        tmp1 = 0.0;
        limit_state = 0.0;
    }
    /* write final output value to output pin */
    sf(output, tmp1);

    /* set 'saturated' outputs */
    if(limit_state) {
        sb(saturated, true);
        sf(saturated_s, gf(saturated_s) + (hal_float_t) period * 1e-9);
        if(gs(saturated_count) != 2147483647)
            incs(saturated_count);
    } else {
        sb(saturated, false);
        sf(saturated_s, 0.0);
        ss(saturated_count, 0);
    }
    /* done */
    return 0;
}

