1 files changed, 68 insertions, 0 deletions
diff --git a/regulator.h b/regulator.h
new file mode 100644
index 0000000..f243848
--- /dev/null
+++ b/regulator.h
@@ -0,0 +1,68 @@
+#ifndef __REGULATOR_H__
+#define __REGULATOR_H__
+class Regulator
+{
+	private:
+		long int lasttime;
+		bool first_run;
+		double prop,integ,diff;
+		double sum, result;
+		double damped_val;
+		double damping;
+	public:
+		void reset(double p, double i, double d, double damp)
+		{
+			prop=p;
+			integ=i;
+			diff=d;
+			damping=damp;
+			
+			first_run = true;
+			sum=0;
+			result=0.0;
+		}
+
+		void reset()
+		{
+			reset(prop,integ,diff,damping);
+		}
+
+		Regulator(double p, double i, double d, double damp)
+		{
+			reset(p,i,d,damp);
+		}
+
+		void put(double val, long int time_msec)
+		{
+			double deriv;
+			double last_damped_val;
+
+
+			if (first_run)
+			{
+				damped_val = val;
+				last_damped_val = damped_val;
+				lasttime = time_msec - 1; // prevent divison by zero
+				first_run = false;
+			}
+			else
+			{
+				last_damped_val = damped_val;
+				damped_val = damping * damped_val + (1-damping) * val;
+			}
+
+			double delta_t = (time_msec - lasttime) / 1000.;
+			lasttime = time_msec;
+
+			sum += val * delta_t;
+			deriv = (damped_val - last_damped_val) / delta_t;
+
+			result = prop * val   +   integ * sum   +   diff * deriv;
+		}
+
+		double get()
+		{
+			return result;
+		}
+};
+#endif