File content as of revision 0:214d306295fa:

/*
 * PIDControl.cpp
 *
 *  Created on: 16.04.2017
 *      Author: chris
 */
 
#include "PID_Control.h"
 
/**
 * Constructor
 */
PID_Control::PID_Control() :
    kp(0), ki(0), kd(0)
{
    eOld = 0.0f;
    iSum = 0.0f;
}
 
/**
 * Destructor
 */
PID_Control::~PID_Control()
{
}
 
/**
 * Sets the PID values
 * @param p proportional gain
 * @param i integral gain
 * @param d differencial gain
 */
void PID_Control::setPIDValues(float p, float i, float d, float _max, float _min, float _iMax)
{
    kp = p;
    ki = i;
    kd = d;
 
    max = _max;
    min = _min;
    iMax = _iMax;
}
 
/**
 * Calculate and returns the next value from PID control
 * @param e the error
 * @param period the period
 * @return
 */
float PID_Control::calc(float e, float period)
{
    static float dpart = 0.0f;
    float out(0.0f);
 
    iSum += e;
 
    //Saturate i part
    if (iSum > iMax)
        iSum = iMax;
    if (iSum < -iMax)
        iSum = -iMax;
 
    out = kp * e;
    out += ki * iSum * period;
     
    dpart = 0.7f * dpart + 0.3f * (e - eOld) * 1.0f / period;
    out += kd * dpart;
 
   // out += kd * (e - eOld) * 1.0f / period;
 
    eOld = e;
 
    if( out > max ) out = max;
    else if( out < min) out = min;
 
    return out;
}