diff core/Pid.cpp @ 149:134bff10e561 ClockSync

Added simple one-variable one-measurement Kalman filter, Pid controller(which output is not used). Virtual clock is now much more precise and reactive for period. Still it is lagging behind a bit on the overall offset.

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/core/Pid.cpp	Mon Sep 21 03:12:21 2015 +0100
@@ -0,0 +1,43 @@
+/*
+ * Pid.cpp
+ *
+ *  Created on: 14 Sep 2015
+ *      Author: giulio
+ */
+
+#include "Pid.h"
+
+Pid::Pid(){
+	integralError = 0;
+	pastError = 0;
+	error = 0;
+	differentialError = 0;
+	kp = 5;
+	ki = 2;
+	kd = 0.8;
+	idleTimeout = 0;
+	timeoutCount = 0;
+	output = 0;
+//	printf("----------%f %f %f %f %f %f\n", kp, error, ki, integralError, kd, differentialError);
+}
+void Pid::updateIntegralError(){
+	integralError += (error + pastError) * 0.5 * ts;
+}
+void Pid::updateDifferentialError(){
+	differentialError = (error - pastError) / ts;
+}
+float Pid::setError(float anError){
+	pastError = error;
+	error = anError;
+	ts = 1; //TODO: this should be passed as a parameter, unless setError() gets always called at constant intervals
+	updateIntegralError();
+	updateDifferentialError();
+	output= kp * error + ki * integralError + kd * differentialError;
+//	printf("%f %f %f %f %f %f %f\n", output, kp, error, ki, integralError, kd, differentialError);
+	return output;
+}
+float Pid::getOutput(){
+	return output;
+}
+
+