File content as of revision 0:8a420ac6394e:

#include "PID.h"

const int SCALING_FACTOR = 10000;

PID::PID(void)
{
    minOutput = 0;
    maxOutput = 0;
    kp = 0;
    ki = 0;
    kd = 0;
    errorIntegral = 0;
    lastError = 0;
    msPeriod = 1;
    target = 0;
    currentOutput = 0;
}


void PID::Reset(void)
{
    errorIntegral = 0;
    lastError = 0;
    currentOutput = 0;
}


void PID::SetOutputRange(double minOutput, double maxOutput)
{
    this->minOutput = minOutput;
    this->maxOutput = maxOutput;
}


void PID::SetPID(double kp, double ki, double kd)
{
    this->kp = (kp/SCALING_FACTOR);
    this->ki = (ki/SCALING_FACTOR);
    this->kd = (kd/SCALING_FACTOR);
}


void PID::AddSample(double sampleValue, double control)
{
    pidSampleValue = sampleValue;
    // calculate error
    double error = target - sampleValue;
    
    // update our integral; the I & D gain are assumed to use seconds as their units
    errorIntegral += error * msPeriod / 1000;
    
    // calculate our differential
    double errorDifferential = (error - lastError) / (msPeriod / 1000);
    lastError = error;
    
    // that gives us what we need to calculate an output
    double output = control+(
        kp * error +
        ki * errorIntegral +
        kd * errorDifferential)
        ;
    if (output < minOutput)
        output = minOutput;
    if (output > maxOutput)
        output = maxOutput;
        
    currentOutput = output;
}