C:/Active/libraries/cxutils/2.0/src/cxutils/math/pidcontroller.cpp

Go to the documentation of this file.

#include <limits.h>
#include "cxutils/timer.h"
#include "cxutils/math/pidcontroller.h"

using namespace CxUtils;

PIDController::PIDController()
{
    Reset();
    //Default DT if not set elsewhere
    mDT = 1.0;
}


PIDController::~PIDController()
{
}


void PIDController::SetRate(const double dt)
{
    mDT = dt;
}


double PIDController::UpdatePID(const double error)
{
    double solution;

    //  Prevent over winding the sum of error.  This basically
    //  means, if you are already giving the system 100%, you can't
    //  give it higher than that, so don't keep increasing
    //  the error
    if(mPreviousSolution > mLowerBound && mPreviousSolution < mUpperBound)
    {
        this->mSumOfError += error;
    }
    solution = mP*error + mI*mSumOfError*mDT + mD*(error - mPreviousError)/mDT;
    mPreviousError = error;

    //  Add any offset for the system
    solution += this->mOffset;

    //  Check upper and lower bounds of solution for
    //  the system being used
    if(solution <= mLowerBound)
        solution = mLowerBound;
    if(solution >= mUpperBound)
        solution = mUpperBound;

    this->mPreviousSolution = solution;

    return solution;
}


void PIDController::SetOffset(const double offset)
{
    this->mOffset = offset;
}


void PIDController::SetBounds(const double min, const double max)
{
    if(max > min)
    {
        this->mLowerBound = min;
        this->mUpperBound = max;
    }
}


void PIDController::SetGains(const double p, const double i, const double d)
{
    mP = p;
    mI = i;
    mD = d;
}


void PIDController::ClearError()
{
    mSumOfError = 0;
}


void PIDController::Reset()
{
    mLowerBound = -LONG_MAX;
    mUpperBound = LONG_MAX;

    mP = 0;
    mI = 0;
    mD = 0;
    mSumOfError = 0;
    mPreviousSolution = 0;
    mPreviousError = 0;
    mOffset = 0;
}


void PIDController::Reset(const double lower, 
                          const double upper, 
                          const double offset, 
                          const double p, 
                          const double i, 
                          const double d)
{
    SetBounds(lower, upper);
    SetGains(p, i, d);
    SetOffset(offset);
    mSumOfError = 0;
    mPreviousSolution = mOffset;
    mPreviousError = 0;
}

/*  End of File */