From adfe7c37b333c245256885aa3d8ac1077e24ff18 Mon Sep 17 00:00:00 2001
From: Sebastian Reichel <sre@ring0.de>
Date: Fri, 2 Nov 2012 19:24:33 +0100
Subject: make PID timing more precise, setup P, I and D

---
 ctrl/pid-controller.vala | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

(limited to 'ctrl/pid-controller.vala')

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();
 	}
 }
-- 
cgit v1.2.3