Whiplash141

Whip's PID Class

Aug 12th, 2017
1,392
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 2.32 KB | None | 0 0
  1. //Whip's PID controller class v6 - 11/22/17
  2. public class PID
  3. {
  4.     double _kP = 0;
  5.     double _kI = 0;
  6.     double _kD = 0;
  7.     double _integralDecayRatio = 0;
  8.     double _lowerBound = 0;
  9.     double _upperBound = 0;
  10.     double _timeStep = 0;
  11.     double _inverseTimeStep = 0;
  12.     double _errorSum = 0;
  13.     double _lastError = 0;
  14.     bool _firstRun = true;
  15.     bool _integralDecay = false;
  16.     public double Value { get; private set; }
  17.  
  18.     public PID(double kP, double kI, double kD, double lowerBound, double upperBound, double timeStep)
  19.     {
  20.         _kP = kP;
  21.         _kI = kI;
  22.         _kD = kD;
  23.         _lowerBound = lowerBound;
  24.         _upperBound = upperBound;
  25.         _timeStep = timeStep;
  26.         _inverseTimeStep = 1 / _timeStep;
  27.         _integralDecay = false;
  28.     }
  29.  
  30.     public PID(double kP, double kI, double kD, double integralDecayRatio, double timeStep)
  31.     {
  32.         _kP = kP;
  33.         _kI = kI;
  34.         _kD = kD;
  35.         _timeStep = timeStep;
  36.         _inverseTimeStep = 1 / _timeStep;
  37.         _integralDecayRatio = integralDecayRatio;
  38.         _integralDecay = true;
  39.     }
  40.  
  41.     public double Control(double error)
  42.     {
  43.         //Compute derivative term
  44.         var errorDerivative = (error - _lastError) * _inverseTimeStep;
  45.  
  46.         if (_firstRun)
  47.         {
  48.             errorDerivative = 0;
  49.             _firstRun = false;
  50.         }
  51.  
  52.         //Compute integral term
  53.         if (!_integralDecay)
  54.         {
  55.             _errorSum += error * _timeStep;
  56.  
  57.             //Clamp integral term
  58.             if (_errorSum > _upperBound)
  59.                 _errorSum = _upperBound;
  60.             else if (_errorSum < _lowerBound)
  61.                 _errorSum = _lowerBound;
  62.         }
  63.         else
  64.         {
  65.             _errorSum = _errorSum * (1.0 - _integralDecayRatio) + error * _timeStep;
  66.         }
  67.  
  68.         //Store this error as last error
  69.         _lastError = error;
  70.  
  71.         //Construct output
  72.         this.Value = _kP * error + _kI * _errorSum + _kD * errorDerivative;
  73.         return this.Value;
  74.     }
  75.    
  76.     public double Control(double error, double timeStep)
  77.     {
  78.         _timeStep = timeStep;
  79.         _inverseTimeStep = 1 / _timeStep;
  80.         return Control(error);
  81.     }
  82.  
  83.     public void Reset()
  84.     {
  85.         _errorSum = 0;
  86.         _lastError = 0;
  87.         _firstRun = true;
  88.     }
  89. }
Advertisement
Add Comment
Please, Sign In to add comment