When in zero velocity mode, ensure we don't apply any ki

mjbots · Mar 12, 2022 · 482459d · 482459d
1 parent 111b288
commit 482459d
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 3 deletions.
diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc
@@ -1710,8 +1710,9 @@ class BldcServo::Impl {
 
   void ISR_DoZeroVelocity(const SinCos& sin_cos, CommandData* data) MOTEUS_CCM_ATTRIBUTE {
     PID::ApplyOptions apply_options;
-    apply_options.kp_scale = 0.0;
-    apply_options.kd_scale = 1.0;
+    apply_options.kp_scale = 0.0f;
+    apply_options.kd_scale = 1.0f;
+    apply_options.ki_scale = 0.0f;
 
     ISR_DoPositionCommon(sin_cos, data,
                          apply_options, config_.timeout_max_torque_Nm,

diff --git a/fw/pid.h b/fw/pid.h
@@ -100,6 +100,7 @@ class PID {
   struct ApplyOptions {
     float kp_scale = 1.0f;
     float kd_scale = 1.0f;
+    float ki_scale = 1.0f;
 
     ApplyOptions() {}
   };
@@ -153,7 +154,8 @@ class PID {
     state_->d = apply_options.kd_scale * config_->kd * state_->error_rate;
     state_->pd = state_->p + state_->d;
 
-    state_->command = config_->sign * (state_->pd + state_->integral);
+    state_->command = config_->sign *
+        (state_->pd + apply_options.ki_scale * state_->integral);
 
     return state_->command;
   }