diff options
author | Sebastian Reichel <sre@ring0.de> | 2012-11-02 19:24:33 +0100 |
---|---|---|
committer | Sebastian Reichel <sre@ring0.de> | 2012-11-02 19:24:33 +0100 |
commit | adfe7c37b333c245256885aa3d8ac1077e24ff18 (patch) | |
tree | 215e00e7e64a042fabbbd8b6ffc412137c223992 /ctrl/pid-controller.vala | |
parent | a0bd460f7f901d48d111511d9e7c2d92aaac0fa2 (diff) | |
download | microcopterd-master.tar.bz2 |
Diffstat (limited to 'ctrl/pid-controller.vala')
-rw-r--r-- | ctrl/pid-controller.vala | 16 |
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(); } } |