Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Simulation/Algorithm/PID.cs

@@ -0,0 +1,55 @@
+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;
+    }
+}