92 lines
2.6 KiB
C#
92 lines
2.6 KiB
C#
|
|
|
|
using System;
|
|
|
|
namespace ln.ethercat.controller
|
|
{
|
|
public delegate double LoopGetProcessValueDelegate();
|
|
|
|
public abstract class ControlLoop
|
|
{
|
|
public Controller Controller { get; }
|
|
|
|
public LoopGetProcessValueDelegate GetProcessValueDelegate { get; set; }
|
|
|
|
public double SetPoint { get; set; }
|
|
public double ErrorValue { get; private set; }
|
|
public double Output { get; private set; }
|
|
|
|
public ControlLoop(Controller controller, LoopGetProcessValueDelegate getProcessValueDelegate)
|
|
{
|
|
Controller = controller;
|
|
GetProcessValueDelegate = getProcessValueDelegate;
|
|
}
|
|
|
|
public virtual void Clear()
|
|
{
|
|
ErrorValue = 0;
|
|
Output = 0;
|
|
}
|
|
public virtual void Loop() => Loop(GetProcessValueDelegate());
|
|
public virtual void Loop(double processValue)
|
|
{
|
|
ErrorValue = SetPoint - processValue;
|
|
Output = LoopImplementation();
|
|
}
|
|
|
|
protected abstract double LoopImplementation();
|
|
protected abstract void ClearImplementation();
|
|
}
|
|
|
|
public class PIDControlLoop : ControlLoop
|
|
{
|
|
public double Kp { get; set; } = 1.0;
|
|
public double Ki { get; set; } = 1.0;
|
|
public double Tn {
|
|
get => 1.0 / Ki;
|
|
set => Ki = (value == 0) ? 0.0 : (1.0 / value);
|
|
}
|
|
|
|
public double Kd { get; set; } = 1.0;
|
|
|
|
|
|
public PIDControlLoop(Controller controller, LoopGetProcessValueDelegate getProcessValueDelegate)
|
|
:base(controller, getProcessValueDelegate)
|
|
{}
|
|
|
|
public double Integral { get; set; }
|
|
public double MinIntregal { get; set; } = double.MinValue;
|
|
public double MaxIntegral { get; set; } = double.MaxValue;
|
|
public double LastError { get; private set; }
|
|
|
|
protected override void ClearImplementation()
|
|
{
|
|
Integral = Math.Clamp(0, MinIntregal, MaxIntegral);
|
|
LastError = 0;
|
|
}
|
|
|
|
protected override double LoopImplementation()
|
|
{
|
|
double output = (Kp * ErrorValue);
|
|
|
|
if (Ki > 0.0)
|
|
{
|
|
Integral += (ErrorValue * Controller.ControllerLoopInterval * Ki) * Kp;
|
|
Integral = Math.Clamp(Integral, MinIntregal, MaxIntegral);
|
|
output += Integral;
|
|
}
|
|
if (Kd != 0)
|
|
{
|
|
output += (Kd * (ErrorValue - LastError) * Controller.ControllerLoopInterval) * Kp;
|
|
LastError = ErrorValue;
|
|
}
|
|
|
|
return output;
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
} |