Make vel_ramp safer by limiting the vel_setpoint to vel_limit

odriverobotics · Sep 22, 2022 · e88fa10 · e88fa10
1 parent f64c075
commit e88fa10
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
@@ -267,6 +267,13 @@ bool Controller::update() {
 
     }
 
+    // Never command a setpoint beyond its limit
+    if(config_.enable_vel_limit) {
+        vel_setpoint_ = std::clamp(vel_setpoint_, -config_.vel_limit, config_.vel_limit);
+    }
+    const float Tlim = axis_->motor_.max_available_torque();
+    torque_setpoint_ = std::clamp(torque_setpoint_, -Tlim, Tlim);
+
     // Position control
     // TODO Decide if we want to use encoder or pll position here
     float gain_scheduling_multiplier = 1.0f;
@@ -373,7 +380,6 @@ bool Controller::update() {
 
     // Torque limiting
     bool limited = false;
-    float Tlim = axis_->motor_.max_available_torque();
     if (torque > Tlim) {
         limited = true;
         torque = Tlim;