update pid in arduino sketch

2 files changed, 5 insertions, 6 deletions

diff --git a/zumo/pid.cpp b/zumo/pid.cpp
index fabe1da..7c1cbda 100644
--- a/zumo/pid.cpp
+++ b/zumo/pid.cpp
@@ -41,13 +41,12 @@ void PID::reset(float value) {
 	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);
+PID speed_pid = PID();
+PID steer_pid = PID();
+PID speed_mod_pid = PID();
 void apply_pid(dui_state_t* target, dui_state_t* current) {
 	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;
+	current->speed_mod = speed_mod_pid.iter(target->speed_mod);
 }
 
diff --git a/zumo/pid.h b/zumo/pid.h
index ef733d6..beb73ac 100644
--- a/zumo/pid.h
+++ b/zumo/pid.h
@@ -11,7 +11,7 @@ private:
 	const float N = 10.0;
 
 public:
-	PID(float P, float I, float D);
+	PID(float P = -0.02, float I = 0.13, float D = -300.0);
 	float iter(float target);
 	void reset(float value);
 };