Merge pull request #277 from FRC5010/dev

Maximum speed settings do not allow limiting robot to less than full speed
BroncBotz3481 · Jan 7, 2025 · f557d07 · f557d07
2 parents 6d8250a + 2152386
commit f557d07
Showing 1 changed file with 26 additions and 9 deletions.
diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
@@ -330,6 +330,8 @@ public SwerveDrive(
     checkIfTunerXCompatible();
 
     HAL.report(kResourceType_RobotDrive, kRobotDriveSwerve_YAGSL);
+    // Defaulting to something reasonable for most robots
+    setMaximumAttainableSpeeds(6, 2 * Math.PI);
   }
 
   /**
@@ -619,31 +621,46 @@ public void drive(ChassisSpeeds robotRelativeVelocity, boolean isOpenLoop, Trans
   }
 
   /**
-   * Set the maximum speeds for desaturation.
+   * Set the maximum attainable speeds for desaturation.
    *
    * @param attainableMaxTranslationalSpeedMetersPerSecond  The absolute max speed that your robot can reach while
    *                                                        translating in meters per second.
    * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can reach while rotating in
    *                                                        radians per second.
    */
-  public void setMaximumSpeeds(
+  public void setMaximumAttainableSpeeds(
       double attainableMaxTranslationalSpeedMetersPerSecond,
       double attainableMaxRotationalVelocityRadiansPerSecond)
   {
     this.attainableMaxTranslationalSpeedMetersPerSecond = attainableMaxTranslationalSpeedMetersPerSecond;
     this.attainableMaxRotationalVelocityRadiansPerSecond = attainableMaxRotationalVelocityRadiansPerSecond;
-    this.swerveController.config.maxAngularVelocity = attainableMaxRotationalVelocityRadiansPerSecond;
+  }
+
+  /**
+   * Set the maximum allowable speeds for desaturation.
+   *
+   * @param maxTranslationalSpeedMetersPerSecond  The allowable max speed that your robot should reach while
+   *                                                        translating in meters per second.
+   * @param maxRotationalVelocityRadiansPerSecond The allowable max speed the robot should reach while rotating in
+   *                                                        radians per second.
+   */
+  public void setMaximumAllowableSpeeds(
+      double maxTranslationalSpeedMetersPerSecond,
+      double maxRotationalVelocityRadiansPerSecond)
+  {
+    this.maxChassisSpeedMPS = maxTranslationalSpeedMetersPerSecond;
+    this.swerveController.config.maxAngularVelocity = maxRotationalVelocityRadiansPerSecond;
   }
 
   /**
    * Get the maximum velocity from {@link SwerveDrive#attainableMaxTranslationalSpeedMetersPerSecond} or
-   * {@link SwerveDrive#maxChassisSpeedMPS} whichever is higher.
+   * {@link SwerveDrive#maxChassisSpeedMPS} whichever is the lower limit on the robot's speed. 
    *
-   * @return Maximum speed in meters/second.
+   * @return Minimum speed in meters/second of physically attainable and user allowable limits.
    */
   public double getMaximumChassisVelocity()
   {
-    return Math.max(this.attainableMaxTranslationalSpeedMetersPerSecond, maxChassisSpeedMPS);
+    return Math.min(this.attainableMaxTranslationalSpeedMetersPerSecond, maxChassisSpeedMPS);
   }
 
   /**
@@ -668,13 +685,13 @@ public AngularVelocity getMaximumModuleAngleVelocity()
 
   /**
    * Get the maximum angular velocity, either {@link SwerveDrive#attainableMaxRotationalVelocityRadiansPerSecond} or
-   * {@link SwerveControllerConfiguration#maxAngularVelocity}.
+   * {@link SwerveControllerConfiguration#maxAngularVelocity}, whichever is the lower limit on the robot's speed.
    *
-   * @return Maximum angular velocity in radians per second.
+   * @return Minimum angular velocity in radians per second of physically attainable and user allowable limits.
    */
   public double getMaximumChassisAngularVelocity()
   {
-    return Math.max(this.attainableMaxRotationalVelocityRadiansPerSecond, swerveController.config.maxAngularVelocity);
+    return Math.min(this.attainableMaxRotationalVelocityRadiansPerSecond, swerveController.config.maxAngularVelocity);
   }
 
   /**