diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 16039e109..8c90d1c6d 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -105,16 +105,19 @@ void TorqueControllerMux::tick( // Apply setpoints value limits // Derating for endurance - applyDerate(&drivetrainCommand_, accDerateFactor); + + if (muxMode_ != TC_CASE_SYSTEM) - { + { // Safety checks for CASE: CASE handles regen, torque, and power limit internally applyRegenLimit(&drivetrainCommand_, &drivetrainData); // Apply torque limit before power limit to not power limit applyTorqueLimit(&drivetrainCommand_); applyPowerLimit(&drivetrainCommand_, &drivetrainData); - } + } else{ + applyDerate(&drivetrainCommand_, accDerateFactor); + } // Uniformly apply speed limit to all controller modes applyPosSpeedLimit(&drivetrainCommand_); @@ -178,10 +181,18 @@ void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s *command, const Dr */ void TorqueControllerMux::applyDerate(DrivetrainCommand_s *command, float accDerateFactor) { - for (int i = 0; i < NUM_MOTORS; i++) - { - command->torqueSetpoints[i] *= accDerateFactor; + if( command->speeds_rpm[0] != 0 && + command->speeds_rpm[1] != 0 && + command->speeds_rpm[2] != 0 && + command->speeds_rpm[3] != 0) { + for (int i = 0; i < NUM_MOTORS; i++) + { + command->torqueSetpoints[i] *= accDerateFactor; + } + } + + } /*