File content as of revision 3:c6caae712d5d:

/**********************************************************************************************
 * Arduino PID Library - Version 1.1.1
 * by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
 *
 * This Library is licensed under a GPLv3 License
 **********************************************************************************************/
#if 0
#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#endif
#include "mbed.h"
#include <PID_v2.h>

extern Timer g_Timer;
#define constrain(x,a,b) ((x) < (a) ? (a) : ((x) > (b) ? (b) : (x)))

/*Constructor (...)*********************************************************
 *    The parameters specified here are those for for which we can't set up
 *    reliable defaults, so we need to have the user set them.
 ***************************************************************************/
PID::PID(double Kp, double Ki, double Kd, int ControllerDirection)
{

//   myOutput = Output;
//   myInput = Input;
//   mySetpoint = Setpoint;
    inAuto = false;

    PID::SetOutputLimits(0, 255);				//default output limit corresponds to
    PID::SetITermLimits(0, 255);
    //the arduino pwm limits

    SampleTime = 1;							//default Controller Sample Time is 0.1 seconds

    PID::SetControllerDirection(ControllerDirection);
    PID::SetTunings(Kp, Ki, Kd);

    //lastTime = micros()/1000 - SampleTime;
    lastTime = g_Timer.read_us() / 1000 - SampleTime;
}


/* Compute() **********************************************************************
 *     This, as they say, is where the magic happens.  this function should be called
 *   every time "void loop()" executes.  the function will decide for itself whether a new
 *   pid Output needs to be computed.  returns true when the output is computed,
 *   false when nothing has been done.
 **********************************************************************************/
float PID::Compute(float Input, float Setpoint)
{
    float output = 0;
    if(!inAuto) return output;
    //unsigned long now = micros();
    unsigned long now = g_Timer.read_us();
    float dt = (now - lastTime)/1000.0;

    //if(dt>=SampleTime) {
	if (1) {
        /*Compute all the working error variables*/
        float input = Input;
        float error = Setpoint - input;
        ITerm += error * dt;
        ITerm = constrain(ITerm, errorMin, errorMax);
        float dInput = (input - lastInput)/dt;

        /*Compute PID Output*/
        output = kp * error + ki * ITerm - kd * dInput;

        output = constrain(output, outMin, outMax);
//	  *myOutput = output;

        /*Remember some variables for next time*/
        lastInput = input;
        lastTime = now;
        //  return true;
    }
    return output;
}


/* SetTunings(...)*************************************************************
 * This function allows the controller's dynamic performance to be adjusted.
 * it's called automatically from the constructor, but tunings can also
 * be adjusted on the fly during normal operation
 ******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd)
{
    if (Kp<0 || Ki<0 || Kd<0) return;

    //double SampleTimeInSec = ((double)SampleTime)/1000;
    kp = Kp;
    ki = Ki;
    kd = Kd;

    dispKp = Kp;
    dispKi = Ki;
    dispKd = Kd;

    if(controllerDirection ==REVERSE) {
        kp = (0 - kp);
        ki = (0 - ki);
        kd = (0 - kd);
    }
}

/* SetSampleTime(...) *********************************************************
 * sets the period, in Milliseconds, at which the calculation is performed
 ******************************************************************************/
void PID::SetSampleTime(int NewSampleTime)
{
    if (NewSampleTime > 0) {
        double ratio  = (double)NewSampleTime
                        / (double)SampleTime;
        ki *= ratio;
        kd /= ratio;
        SampleTime = (unsigned long)NewSampleTime;
    }
}

/* SetOutputLimits(...)****************************************************
 *     This function will be used far more often than SetInputLimits.  while
 *  the input to the controller will generally be in the 0-1023 range (which is
 *  the default already,)  the output will be a little different.  maybe they'll
 *  be doing a time window and will need 0-8000 or something.  or maybe they'll
 *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
 *  here.
 **************************************************************************/
void PID::SetOutputLimits(double Min, double Max)
{
    if(Min >= Max) return;
    outMin = Min;
    outMax = Max;

    if(inAuto)
        *myOutput = constrain(*myOutput, outMin, outMax);
}

void PID::SetITermLimits(double Min, double Max)
{
    if(Min >= Max) return;
    errorMin = Min;
    errorMax = Max;

    if(inAuto)
        ITerm = constrain(ITerm, errorMin, errorMax);
}

/* SetMode(...)****************************************************************
 * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
 * when the transition from manual to auto occurs, the controller is
 * automatically initialized
 ******************************************************************************/
void PID::SetMode(int Mode)
{
    bool newAuto = (Mode == AUTOMATIC);
    if(newAuto == !inAuto) {
        /*we just went from manual to auto*/
        PID::Initialize();
    }
    inAuto = newAuto;
}

/* Initialize()****************************************************************
 *	does all the things that need to happen to ensure a bumpless transfer
 *  from manual to automatic mode.
 ******************************************************************************/
void PID::Initialize()
{
    ITerm = *myOutput;
    lastInput = *myInput;
    ITerm = constrain(ITerm, errorMin, errorMax);
}

/* SetControllerDirection(...)*************************************************
 * The PID will either be connected to a DIRECT acting process (+Output leads
 * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
 * know which one, because otherwise we may increase the output when we should
 * be decreasing.  This is called from the constructor.
 ******************************************************************************/
void PID::SetControllerDirection(int Direction)
{
    if(inAuto && Direction !=controllerDirection) {
        kp = (0 - kp);
        ki = (0 - ki);
        kd = (0 - kd);
    }
    controllerDirection = Direction;
}

/* Status Funcions*************************************************************
 * Just because you set the Kp=-1 doesn't mean it actually happened.  these
 * functions query the internal state of the PID.  they're here for display
 * purposes.  this are the functions the PID Front-end uses for example
 ******************************************************************************/
double PID::GetKp()
{
    return  dispKp;
}
double PID::GetKi()
{
    return  dispKi;
}
double PID::GetKd()
{
    return  dispKd;
}
int PID::GetMode()
{
    return  inAuto ? AUTOMATIC : MANUAL;
}
int PID::GetDirection()
{
    return controllerDirection;
}