Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Tools/Math/PIDsmoothedValue.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file PIDsmoothedValue.cpp
    00003 * Implements class PIDsmoothedValue
    00004 * that calculates the PIDvalue for a given value
    00005 *
    00006 * @author <a href="mailto:goehring@informatik.hu-berlin.de">Daniel Goehring</a>
    00007 */
    00008
    00009
    00010 #include "PIDsmoothedValue.h"
    00011 #include "Tools/Debugging/DebugDrawings.h"
    00012
    00013
    00014 /**
    00015 * Default constructor contains default vaules and weights.
    00016 * Value types are: First result value = 0; P-weight,I-weight,D-weight,
    00017 * Min PID - Value, Max PID - Value, Max PID - value change since recent PID - value.
    00018 *
    00019 */
    00020
    00021 PIDsmoothedValue::PIDsmoothedValue():value(0.0),p(0.5),i(0.5),d(0.5),min(-10000),max(10000),maxaction(5000)
    00022 {
    00023 }
    00024
    00025
    00026 /**
    00027 * Constructor for user chosen weights and value
    00028 * types as above in the default constructor.
    00029 */
    00030
    00031 PIDsmoothedValue::PIDsmoothedValue(double value, double p, double i, double d, double min, double max, double maxaction):
    00032   lastDifference(0.0),
    00033   lastTargetVal(0.0),
    00034   value(value),
    00035   lastValue(0.0),
    00036   started(false),
    00037   integralError(0.0),
    00038   lastTime(0.0),
    00039   p(p), i(i), d(d),
    00040   min(min), max(max),
    00041   maxaction(maxaction)
    00042 {}
    00043
    00044
    00045 /**
    00046 * Set the min value
    00047 */
    00048
    00049 void PIDsmoothedValue::setMin(double m)
    00050 {
    00051   PIDsmoothedValue::min = m;
    00052 }
    00053
    00054
    00055 /**
    00056 * Set the max value
    00057 */
    00058
    00059 void PIDsmoothedValue::setMax(double m)
    00060 {
    00061   PIDsmoothedValue::max = m;
    00062 }
    00063
    00064
    00065 /**
    00066 * Set the weight for P afterwards, i.e. after using the constructor
    00067 */
    00068
    00069 void PIDsmoothedValue::setWeightP(double p)
    00070 {
    00071   PIDsmoothedValue::p = p;
    00072 }
    00073
    00074
    00075 /**
    00076 * Set the weight for I afterwards, i.e. after using the constructor
    00077 */
    00078
    00079 void PIDsmoothedValue::setWeightI(double i)
    00080 {
    00081   PIDsmoothedValue::i = i;
    00082 }
    00083
    00084
    00085 /**
    00086 * Set the weight for D afterwards, i.e. after using the constructor
    00087 */
    00088
    00089 void PIDsmoothedValue::setWeightD(double d)
    00090 {
    00091   PIDsmoothedValue::d = d;
    00092 }
    00093
    00094
    00095 /**
    00096 * Calculation of the returned PID-value
    00097 * thereby it will not just calculate the PID-sum but also ensure that it lies
    00098 * within the given boundaries
    00099 */
    00100
    00101 double PIDsmoothedValue::approximateVal(double targetVal)
    00102 {
    00103
    00104   double diff = targetVal - value;
    00105   // target to object difference = P - part of the sum
    00106   // i.e. Proportional value
    00107
    00108   double dDiff = diff - lastDifference;
    00109
    00110   if (!started)
    00111   {
    00112     dDiff = 0;
    00113     started = true;
    00114   }
    00115   // object to target difference change since last turn = D - part of the sum
    00116   // i.e. Differential value
    00117
    00118   integralError += diff;
    00119   // the sum of all target to object differences = I - part of the sum
    00120   // i.e. Integral values, because it is an integral for non discrete state functions
    00121
    00122   lastValue = value;
    00123   // just remember the new value for the next calculation
    00124
    00125   value += p*diff + i*integralError + d*dDiff;
    00126   // generate the weighted sum of the P, I, D - parts and get an unchecked PID-value
    00127
    00128
    00129   if (value < min)
    00130   {
    00131     value = min;
    00132   }
    00133   // check, if the PID-value is not too small and - if necessary correct it
    00134
    00135   if (value > max)
    00136   {
    00137     value = max;
    00138   }
    00139   // check, if the PID-value is not too big and - if necessary correct it
    00140
    00141
    00142   if ((value - lastValue) > maxaction) { value = lastValue + maxaction; }
    00143   else if ((lastValue - value) > maxaction) { value = lastValue - maxaction; }
    00144   // check, if the PID-value has not changed too fast since last time and -
    00145   // if necessary correct it, considering if it got too big or too small
    00146
    00147   lastDifference = diff;
    00148
    00149   return value;
    00150   // return the - if necessary modified - PID-value
    00151 }
    00152
    00153
    00154
    00155 /** Discreet and iterative value calculation
    00156 */
    00157
    00158 double PIDsmoothedValue::apprDiscrVal(double targetVal, double time, double maxIteration) {
    00159
    00160
    00161   if (time >= lastTime + maxIteration)  // maxIteration is the upper bound
    00162   {
    00163     resetTo(targetVal);
    00164     return value;
    00165   }
    00166
    00167   if (time < lastTime + 1)
    00168   {
    00169     return value;
    00170   }
    00171
    00172
    00173   while (time >= lastTime + 1)
    00174   {
    00175     value = approximateVal(lastValue + 1/(time-lastTime)*(targetVal-lastValue));
    00176     // the target value will be interpolated while executing more than one
    00177     // step
    00178
    00179     ++lastTime;
    00180   }
    00181   return value;
    00182 }
    00183
    00184
    00185 /**
    00186 * Calculation of the returned PID-value, just an alternative approach
    00187 * without any boundary checks and the D-value is not just calculated by
    00188 * the change of the target to object difference but by the object position change.
    00189 * During first tests the upper algorithm "approximateValue" showed a better
    00190 * convergence behavior that this one and is considered as the original algorithm
    00191 */
    00192
    00193 double PIDsmoothedValue::approximateValNew(double targetVal)
    00194 {
    00195
    00196   double diff = targetVal - value;
    00197   // target to object difference = P - part of the sum
    00198
    00199   double tDiff = targetVal - lastTargetVal;
    00200   if (!started) {
    00201     tDiff = 0;
    00202     started = true;
    00203   }
    00204   // target to object difference = D - part of the sum - here is the
    00205   // parts where both algorithms approximateValue and approximateValNew
    00206   // differ
    00207
    00208   integralError += diff;
    00209   // target to object difference = I - part of the sum
    00210
    00211   lastTargetVal = targetVal;
    00212
    00213
    00214   value += p*diff + i*integralError + d*tDiff;
    00215   // generate the weighted sum of the P, I, D - parts and get an unchecked PID-value
    00216
    00217
    00218
    00219   return value;
    00220   // return the calculated PID-value
    00221 }
    00222
    00223 /** Continuous calculation of the PID - values at ones ()
    00224 * non iterative, allows non aequidistant values
    00225 */
    00226
    00227 double PIDsmoothedValue::apprContVal(double targetVal, double time)
    00228 {
    00229   double diff = targetVal - value;
    00230   double dDiff = diff - lastDifference;
    00231   double timeDiff = time - lastTime;
    00232   double diffQuot = 0;
    00233
    00234   if (!started)
    00235   {
    00236     dDiff = 0;
    00237     started = true;
    00238   }
    00239
    00240   integralError += diff*timeDiff*timeDiff;
    00241
    00242   if (timeDiff != 0)
    00243   {
    00244     diffQuot = dDiff;
    00245   }
    00246
    00247   lastValue = value;
    00248   lastDifference = diff;
    00249   lastTime = time;
    00250
    00251   value += p*diff*timeDiff + i*integralError + d*diffQuot;
    00252
    00253   if (value < min)
    00254   {
    00255     value = min;
    00256   }
    00257   // check, if the PID-value is not too small and - if necessary correct it
    00258
    00259   if (value > max)
    00260   {
    00261     value = max;
    00262   }
    00263   // check, if the PID-value is not too big and - if necessary correct it
    00264
    00265
    00266   if ((value - lastValue) > maxaction)
    00267   {
    00268     value = lastValue + maxaction;
    00269   }
    00270
    00271   if ((lastValue - value) > maxaction)
    00272   {
    00273     value = lastValue - maxaction;
    00274   }
    00275   // check, if the PID-value has not changed too fast since last time and -
    00276   // if necessary correct it, considering if it got too big or too small
    00277
    00278
    00279   return value;
    00280 }
    00281
    00282
    00283 /**
    00284 * To regain the PID-value, if it was already calculated
    00285 */
    00286
    00287 double PIDsmoothedValue::getVal() {      return value;  }
    00288
    00289
    00290 /** get the P-weight */
    00291 double PIDsmoothedValue::getWeightP() {  return p;      }
    00292
    00293 /** get the I-weight */
    00294 double PIDsmoothedValue::getWeightI() {  return i;      }
    00295
    00296 /** get the D-weight */
    00297 double PIDsmoothedValue::getWeightD() {  return d;      }
    00298
    00299 double PIDsmoothedValue::getMin() {  return min;      }
    00300 double PIDsmoothedValue::getMax() {  return max;      }
    00301
    00302
    00303 /**
    00304 * reset all values and the sums necessary for its calculation
    00305 */
    00306
    00307 void PIDsmoothedValue::reset()
    00308 {
    00309
    00310   lastTargetVal = 0;  // reset the last target value
    00311   value  = 0;         // reset the recent PID-value
    00312   integralError = 0;  // reset the recent sum for the I-value
    00313   started = false;
    00314   lastTime = 0;
    00315
    00316 }
    00317
    00318 void PIDsmoothedValue::resetTo(double val)
    00319 {
    00320   reset();
    00321   value = val;
    00322   approximateVal(val);
    00323 }
    00324
    00325 double PIDsmoothedValue::addOffset(double offset)
    00326 {
    00327   value += offset;
    00328   lastValue += offset;
    00329   return value;
    00330 }
    00331
    00332 double PIDsmoothedValue::setTo(double val)
    00333 {
    00334   double offset = val - value;
    00335   return addOffset(offset);
    00336 }
    00337