Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharp/PID.cs

@@ -0,0 +1,80 @@
+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| &lt;= 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;
+    }
+}