make PID timing more precise, setup P, I and DHEADmaster

1 files changed, 10 insertions, 6 deletions

diff --git a/ctrl/pid-controller.vala b/ctrl/pid-controller.vala
index 5866fe5..ae2add0 100644
--- a/ctrl/pid-controller.vala
+++ b/ctrl/pid-controller.vala
@@ -22,16 +22,20 @@ public class PID {
 	double I;
 	double D;
 
+	Timer  timer;
 	double lastPosition;
-	uint64 previousPIDTime;
 	double integratedError;
 	double windupGuard;
 
+	public void calibrate(double P, double I, double D) {
+		this.P = P;
+		this.I = I;
+		this.D = D;
+	}
+
 	public double update(double targetPosition, double currentPosition) {
-		/* calculate time delta */
-		uint64 currentTime = time_t();
-		uint64 deltaPIDTime = currentTime - previousPIDTime;
-		previousPIDTime = currentTime;
+		/* delta time in seconds */
+		double deltaPIDTime = timer.ellapsed() / 1000000.0;
 
 		/* calculate error */
 		double error = targetPosition - currentPosition;
@@ -46,6 +50,6 @@ public class PID {
 
 	public void clear() {
 		integratedError = 0;
-		previousPIDTime = time_t();
+		timer.start();
 	}
 }