43 lines
1.0 KiB
C#
43 lines
1.0 KiB
C#
namespace RobotNet.RobotManager.Services.Simulation.Algorithm;
|
|
|
|
public class PID
|
|
{
|
|
private double Kp = 0.3;
|
|
private double Ki = 0.0001;
|
|
private double Kd = 0.01;
|
|
private double Pre_Error;
|
|
private double Pre_Pre_Error;
|
|
private double Pre_Out;
|
|
|
|
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 LimitMax, double LimitMin, double TimeSample)
|
|
{
|
|
double P_part = Kp * (Error - Pre_Error);
|
|
double I_part = 0.5 * Ki * TimeSample * (Error + Pre_Error);
|
|
double D_part = Kd / TimeSample * (Error - 2 * Pre_Error + Pre_Pre_Error);
|
|
double Out = Pre_Out + P_part + I_part + D_part;
|
|
Pre_Pre_Error = Pre_Error;
|
|
Pre_Error = Error;
|
|
Pre_Out = Out;
|
|
Out = MathExtension.CheckLimit(Out, LimitMax, LimitMin);
|
|
return Out;
|
|
}
|
|
}
|