Skip to content

Instantly share code, notes, and snippets.

@aemarkov

aemarkov/PID_controller.cs

Created July 28, 2019 16:07
Show Gist options
  • Save aemarkov/284995fda2ca10772774a20a88103506 to your computer and use it in GitHub Desktop.
Save aemarkov/284995fda2ca10772774a20a88103506 to your computer and use it in GitHub Desktop.
Download ZIP
Simple implementation of the PID-controller on C#
Raw
PID_controller.cs
class PID <T>
{
private float _kGain; // P (пропорциональный) коэффициент
private float _iGain; // I (интегральный) коэффициент
private float _dGain; // D (дифференциальный) коэффициент
private T _dState; // Значение с предыдущего шага, используется для вычисления D-составляющей
private T _iSTate; // Накопленная ошибка, используется для вычисления I-составляющей
private T _iMax; // Минимальное и максимальное значение накопленной ошибки, чтобы
private T _iMin; // она не улетела куда-то очень далеко
public PID(/*...*/)
{
// Какой-то конструктор, передать коэффициенты или еще что-нибудь
}
/// <summary>
/// Расчитывает управление с помощью ПИД-регулятора
/// </summary>
/// <param name="current">Текущее положение системы</param>
/// <param name="target">Целевое положение системы</param>
/// <returns>
/// </returns>
public Calc(T current, T target)
{
T error = target - current;
// Расчет П-составляющей
// Она просто пропрциональна отклонению
var pTerm = _pGain * error;
// Расчет И-составлющей
// Она пропорциональна суммарному накопленному отклонению
// И составляющая позволяет учесть какую-то систематическую ошибку,
// например, постоянно приложннную силу
_iState += error;
if(_iState > _iMax) _iState = _iMax;
if(_iState < _iMin) _iState = _iMin;
var iTerm = _iGain * _iState;
// Рассчет Д-составляющей
// Она пропорциональна скорости изменения положения системы
// (производной, по-сути)
var dTerm = _dGain * (current - _dState);
_dState = current;
return pTerm + iTerm + dTerm;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment