first commit

utils/pid.cpp

@@ -0,0 +1,28 @@
+#include "pid.h"
+
+static inline float Saturation(float val, float min, float max)
+{
+  if (val < min) val = min;
+  if (val > max) val = max;
+  return val;
+}
+
+float PID_Update(float Value, float Current, PID_Data& Coef, float de)
+{
+  float p, i, d;
+
+  float e = Value - Current;
+
+  p = e * Coef.P.C;
+  p = Saturation(p, Coef.P.Min, Coef.P.Max);
+
+  i = Coef.i;
+  i += e * Coef.I.C;
+  i = Saturation(i, Coef.I.Min, Coef.I.Max);
+  Coef.i = i;
+  
+  d = de * Coef.D.C;
+  d = Saturation(d, Coef.D.Min, Coef.D.Max);
+
+  return Saturation(p + i + d, Coef.Min, Coef.Max);
+}