Working rate limiting solution

Author: OrionSword

src/main/java/frc/robot/Constants.java

@@ -114,6 +114,11 @@ public static final class ModuleConstants {
 
   public static final class OIConstants {
     public static final int kDriverControllerPort = 0;
+    public static final double kDriveDeadband = 0.05;
+    public static final double kMagnitudeDeadband = 0.05;
+    public static final double kDirectionSlewRate = 1.2; // radians per second
+    public static final double kMagnitudeSlewRate = 1.8; // percent per second (1 = 100%)
+    public static final double kRotationalSlewRate = 2.0; // percent per second (1 = 100%)
   }
 
   public static final class AutoConstants {

src/main/java/frc/robot/RobotContainer.java

@@ -4,7 +4,6 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -51,10 +50,10 @@ public RobotContainer() {
         // Turning is controlled by the X axis of the right stick.
         new RunCommand(
             () -> m_robotDrive.drive(
-                MathUtil.applyDeadband(-m_driverController.getLeftY(), 0.06),
-                MathUtil.applyDeadband(-m_driverController.getLeftX(), 0.06),
-                MathUtil.applyDeadband(-m_driverController.getRightX(), 0.06),
-                true),
+                Math.max(0.0, (Math.abs(m_driverController.getLeftY())-OIConstants.kDriveDeadband)/(1.0-OIConstants.kDriveDeadband)) * Math.signum(-m_driverController.getLeftY()),
+                Math.max(0.0, (Math.abs(m_driverController.getLeftX())-OIConstants.kDriveDeadband)/(1.0-OIConstants.kDriveDeadband)) * Math.signum(-m_driverController.getLeftX()),
+                Math.max(0.0, (Math.abs(m_driverController.getRightX())-OIConstants.kDriveDeadband)/(1.0-OIConstants.kDriveDeadband)) * Math.signum(-m_driverController.getRightX()),
+                true, true),
             m_robotDrive));
   }
 
@@ -117,6 +116,6 @@ public Command getAutonomousCommand() {
     m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
 
     // Run path following command, then stop at the end.
-    return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
+    return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false, false));
   }
 }

src/main/java/frc/utils/SwerveUtils.java

@@ -0,0 +1,89 @@
+package frc.utils;
+
+public class SwerveUtils {
+
+    /**
+     * Steps a value towards a target with a specified step size.
+     * @param _current The current or starting value.  Can be positive or negative.
+     * @param _target The target value the algorithm will step towards.  Can be positive or negative.
+     * @param _stepsize The maximum step size that can be taken.
+     * @return The new value for {@code _current} after performing the specified step towards the specified target.
+     */
+    public static double StepTowards(double _current, double _target, double _stepsize) {
+        if (Math.abs(_current - _target) <= _stepsize) {
+            return _target;
+        }
+        else if (_target < _current) {
+            return _current - _stepsize;
+        }
+        else {
+            return _current + _stepsize;
+        }
+    }
+
+    /**
+     * Steps a value (angle) towards a target (angle) taking the shortest path with a specified step size.
+     * @param _current The current or starting angle (in radians).  Can lie outside the 0 to 2*PI range.
+     * @param _target The target angle (in radians) the algorithm will step towards.  Can lie outside the 0 to 2*PI range.
+     * @param _stepsize The maximum step size that can be taken (in radians).
+     * @return The new angle (in radians) for {@code _current} after performing the specified step towards the specified target.
+     * This value will always lie in the range 0 to 2*PI (exclusive).
+     */
+    public static double StepTowardsCircular(double _current, double _target, double _stepsize) {
+        _current = WrapAngle(_current);
+        _target = WrapAngle(_target);
+
+        double stepDirection = Math.signum(_target - _current);
+        double difference = Math.abs(_current - _target);
+        
+        if (difference <= _stepsize) {
+            return _target;
+        }
+        else if (difference > Math.PI) { //does the system need to wrap over eventually?
+            //handle the special case where you can reach the target in one step while also wrapping
+            if (_current + 2*Math.PI - _target < _stepsize || _target + 2*Math.PI - _current < _stepsize) {
+                return _target;
+            }
+            else {
+                return WrapAngle(_current - stepDirection * _stepsize); //this will handle wrapping gracefully
+            }
+
+        }
+        else {
+            return _current + stepDirection * _stepsize;
+        }
+    }
+
+    /**
+     * Finds the (unsigned) minimum difference between two angles including calculating across 0.
+     * @param _angleA An angle (in radians).
+     * @param _angleB An angle (in radians).
+     * @return The (unsigned) minimum difference between the two angles (in radians).
+     */
+    public static double AngleDifference(double _angleA, double _angleB) {
+        double difference = Math.abs(_angleA - _angleB);
+        return difference > Math.PI? (2 * Math.PI) - difference : difference;
+    }
+
+    /**
+     * Wraps an angle until it lies within the range from 0 to 2*PI (exclusive).
+     * @param _angle The angle (in radians) to wrap.  Can be positive or negative and can lie multiple wraps outside the output range.
+     * @return An angle (in radians) from 0 and 2*PI (exclusive).
+     */
+    public static double WrapAngle(double _angle) {
+        double twoPi = 2*Math.PI;
+
+        if (_angle == twoPi) { // Handle this case separately to avoid floating point errors with the floor after the division in the case below
+            return 0.0;
+        }
+        else if (_angle > twoPi) {
+            return _angle - twoPi*Math.floor(_angle / twoPi);
+        }
+        else if (_angle < 0.0) {
+            return _angle + twoPi*(Math.floor((-_angle) / twoPi)+1);
+        }
+        else {
+            return _angle;
+        }
+    }
+}