diff options
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(); } } |