PID.nxc

Go to the documentation of this file.

/* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 4 -*- */
/*
    PID.nxc
    Copyright (C) 2008 Naba Kumar   <naba@gnome.org>

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program 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 program; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/

#ifndef _PID_CONTROLLER_H_
#define _PID_CONTROLLER_H_

struct PIDControl
{
    long PIDScale;                   
    long PValue;                     
    long IValue;                     
    long DValue;                     
    long setPoint;                   
    int steadyStateCountThreshold;   
    /* Output parameters */
    long outputGain;                  
    long absMaxOutput;                
    long absIntegralLimit;            
    /* State */                       
    long integral;                    
    long lastError;                   
    long steadyStateCount;
};

void PIDControlInit(PIDControl &pidControl, long PIDScale,
                    long PValue, long IValue, long DValue,
                    int steadyStateCountThreshold,
                    long outputGain, long absMaxOutput)
{
    pidControl.PIDScale = PIDScale;
    pidControl.PValue = PValue;
    pidControl.IValue = IValue;
    pidControl.DValue = DValue;
    pidControl.steadyStateCountThreshold = steadyStateCountThreshold;
    pidControl.outputGain = outputGain;
    pidControl.absMaxOutput = Abs(absMaxOutput);
    pidControl.absIntegralLimit = pidControl.absMaxOutput;
    pidControl.setPoint = 0;
    pidControl.integral = 0;
    pidControl.lastError = 0;
    pidControl.steadyStateCount = 0;
}

void PIDControlSetIntegralLimit(PIDControl &pidControl, long absIntegralLimit)
{
    pidControl.absIntegralLimit = Abs(absIntegralLimit);
}

void PIDControlSetPoint(PIDControl &pidControl, long setPoint)
{
    pidControl.setPoint = setPoint;
    pidControl.integral = 0;
    pidControl.steadyStateCount = 0;
}

bool PIDControlCheckEnd(PIDControl &pidControl)
{
    if (pidControl.steadyStateCount > pidControl.steadyStateCountThreshold)
        return true;
    else
        return false;
}

long PIDControlGetLastError(PIDControl &pidControl)
{
    return pidControl.lastError;
}

long PIDControlStep(PIDControl &pidControl, long currentPoint)
{
    long P, I, D, error, output;
    
    if (PIDControlCheckEnd(pidControl))
        return 0;

    error = pidControl.setPoint - currentPoint;

    /* If error hasn't changed for last steadyStateCountThreshold samples, end PID control */
    if (pidControl.steadyStateCountThreshold > 0 && (error - pidControl.lastError) == 0)
        pidControl.steadyStateCount++;

    P = ((pidControl.PValue * error) / pidControl.PIDScale);
    I = pidControl.integral + ((pidControl.IValue * error) / pidControl.PIDScale);
    D = ((pidControl.DValue) * (error - pidControl.lastError)) / pidControl.PIDScale;

    /* Limit integral output */
    if (I > (pidControl.absIntegralLimit/pidControl.outputGain))
       I = pidControl.absIntegralLimit;
    if (I < -(pidControl.absIntegralLimit/pidControl.outputGain))
       I = -pidControl.absIntegralLimit;

    output = pidControl.outputGain * (P + I + D);
    if (output > pidControl.absMaxOutput) output = pidControl.absMaxOutput;
    if (output < -pidControl.absMaxOutput) output = -pidControl.absMaxOutput;

    pidControl.integral = I;
    pidControl.lastError = error;
    
    return output;
}

#endif