56 lines
1.2 KiB
C#
56 lines
1.2 KiB
C#
namespace RobotNet10.RobotApp.Services.Simulation.Algorithm;
|
|
|
|
public class PID
|
|
{
|
|
private double Kp = 0.3;
|
|
private double Ki = 0.0001;
|
|
private double Kd = 0.01;
|
|
|
|
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 double PID_step(double error, double max, double min, double timeSample)
|
|
{
|
|
double integralStep = 0.5 * (error + _prevError) * timeSample;
|
|
_integral += integralStep;
|
|
|
|
double derivative = (error - _prevError) / timeSample;
|
|
_prevError = error;
|
|
|
|
double Out = Kp * error
|
|
+ Ki * _integral
|
|
+ Kd * derivative;
|
|
|
|
// Anti-windup
|
|
double clamped = Math.Clamp(Out, min, max);
|
|
if (clamped != Out)
|
|
_integral -= integralStep;
|
|
|
|
return clamped;
|
|
}
|
|
|
|
public void Reset()
|
|
{
|
|
_prevError = 0;
|
|
_integral = 0;
|
|
}
|
|
}
|