WIP

TestApp/PidController.cs

@@ -0,0 +1,31 @@
+using TestApp.Configuration;
+
+namespace TestApp
+{
+    internal class PidController
+    {
+        private DateTime _PreviousUpdateTime;
+        private double _PreviousError;
+        private double _Integral;
+
+        public PidSetting PositiveSettings { get; set; } = new PidSetting();
+        public PidSetting NegativeSettings { get; set; } = new PidSetting();
+        public double SetPoint { get; set; }
+
+        public double Update(double measurement) {
+            var error = measurement - SetPoint;
+            var dt = (DateTime.UtcNow - _PreviousUpdateTime).TotalSeconds;
+
+            var settings = error > 0 ? PositiveSettings : NegativeSettings;
+
+            var proportional = error * settings.KP;
+            _Integral += error * settings.KI * dt;
+            var differential = (error - _PreviousError) * settings.KD / dt;
+
+            _PreviousError = error;
+            _PreviousUpdateTime = DateTime.UtcNow;
+
+            return proportional + _Integral + differential;
+        }
+    }
+}