76 lines
2.1 KiB
C#
76 lines
2.1 KiB
C#
using System;
|
|
|
|
namespace PlaneOnBoardSoftware
|
|
{
|
|
class PidController
|
|
{
|
|
private float lastError = 0;
|
|
private DateTime lastTime;
|
|
private float integral = 0;
|
|
private float _minOutput = 0;
|
|
private float _maxOutput = 0;
|
|
private float Spann = 0;
|
|
|
|
public float P = 0;
|
|
public float I = 0;
|
|
public float D = 0;
|
|
public float SetPoint = 0;
|
|
public float InputVariable = 0;
|
|
|
|
public bool Circle360DegMode = false;
|
|
|
|
public PidController(float MinOutput, float MaxOutput, float PropValue, float InteValue, float DiffValue, bool Circle360DegreeMode)
|
|
{
|
|
Circle360DegMode = Circle360DegreeMode;
|
|
|
|
P = PropValue;
|
|
I = InteValue;
|
|
D = DiffValue;
|
|
|
|
lastTime = DateTime.UtcNow;
|
|
_minOutput = MinOutput;
|
|
_maxOutput = MaxOutput;
|
|
Spann = MaxOutput - MinOutput;
|
|
}
|
|
|
|
public PidController(float MinOutput, float MaxOutput, float PropValue, float InteValue, float DiffValue)
|
|
{
|
|
P = PropValue;
|
|
I = InteValue;
|
|
D = DiffValue;
|
|
|
|
lastTime = DateTime.UtcNow;
|
|
_minOutput = MinOutput;
|
|
_maxOutput = MaxOutput;
|
|
Spann = MaxOutput - MinOutput;
|
|
}
|
|
|
|
public float GetOutput(float dt, float inputValue)
|
|
{
|
|
InputVariable = inputValue;
|
|
float error = SetPoint - InputVariable;
|
|
|
|
if (Circle360DegMode)
|
|
{
|
|
float errev = error - 360;
|
|
if (errev*errev < error*error) error = errev;
|
|
}
|
|
|
|
float iError = integral + P * error * dt / I;
|
|
|
|
if (iError < Spann && iError > 0)
|
|
integral = iError;
|
|
|
|
float correction = _minOutput + P * (error + D * (error - lastError) / dt) + integral;
|
|
|
|
lastError = error;
|
|
|
|
if (correction > _maxOutput)
|
|
correction = _maxOutput;
|
|
else if (correction < _minOutput)
|
|
correction = _minOutput;
|
|
|
|
return correction;
|
|
}
|
|
}
|
|
} |