81 lines
2.0 KiB
C#
81 lines
2.0 KiB
C#
namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
|
|
|
|
public class PIDConfig
|
|
{
|
|
public double Kp { get; set; }
|
|
public double Ki { get; set; }
|
|
public double Kd { get; set; }
|
|
/// <summary>
|
|
/// Integral chỉ tích lũy khi |error| <= IntegralZone.
|
|
/// Giá trị 0 = không giới hạn (integral luôn tích lũy).
|
|
/// </summary>
|
|
public double IntegralZone { get; set; }
|
|
}
|
|
|
|
public class PID(PIDConfig config)
|
|
{
|
|
private double Kp = config.Kp;
|
|
private double Ki = config.Ki;
|
|
private double Kd = config.Kd;
|
|
private double IntegralZone = config.IntegralZone;
|
|
|
|
private double _prevError;
|
|
private double _integral;
|
|
|
|
public PID WithKp(double kp)
|
|
{
|
|
Kp = kp;
|
|
return this;
|
|
}
|
|
|
|
public PID WithKi(double ki)
|
|
{
|
|
Ki = ki;
|
|
return this;
|
|
}
|
|
|
|
public PID WithKd(double kd)
|
|
{
|
|
Kd = kd;
|
|
return this;
|
|
}
|
|
|
|
public PID WithIntegralZone(double integralZone)
|
|
{
|
|
IntegralZone = integralZone;
|
|
return this;
|
|
}
|
|
|
|
public double PID_step(double error, double max, double min, double timeSample)
|
|
{
|
|
double integralStep = 0.5 * (error + _prevError) * timeSample;
|
|
|
|
// Integral Zone: chỉ tích lũy khi |error| nằm trong vùng cho phép
|
|
bool inIntegralZone = IntegralZone <= 0 || Math.Abs(error) <= IntegralZone;
|
|
if (inIntegralZone)
|
|
_integral += integralStep;
|
|
else
|
|
_integral = 0;
|
|
|
|
double derivative = (error - _prevError) / timeSample;
|
|
_prevError = error;
|
|
|
|
double Out = Kp * error
|
|
+ Ki * _integral
|
|
+ Kd * derivative;
|
|
|
|
// Anti-windup: hoàn tác integralStep khi output bị bão hòa
|
|
double clamped = Math.Clamp(Out, min, max);
|
|
if (clamped != Out && inIntegralZone)
|
|
_integral -= integralStep;
|
|
|
|
return clamped;
|
|
}
|
|
|
|
public void Reset()
|
|
{
|
|
_prevError = 0;
|
|
_integral = 0;
|
|
}
|
|
}
|