From 6ac2f43587fe76f5135edfe71908def4b41de015 Mon Sep 17 00:00:00 2001
From: Gary Servin <garyservin@gmail.com>
Date: Fri, 27 Oct 2023 21:33:15 -0300
Subject: [PATCH] Only send motor commands if pid is enabled

Signed-off-by: Gary Servin <gary@servin.dev>
---
 andino_firmware/src/app.cpp | 24 ++++++++++++++----------
 andino_firmware/src/pid.cpp |  9 ++++++++-
 andino_firmware/src/pid.h   | 10 +++++++---
 3 files changed, 29 insertions(+), 14 deletions(-)

diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp
index 86700174..2f338253 100644
--- a/andino_firmware/src/app.cpp
+++ b/andino_firmware/src/app.cpp
@@ -168,15 +168,19 @@ void App::adjust_motors_speed() {
   int right_motor_speed = 0;
   left_pid_controller_.compute(left_encoder_.read(), left_motor_speed);
   right_pid_controller_.compute(right_encoder_.read(), right_motor_speed);
-  left_motor_.set_speed(left_motor_speed);
-  right_motor_.set_speed(right_motor_speed);
+  if (left_pid_controller_.enabled()){
+    left_motor_.set_speed(left_motor_speed);
+  }
+  if (right_pid_controller_.enabled()){
+    right_motor_.set_speed(right_motor_speed);
+  }
 }
 
 void App::stop_motors() {
   left_motor_.set_speed(0);
   right_motor_.set_speed(0);
-  left_pid_controller_.enable(false);
-  right_pid_controller_.enable(false);
+  left_pid_controller_.disable();
+  right_pid_controller_.disable();
 }
 
 void App::cmd_unknown_cb(int, char**) { Serial.println("Unknown command."); }
@@ -228,11 +232,11 @@ void App::cmd_set_motors_speed_cb(int argc, char** argv) {
     right_motor_.set_speed(0);
     left_pid_controller_.reset(left_encoder_.read());
     right_pid_controller_.reset(right_encoder_.read());
-    left_pid_controller_.enable(false);
-    right_pid_controller_.enable(false);
+    left_pid_controller_.disable();
+    right_pid_controller_.disable();
   } else {
-    left_pid_controller_.enable(true);
-    right_pid_controller_.enable(true);
+    left_pid_controller_.enable();
+    right_pid_controller_.enable();
   }
 
   // The target speeds are in ticks per second, so we need to convert them to ticks per
@@ -253,8 +257,8 @@ void App::cmd_set_motors_pwm_cb(int argc, char** argv) {
   left_pid_controller_.reset(left_encoder_.read());
   right_pid_controller_.reset(right_encoder_.read());
   // Sneaky way to temporarily disable the PID.
-  left_pid_controller_.enable(false);
-  right_pid_controller_.enable(false);
+  left_pid_controller_.disable();
+  right_pid_controller_.disable();
   left_motor_.set_speed(left_motor_pwm);
   right_motor_.set_speed(right_motor_pwm);
   Serial.println("OK");
diff --git a/andino_firmware/src/pid.cpp b/andino_firmware/src/pid.cpp
index 83cc78a4..c89e1a3d 100644
--- a/andino_firmware/src/pid.cpp
+++ b/andino_firmware/src/pid.cpp
@@ -81,7 +81,14 @@ void PID::reset(int encoder_count) {
   last_output_ = 0;
 }
 
-void PID::enable(bool enabled) { enabled_ = enabled; }
+/// @brief Enable PID
+void PID::enable() { enabled_ = true; }
+
+/// @brief Is the PID controller enabled?
+bool PID::enabled() { return enabled_; }
+
+/// @brief Disable PID
+void PID::disable() { enabled_ = false; }
 
 void PID::compute(int encoder_count, int& computed_output) {
   if (!enabled_) {
diff --git a/andino_firmware/src/pid.h b/andino_firmware/src/pid.h
index 768f8cb7..71ab4999 100644
--- a/andino_firmware/src/pid.h
+++ b/andino_firmware/src/pid.h
@@ -55,10 +55,14 @@ class PID {
   /// @param encoder_count Current encoder value.
   void reset(int encoder_count);
 
+  /// @brief Returns if the PID controller is enabled or not.
+  bool enabled();
+
   /// @brief Enables the PID controller.
-  ///
-  /// @param enabled True to enable the PID, false otherwise.
-  void enable(bool enabled);
+  void enable();
+
+  /// @brief Disables the PID controller.
+  void disable();
 
   /// @brief Computes a new output.
   ///