1 files changed, 21 insertions, 23 deletions

diff --git a/zumo/pid.cpp b/zumo/pid.cpp
index 594d136..9bef08f 100644
--- a/zumo/pid.cpp
+++ b/zumo/pid.cpp
@@ -1,35 +1,33 @@
 #include "pid.h"
 
-class PID {
-private:
-	float A0, A1, A2;
-	float error[3] = {0};
-	float dt = 0.010;
-	float output = 0;
+PID::PID(float P, float I, float D) {
+	A0 = P + I*dt + D/dt;
+	A1 = -P - 2*D/dt;
+	A2 = D/dt;
+}
 
-public:
-	PID(float P, float I, float D) {
-		A0 = P + I*dt + D/dt;
-		A1 = -P - 2*D/dt;
-		A2 = D/dt;
-	}
+// https://en.wikipedia.org/wiki/PID_controller#Pseudocode
+float PID::iter(float target) {
+	error[2] = error[1];
+	error[1] = error[0];
+	error[0] = target - output;
+	output = output + A0 * error[0] + A1 * error[1] + A2 * error[2];
+	return output;
+}
 
-	// https://en.wikipedia.org/wiki/PID_controller#Pseudocode
-	float iter(float current, float target) {
-		error[2] = error[1];
-		error[1] = error[0];
-		error[0] = target - current;
-		output = output + A0 * error[0] + A1 * error[1] + A2 * error[2];
-		return output;
-	}
-};
+void PID::reset(float value) {
+	error[0] = 0.0;
+	error[1] = 0.0;
+	error[2] = 0.0;
+	output = value;
+}
 
 PID speed_pid(0.1, 0.0, 0.0); // TODO: tune these (garbage values)
 PID steer_pid(0.1, 0.0, 0.0);
 PID speed_mod_pid(1, 1, 1);
 void apply_pid(dui_state_t* target, dui_state_t* current) {
-	current->speed = speed_pid.iter(current->speed, target->speed);
-	current->steer = steer_pid.iter(current->steer, target->steer);
+	current->speed = speed_pid.iter(target->speed);
+	current->steer = steer_pid.iter(target->steer);
 	// current->speed_mod = speed_mod_pid.iter(current->speed_mod, target->speed_mod);
 	current->speed_mod = target->speed_mod;
 }