Tools/Math/PIDsmoothedValue.cpp

Go to the documentation of this file.

/** 
* @file PIDsmoothedValue.cpp
* Implements class PIDsmoothedValue
* that calculates the PIDvalue for a given value
*
* @author <a href="mailto:goehring@informatik.hu-berlin.de">Daniel Goehring</a>
*/


#include "PIDsmoothedValue.h"
#include "Tools/Debugging/DebugDrawings.h"


/**
* Default constructor contains default vaules and weights.
* Value types are: First result value = 0; P-weight,I-weight,D-weight, 
* Min PID - Value, Max PID - Value, Max PID - value change since recent PID - value.
* 
*/

PIDsmoothedValue::PIDsmoothedValue():value(0.0),p(0.5),i(0.5),d(0.5),min(-10000),max(10000),maxaction(5000)   
{
}


/**
* Constructor for user chosen weights and value 
* types as above in the default constructor.
*/

PIDsmoothedValue::PIDsmoothedValue(double value, double p, double i, double d, double min, double max, double maxaction):
  lastDifference(0.0), 
  lastTargetVal(0.0),
  value(value), 
  lastValue(0.0), 
  started(false), 
  integralError(0.0),
  lastTime(0.0),
  p(p), i(i), d(d), 
  min(min), max(max),
  maxaction(maxaction)
{}


/**
* Set the min value
*/

void PIDsmoothedValue::setMin(double m) 
{
  PIDsmoothedValue::min = m;
}


/**
* Set the max value
*/

void PIDsmoothedValue::setMax(double m) 
{
  PIDsmoothedValue::max = m;
}


/**
* Set the weight for P afterwards, i.e. after using the constructor
*/

void PIDsmoothedValue::setWeightP(double p) 
{
  PIDsmoothedValue::p = p;
}


/**
* Set the weight for I afterwards, i.e. after using the constructor
*/

void PIDsmoothedValue::setWeightI(double i) 
{
  PIDsmoothedValue::i = i;
}


/**
* Set the weight for D afterwards, i.e. after using the constructor
*/

void PIDsmoothedValue::setWeightD(double d) 
{
  PIDsmoothedValue::d = d;
}


/**
* Calculation of the returned PID-value
* thereby it will not just calculate the PID-sum but also ensure that it lies
* within the given boundaries
*/

double PIDsmoothedValue::approximateVal(double targetVal) 
{
  
  double diff = targetVal - value; 
  // target to object difference = P - part of the sum
  // i.e. Proportional value
  
  double dDiff = diff - lastDifference; 
  
  if (!started) 
  {
    dDiff = 0;
    started = true;
  }
  // object to target difference change since last turn = D - part of the sum
  // i.e. Differential value
  
  integralError += diff;
  // the sum of all target to object differences = I - part of the sum
  // i.e. Integral values, because it is an integral for non discrete state functions
  
  lastValue = value;
  // just remember the new value for the next calculation
  
  value += p*diff + i*integralError + d*dDiff;
  // generate the weighted sum of the P, I, D - parts and get an unchecked PID-value
  
  
  if (value < min) 
  {
    value = min;
  }
  // check, if the PID-value is not too small and - if necessary correct it 
  
  if (value > max) 
  {
    value = max;
  }
  // check, if the PID-value is not too big and - if necessary correct it 
  
  
  if ((value - lastValue) > maxaction) { value = lastValue + maxaction; } 
  else if ((lastValue - value) > maxaction) { value = lastValue - maxaction; }
  // check, if the PID-value has not changed too fast since last time and - 
  // if necessary correct it, considering if it got too big or too small
  
  lastDifference = diff;
  
  return value;
  // return the - if necessary modified - PID-value
}



/** Discreet and iterative value calculation
*/

double PIDsmoothedValue::apprDiscrVal(double targetVal, double time, double maxIteration) {
  
  
  if (time >= lastTime + maxIteration)  // maxIteration is the upper bound
  {
    resetTo(targetVal);
    return value; 
  }
  
  if (time < lastTime + 1)
  {
    return value;
  }

  
  while (time >= lastTime + 1)
  {
    value = approximateVal(lastValue + 1/(time-lastTime)*(targetVal-lastValue));
    // the target value will be interpolated while executing more than one
    // step
    
    ++lastTime;
  }
  return value;
}


/**
* Calculation of the returned PID-value, just an alternative approach
* without any boundary checks and the D-value is not just calculated by
* the change of the target to object difference but by the object position change.
* During first tests the upper algorithm "approximateValue" showed a better
* convergence behavior that this one and is considered as the original algorithm
*/

double PIDsmoothedValue::approximateValNew(double targetVal) 
{
  
  double diff = targetVal - value;
  // target to object difference = P - part of the sum
  
  double tDiff = targetVal - lastTargetVal;
  if (!started) {
    tDiff = 0;
    started = true;
  }
  // target to object difference = D - part of the sum - here is the 
  // parts where both algorithms approximateValue and approximateValNew
  // differ
  
  integralError += diff;
  // target to object difference = I - part of the sum
  
  lastTargetVal = targetVal;
  
  
  value += p*diff + i*integralError + d*tDiff;
  // generate the weighted sum of the P, I, D - parts and get an unchecked PID-value
  
  
  
  return value;
  // return the calculated PID-value
}

/** Continuous calculation of the PID - values at ones ()
* non iterative, allows non aequidistant values
*/

double PIDsmoothedValue::apprContVal(double targetVal, double time) 
{
  double diff = targetVal - value;  
  double dDiff = diff - lastDifference; 
  double timeDiff = time - lastTime;
  double diffQuot = 0;
  
  if (!started) 
  {
    dDiff = 0;
    started = true;
  }
  
  integralError += diff*timeDiff*timeDiff;
  
  if (timeDiff != 0) 
  {
    diffQuot = dDiff;
  }
  
  lastValue = value;
  lastDifference = diff;
  lastTime = time;
  
  value += p*diff*timeDiff + i*integralError + d*diffQuot;
  
  if (value < min) 
  {
    value = min;
  }
  // check, if the PID-value is not too small and - if necessary correct it 
  
  if (value > max) 
  { 
    value = max;
  }
  // check, if the PID-value is not too big and - if necessary correct it 
  
  
  if ((value - lastValue) > maxaction) 
  {
    value = lastValue + maxaction;
  } 
  
  if ((lastValue - value) > maxaction) 
  {
    value = lastValue - maxaction;
  }
  // check, if the PID-value has not changed too fast since last time and - 
  // if necessary correct it, considering if it got too big or too small
  
  
  return value; 
}


/**
* To regain the PID-value, if it was already calculated
*/

double PIDsmoothedValue::getVal() {      return value;  }


/** get the P-weight */
double PIDsmoothedValue::getWeightP() {  return p;      }

/** get the I-weight */
double PIDsmoothedValue::getWeightI() {  return i;      }

/** get the D-weight */
double PIDsmoothedValue::getWeightD() {  return d;      }

double PIDsmoothedValue::getMin() {  return min;      }
double PIDsmoothedValue::getMax() {  return max;      }


/**
* reset all values and the sums necessary for its calculation
*/

void PIDsmoothedValue::reset() 
{
  
  lastTargetVal = 0;  // reset the last target value
  value  = 0;         // reset the recent PID-value
  integralError = 0;  // reset the recent sum for the I-value
  started = false;
  lastTime = 0;
  
}  

void PIDsmoothedValue::resetTo(double val) 
{
  reset();
  value = val;
  approximateVal(val);
}  

double PIDsmoothedValue::addOffset(double offset)
{
  value += offset;
  lastValue += offset;
  return value;
}

double PIDsmoothedValue::setTo(double val)
{
  double offset = val - value;
  return addOffset(offset);
}

---