Fix integral action for AntiWindupStrategy::NONE (#432) (#433)

Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>

Author: mergify[bot]

control_toolbox/include/control_toolbox/pid.hpp

@@ -143,7 +143,7 @@ struct AntiWindupStrategy
           "{})",
           i_min, i_max));
     }
-    if (std::isfinite(i_min) || std::isfinite(i_max))
+    if (type != LEGACY && (std::isfinite(i_min) || std::isfinite(i_max)))
     {
       std::cout << "Warning: The i_min and i_max are only valid for the deprecated LEGACY "
                    "antiwindup strategy. Please use the AntiWindupStrategy::set_type() method to "

control_toolbox/src/pid.cpp

@@ -237,7 +237,7 @@ bool Pid::set_gains(const Gains & gains_in)
   std::string error_msg = "";
   if (!gains_in.validate(error_msg))
   {
-    std::cerr << "PID: Invalid gains: " << error_msg << ". SKipping new gains." << std::endl;
+    std::cerr << "PID: Invalid gains: " << error_msg << ". Skipping new gains." << std::endl;
     return false;
   }
   else
@@ -424,6 +424,11 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
         i_term_ += dt_s * gains_.i_gain_ * error;
       }
     }
+    else if (gains_.antiwindup_strat_.type == AntiWindupStrategy::NONE)
+    {
+      // No anti-windup strategy, so just integrate the error
+      i_term_ += dt_s * gains_.i_gain_ * error;
+    }
   }
 
   return cmd_;