initial code import

1 files changed, 55 insertions, 0 deletions

diff --git a/ctrl/pid-controller.vala b/ctrl/pid-controller.vala
new file mode 100644
index 0000000..e77cda5
--- /dev/null
+++ b/ctrl/pid-controller.vala
@@ -0,0 +1,55 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ * Copyright 2012, Ted Carancho
+ *
+ * Ported from AeroQuad
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+public class PID {
+	double P;
+	double I;
+	double D;
+
+	double lastPosition;
+	uint64 previousPIDTime;
+	double integratedError;
+	double windupGuard;
+
+	private double constrain(double x, double a, double b) {
+		return a > x ? a : b < x ? b : x;
+	}
+
+	public double update(double targetPosition, double currentPosition) {
+		/* calculate time delta */
+		uint64 currentTime = time_t();
+		uint64 deltaPIDTime = currentTime - previousPIDTime;
+		previousPIDTime = currentTime;
+
+		/* calculate error */
+		double error = targetPosition - currentPosition;
+		integratedError += error * deltaPIDTime;
+		integratedError = constrain(integratedError, -windupGuard, windupGuard);
+
+		double dTerm = D * (currentPosition - lastPosition) / (deltaPIDTime * 100);
+		lastPosition = currentPosition;
+
+		return P * error + I * integratedError + dTerm;
+	}
+
+	public void clear() {
+		integratedError = 0;
+		previousPIDTime = time_t();
+	}
+}