Fix diff drive current limiting (#779)

mjansen4857 · Oct 1, 2024 · 089dc2e · 089dc2e
1 parent 997a2bd
commit 089dc2e
Show file tree

Hide file tree

Showing 9 changed files with 37 additions and 28 deletions.
diff --git a/lib/trajectory/config.dart b/lib/trajectory/config.dart
@@ -20,10 +20,11 @@ class RobotConfig {
     required this.holonomic,
   }) : kinematics = SwerveDriveKinematics(moduleLocations);
 
-  static RobotConfig fromPrefs(SharedPreferences prefs) {
-    ModuleConfig moduleConfig = ModuleConfig.fromPrefs(prefs);
+  factory RobotConfig.fromPrefs(SharedPreferences prefs) {
     bool holonomicMode =
         prefs.getBool(PrefsKeys.holonomicMode) ?? Defaults.holonomicMode;
+    int numMotors = holonomicMode ? 1 : 2;
+    ModuleConfig moduleConfig = ModuleConfig.fromPrefs(prefs, numMotors);
     num halfWheelbase =
         (prefs.getDouble(PrefsKeys.robotWheelbase) ?? Defaults.robotWheelbase) /
             2;
@@ -67,22 +68,20 @@ class ModuleConfig {
     required this.wheelCOF,
   });
 
-  ModuleConfig.fromPrefs(SharedPreferences prefs)
+  ModuleConfig.fromPrefs(SharedPreferences prefs, int numMotors)
       : this(
           wheelRadiusMeters: prefs.getDouble(PrefsKeys.driveWheelRadius) ??
               Defaults.driveWheelRadius,
           maxDriveVelocityMPS: prefs.getDouble(PrefsKeys.maxDriveSpeed) ??
               Defaults.maxDriveSpeed,
           driveMotor: DCMotor.fromString(
                   prefs.getString(PrefsKeys.driveMotor) ?? Defaults.driveMotor,
-                  (prefs.getBool(PrefsKeys.holonomicMode) ??
-                          Defaults.holonomicMode)
-                      ? 1
-                      : 2)
+                  numMotors)
               .withReduction(prefs.getDouble(PrefsKeys.driveGearing) ??
                   Defaults.driveGearing),
-          driveCurrentLimit: prefs.getDouble(PrefsKeys.driveCurrentLimit) ??
-              Defaults.driveCurrentLimit,
+          driveCurrentLimit: (prefs.getDouble(PrefsKeys.driveCurrentLimit) ??
+                  Defaults.driveCurrentLimit) *
+              numMotors,
           wheelCOF: prefs.getDouble(PrefsKeys.wheelCOF) ?? Defaults.wheelCOF,
         );
 

diff --git a/lib/widgets/dialogs/settings_dialog.dart b/lib/widgets/dialogs/settings_dialog.dart
@@ -640,6 +640,8 @@ class _SettingsDialogState extends State<SettingsDialog> {
                                   .setBool(PrefsKeys.holonomicMode, value);
                               setState(() {
                                 _holonomicMode = value;
+                                _optimalCurrentLimit =
+                                    _calculateOptimalCurrentLimit();
                               });
                               widget.onSettingsChanged();
                             },
@@ -963,11 +965,12 @@ class _SettingsDialogState extends State<SettingsDialog> {
   }
 
   num _calculateOptimalCurrentLimit() {
-    int numModules = _holonomicMode ? 4 : 2;
-    DCMotor driveMotor = DCMotor.fromString(_driveMotor, _holonomicMode ? 1 : 2)
-        .withReduction(_driveGearing);
-    num moduleFrictionForce = (_wheelCOF * (_mass * 9.8)) / numModules;
-    num maxFrictionTorque = moduleFrictionForce * _wheelRadius;
-    return maxFrictionTorque / driveMotor.kTNMPerAmp;
+    final int numModules = _holonomicMode ? 4 : 2;
+    final int numMotors = _holonomicMode ? 1 : 2;
+    final DCMotor driveMotor =
+        DCMotor.fromString(_driveMotor, numMotors).withReduction(_driveGearing);
+    final num moduleFrictionForce = (_wheelCOF * (_mass * 9.8)) / numModules;
+    final num maxFrictionTorque = moduleFrictionForce * _wheelRadius;
+    return (maxFrictionTorque / driveMotor.kTNMPerAmp) / numMotors;
   }
 }
diff --git a/pathplannerlib-python/pathplannerlib/config.py b/pathplannerlib-python/pathplannerlib/config.py
@@ -37,7 +37,7 @@ class ModuleConfig:
     torqueLoss: float
 
     def __init__(self, wheelRadiusMeters: float, maxDriveVelocityMPS: float, wheelCOF: float,
-                 driveMotor: DCMotor, driveCurrentLimit: float):
+                 driveMotor: DCMotor, driveCurrentLimit: float, numMotors: int):
         """
         Configuration of a robot drive module. This can either be a swerve module,
         or one side of a differential drive train.
@@ -47,12 +47,13 @@ def __init__(self, wheelRadiusMeters: float, maxDriveVelocityMPS: float, wheelCO
         :param wheelCOF: The coefficient of friction between the drive wheel and the carpet. If you are unsure, just use a placeholder value of 1.0.
         :param driveMotor: The DCMotor representing the drive motor gearbox, including gear reduction
         :param driveCurrentLimit: The current limit of the drive motor, in Amps
+        :param numMotors: The number of motors per module. For swerve, this is 1. For differential, this is usually 2.
         """
         self.wheelRadiusMeters = wheelRadiusMeters
         self.maxDriveVelocityMPS = maxDriveVelocityMPS
         self.wheelCOF = wheelCOF
         self.driveMotor = driveMotor
-        self.driveCurrentLimit = driveCurrentLimit
+        self.driveCurrentLimit = driveCurrentLimit * numMotors
 
         self.maxDriveVelocityRadPerSec = self.maxDriveVelocityMPS / self.wheelRadiusMeters
         maxSpeedCurrentDraw = self.driveMotor.current(self.maxDriveVelocityRadPerSec, 12.0)
@@ -200,7 +201,8 @@ def fromGUISettings() -> 'RobotConfig':
                 maxDriveSpeed,
                 wheelCOF,
                 gearbox,
-                driveCurrentLimit
+                driveCurrentLimit,
+                numMotors
             )
 
             if isHolonomic:

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java
@@ -225,7 +225,7 @@ public static Command warmupCommand() {
                 75,
                 6.8,
                 new ModuleConfig(
-                    0.048, 5.0, 1.2, DCMotor.getKrakenX60(1).withReduction(6.14), 60.0),
+                    0.048, 5.0, 1.2, DCMotor.getKrakenX60(1).withReduction(6.14), 60.0, 1),
                 0.55),
             () -> true)
         .andThen(Commands.print("[PathPlanner] FollowPathCommand finished warmup"))

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java
@@ -401,7 +401,7 @@ public static Command warmupCommand() {
                 75,
                 6.8,
                 new ModuleConfig(
-                    0.048, 5.0, 1.2, DCMotor.getKrakenX60(1).withReduction(6.14), 60.0),
+                    0.048, 5.0, 1.2, DCMotor.getKrakenX60(1).withReduction(6.14), 60.0, 1),
                 0.55))
         .andThen(Commands.print("[PathPlanner] PathfindingCommand finished warmup"))
         .ignoringDisable(true);

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/config/ModuleConfig.java b/pathplannerlib/src/main/java/com/pathplanner/lib/config/ModuleConfig.java
@@ -34,18 +34,21 @@ public class ModuleConfig {
    *     unsure, just use a placeholder value of 1.0.
    * @param driveMotor The DCMotor representing the drive motor gearbox, including gear reduction
    * @param driveCurrentLimit The current limit of the drive motor, in Amps
+   * @param numMotors The number of motors per module. For swerve, this is 1. For differential, this
+   *     is usually 2.
    */
   public ModuleConfig(
       double wheelRadiusMeters,
       double maxDriveVelocityMPS,
       double wheelCOF,
       DCMotor driveMotor,
-      double driveCurrentLimit) {
+      double driveCurrentLimit,
+      int numMotors) {
     this.wheelRadiusMeters = wheelRadiusMeters;
     this.maxDriveVelocityMPS = maxDriveVelocityMPS;
     this.wheelCOF = wheelCOF;
     this.driveMotor = driveMotor;
-    this.driveCurrentLimit = driveCurrentLimit;
+    this.driveCurrentLimit = driveCurrentLimit * numMotors;
 
     this.maxDriveVelocityRadPerSec = this.maxDriveVelocityMPS / this.wheelRadiusMeters;
     double maxSpeedCurrentDraw = this.driveMotor.getCurrent(this.maxDriveVelocityRadPerSec, 12.0);

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/config/RobotConfig.java b/pathplannerlib/src/main/java/com/pathplanner/lib/config/RobotConfig.java
@@ -202,7 +202,8 @@ public static RobotConfig fromGUISettings() throws IOException, ParseException {
     gearbox = gearbox.withReduction(gearing);
 
     ModuleConfig moduleConfig =
-        new ModuleConfig(wheelRadius, maxDriveSpeed, wheelCOF, gearbox, driveCurrentLimit);
+        new ModuleConfig(
+            wheelRadius, maxDriveSpeed, wheelCOF, gearbox, driveCurrentLimit, numMotors);
 
     if (isHolonomic) {
       return new RobotConfig(massKG, MOI, moduleConfig, trackwidth, wheelbase);

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/config/RobotConfig.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/config/RobotConfig.cpp
@@ -72,7 +72,7 @@ RobotConfig RobotConfig::fromGUISettings() {
 			numMotors).WithReduction(gearing);
 
 	ModuleConfig moduleConfig(wheelRadius, maxDriveSpeed, wheelCOF, gearbox,
-			driveCurrentLimit);
+			driveCurrentLimit, numMotors);
 
 	if (isHolonomic) {
 		return RobotConfig(mass, MOI, moduleConfig, trackwidth, wheelbase);

diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/config/ModuleConfig.h b/pathplannerlib/src/main/native/include/pathplanner/lib/config/ModuleConfig.h
@@ -35,13 +35,14 @@ class ModuleConfig {
 	 *     unsure, just use a placeholder value of 1.0.
 	 * @param driveMotor The DCMotor representing the drive motor gearbox, including gear reduction
 	 * @param driveCurrentLimit The current limit of the drive motor, in Amps
+	 * @param numMotors The number of motors per module. For swerve, this is 1. For differential, this is usually 2.
 	 */
 	ModuleConfig(units::meter_t wheelRadius,
 			units::meters_per_second_t maxDriveVelocityMPS, double wheelCOF,
-			frc::DCMotor driveMotor, units::ampere_t driveCurrentLimit) : wheelRadius(
-			wheelRadius), maxDriveVelocityMPS(maxDriveVelocityMPS), wheelCOF(
-			wheelCOF), driveMotor(driveMotor), driveCurrentLimit(
-			driveCurrentLimit), maxDriveVelocityRadPerSec {
+			frc::DCMotor driveMotor, units::ampere_t driveCurrentLimit,
+			int numMotors) : wheelRadius(wheelRadius), maxDriveVelocityMPS(
+			maxDriveVelocityMPS), wheelCOF(wheelCOF), driveMotor(driveMotor), driveCurrentLimit(
+			driveCurrentLimit * numMotors), maxDriveVelocityRadPerSec {
 			maxDriveVelocityMPS() / wheelRadius() }, torqueLoss(
 			driveMotor.Torque(
 					units::math::min(