message: Implement better algorithm
diff: @@ -116,12 +116,9 @@ void Controller::update_filter_gains() {
  namespace {
  float limitVel(float vel_limit, float vel_estimate, float vel_gain, float Iq, float current_lim) {
  - float Imax = (vel_limit - std::abs(vel_estimate)) * vel_gain;
  - if (vel_estimate > 0.0f) {
  - return std::clamp(Iq, -current_lim, Imax);
  - } else {
  - return std::clamp(Iq, -Imax, current_lim);
  - }
  + float Imax = std::min((vel_limit - vel_estimate) * vel_gain, current_lim);
  + float Imin = std::max((-vel_limit - vel_estimate) * vel_gain, -current_lim);
  + return std::clamp(Iq, Imin, Imax);
  }
  } // namespace
  