Subversion Repositories HomeAutomation

/**********************************************************************************************

 * C PID Library - Version 1.0.1

 * modified by Linus Lundin <linus.lundin@gmail.com (ported c to be used with AVR-GCC)

 * originally by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com

 *

 * This Library is licensed under a GPLv3 License

 **********************************************************************************************/

#include "PID_v1.h"

#include <drivers/timer/timer.h>

void PID_Initialize(PidType* pid);

void PID_Limit(PidType* pid, FloatType *var);

/*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.

 ***************************************************************************/

void PID_init(PidType* pid, FloatType* Input, FloatType* Output, FloatType* Setpoint, FloatType Kp, FloatType Ki, FloatType Kd,

    PidDirectionType ControllerDirection) {

  PID_SetOutputLimits(pid, 0, 0xffff);

  //default Controller Sample Time is 0.1 seconds

  pid->SampleTime = 100;

  PID_SetControllerDirection(pid, ControllerDirection);

  PID_SetTunings(pid, Kp, Ki, Kd);

  pid->lastTime = Timer_GetTicks() - pid->SampleTime;

  pid->mode = PID_Mode_Manual;

  pid->myInput = Input;

  pid->myOutput = Output;

  pid->mySetpoint = Setpoint;

  pid->MaxKd = 10.0;

}

/* 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.

 **********************************************************************************/

bool PID_Compute(PidType* pid)

{

  if (pid->mode == PID_Mode_Manual)

  {

    return false;

  }

  unsigned long now = Timer_GetTicks();

  unsigned long timeChange = (now - pid->lastTime);

  if (timeChange >= pid->SampleTime)

  {

    // Compute all the working error variables

    FloatType input = *pid->myInput;

    // compute pid_t Output

    FloatType error = *pid->mySetpoint - input;

    FloatType dInput = (input - pid->lastInput);

    pid->PTerm = pid->kp * error;

    pid->ITerm += ( pid->ki * error );

    PID_Limit(pid, &pid->ITerm);

    pid->DTerm = - pid->kd * dInput / ( pid->MaxKd < MAX_KD_MIN ? 1.0 : 1.0 + pid->kd / pid->MaxKd );

    // compute PID Output

    FloatType output = pid->PTerm + pid->ITerm + pid->DTerm;

    PID_Limit(pid, &output);

    *pid->myOutput = output;

    // remember some variables for next time

    pid->lastInput = input;

    pid->lastTime = now;

    return true;

  }

  else

  {

    return false;

  }

}

/* Limit(...)******************************************************************

 * bound supplied variable to (outMin, outMax)

 ******************************************************************************/

void PID_Limit(PidType* pid, FloatType *var)

{

  if (*var < pid->outMin)

  {

    *var = pid->outMin;

  }

  else if (*var > pid->outMax)

  {

    *var = pid->outMax;

  }

}

/* 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(PidType* pid, FloatType Kp, FloatType Ki, FloatType Kd)

{

  if ((Kp < 0.0) || (Ki < 0.0) || (Kd < 0.0))

  {

    return;

  }

  pid->dispKp = Kp;

  pid->dispKi = Ki;

  pid->dispKd = Kd;

  FloatType SampleTimeInSec = ((FloatType) pid->SampleTime) / 1000.0;

  pid->kp = Kp;

  pid->ki = Ki * SampleTimeInSec;

  pid->kd = Kd / SampleTimeInSec;

  if (pid->controllerDirection == PID_Direction_Reverse) {

    pid->kp = (0.0 - pid->kp);

    pid->ki = (0.0 - pid->ki);

    pid->kd = (0.0 - pid->kd);

  }

}

/* SetSampleTime(...) *********************************************************

 * sets the period, in Milliseconds, at which the calculation is performed 

 ******************************************************************************/

void PID_SetSampleTime(PidType* pid, uint32_t NewSampleTime)

{

  if (NewSampleTime > 0) {

    FloatType ratio = (FloatType) NewSampleTime / (FloatType) pid->SampleTime;

    pid->ki *= ratio;

    pid->kd /= ratio;

    pid->SampleTime = (uint32_t) 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(PidType* pid, FloatType newMin, FloatType newMax) {

  if (newMin >= newMax) {

    return;  

  }

  pid->outMin = newMin;

  pid->outMax = newMax;

  if (pid->mode == PID_Mode_Automatic)

  {

    PID_Limit(pid, pid->myOutput);

    PID_Limit(pid, &pid->ITerm);

  }

}

/* 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(PidType* pid, PidModeType newMode)

{

  if (newMode != pid->mode)

  {

    // just changed mode

    PID_Initialize(pid);

    pid->mode = newMode;

  }

}

/* Initialize()****************************************************************

 *  does all the things that need to happen to ensure a bumpless transfer

 *  from manual to automatic mode.

 ******************************************************************************/

void PID_Initialize(PidType* pid)

{

  pid->ITerm = *pid->myOutput;

  pid->lastInput = *pid->myInput;

  PID_Limit(pid, &pid->ITerm);

}

/* 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(PidType* pid, PidDirectionType newDirection)

{

  if ((pid->mode == PID_Mode_Automatic) && (newDirection != pid->controllerDirection))

  {

    pid->kp = (0.0 - pid->kp);

    pid->ki = (0.0 - pid->ki);

    pid->kd = (0.0 - pid->kd);

  }  

  pid->controllerDirection = newDirection;

}

/* pid_set_max_Kd(...)*********************************************************

 * set maximum derivative gain

 * Default 0 means no maximum.

 ******************************************************************************/

void PID_SetMaxKd(PidType* pid, FloatType newMaxKd )

{

  if ( newMaxKd < MAX_KD_MIN )

  {

    // no limit to Kd

    newMaxKd = 0.0;

  }

  pid->MaxKd = newMaxKd;

}  

/* Status Functions************************************************************

 * 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

 ******************************************************************************/

FloatType PID_GetKp(PidType* pid) {

  return pid->dispKp;

}

FloatType PID_GetKi(PidType* pid) {

  return pid->dispKi;

}

FloatType PID_GetKd(PidType* pid) {

  return pid->dispKd;

}

FloatType PID_GetMin(PidType* pid) {

  return pid->outMin;

}

FloatType PID_GetMax(PidType* pid) {

  return pid->outMax;

}

uint32_t PID_GetSampleTime(PidType* pid) {

  return pid->SampleTime;

}

PidModeType PID_GetMode(PidType* pid) {

  return pid->mode;

}

PidDirectionType PID_GetDirection(PidType* pid) {

  return pid->controllerDirection;

}

FloatType PID_GetPTerm(PidType* pid)

{

  return  pid->PTerm;

}

FloatType PID_GetITerm(PidType* pid)

{

  return  pid->ITerm;

}

FloatType PID_GetDTerm(PidType* pid)

{

  return  pid->DTerm;

}