Revert "Fix real module velocity calculation"

This reverts commit ee906a0.

Author: viggy96

src/main/java/org/lasarobotics/drive/MAXSwerveModule.java

@@ -26,12 +26,11 @@
 import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.Pair;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.units.Angle;
 import edu.wpi.first.units.Current;
 import edu.wpi.first.units.Dimensionless;
 import edu.wpi.first.units.Distance;
@@ -132,14 +131,14 @@ private GearRatio(double value) {
   private GearRatio m_driveGearRatio;
   private double m_driveConversionFactor;
   private double m_rotateConversionFactor;
+  private double m_radius;
   private double m_autoLockTime;
   private boolean m_autoLock;
   private double m_runningOdometer;
   private String m_odometerOutputPath;
 
   private TractionControlController m_tractionControlController;
   private Instant m_autoLockTimer;
-  private Pair<Pose2d, Instant> m_previousPose;
 
   /**
    * Create an instance of a MAXSwerveModule
@@ -169,6 +168,7 @@ public MAXSwerveModule(Hardware swerveHardware, ModuleLocation location, GearRat
     DRIVE_METERS_PER_ROTATION = DRIVE_METERS_PER_TICK * encoderTicksPerRotation;
     DRIVE_MAX_LINEAR_SPEED = (swerveHardware.driveMotor.getKind().getMaxRPM() / 60) * DRIVE_METERS_PER_ROTATION * DRIVETRAIN_EFFICIENCY;
     COSINE_CORRECTION = RobotBase.isReal() ? 1 : 0;
+
     this.m_driveMotor = swerveHardware.driveMotor;
     this.m_rotateMotor = swerveHardware.rotateMotor;
     this.m_location = location;
@@ -181,7 +181,6 @@ public MAXSwerveModule(Hardware swerveHardware, ModuleLocation location, GearRat
     this.m_tractionControlController =  new TractionControlController(driveWheel, slipRatio, mass, Units.MetersPerSecond.of(DRIVE_MAX_LINEAR_SPEED));
     this.m_autoLockTimer = Instant.now();
     this.m_runningOdometer = 0.0;
-    this.m_previousPose = new Pair<Pose2d, Instant>(new Pose2d(), Instant.now());
 
     // Set drive encoder conversion factor
     m_driveConversionFactor = driveWheel.diameter.in(Units.Meters) * Math.PI / m_driveGearRatio.value;
@@ -268,6 +267,9 @@ public MAXSwerveModule(Hardware swerveHardware, ModuleLocation location, GearRat
     RobotModeTriggers.disabled().onTrue(Commands.runOnce(() -> disabledInit()).ignoringDisable(true));
     RobotModeTriggers.disabled().onFalse(Commands.runOnce(() -> disabledExit()).ignoringDisable(true));
 
+    // Get distance from center of robot
+    m_radius = m_moduleCoordinate.getNorm();
+
     // Make sure settings are burned to flash
     m_driveMotor.burnFlash();
     m_rotateMotor.burnFlash();
@@ -338,20 +340,12 @@ private SwerveModuleState getDesiredState(SwerveModuleState requestedState) {
 
   /**
    * Get real speed of module
-   * @param currentPose Current pose of robot
+   * @param inertialVelocity Inertial velocity of robot (m/s)
+   * @param rotateRate Rotate rate of robot (degrees/s)
    * @return Speed of module (m/s)
    */
-  private Measure<Velocity<Distance>> calculateRealSpeed(Pose2d currentPose) {
-    var previousPose = m_previousPose.getFirst();
-    var now = Instant.now();
-    var delta = Units.Milliseconds.of(Duration.between(m_previousPose.getSecond(), now).toMillis());
-    var velocity = Units.Meters.of(
-      currentPose.getTranslation().plus(m_moduleCoordinate).getDistance(previousPose.getTranslation())
-    ).per(delta);
-
-    m_previousPose = new Pair<Pose2d, Instant>(currentPose, now);
-
-    return velocity;
+  private Measure<Velocity<Distance>> calculateRealSpeed(double inertialVelocity, double rotateRate) {
+    return Units.MetersPerSecond.of(inertialVelocity + Math.toRadians(rotateRate) * m_radius);
   }
 
   /**
@@ -421,13 +415,14 @@ public void set(SwerveModuleState state) {
   /**
    * Set swerve module direction and speed, automatically applying traction control
    * @param state Desired swerve module state
-   * @param currentPose Current robot pose
+   * @param inertialVelocity Current inertial velocity
+   * @param rotateRate Current robot rotate rate
    */
-  public void set(SwerveModuleState state, Pose2d currentPose) {
+  public void set(SwerveModuleState state, Measure<Velocity<Distance>> inertialVelocity, Measure<Velocity<Angle>> rotateRate) {
     // Apply traction control
     state.speedMetersPerSecond = m_tractionControlController.calculate(
       Units.MetersPerSecond.of(state.speedMetersPerSecond),
-      calculateRealSpeed(currentPose),
+      calculateRealSpeed(inertialVelocity.in(Units.MetersPerSecond), rotateRate.in(Units.DegreesPerSecond)),
       getDriveVelocity()
     ).in(Units.MetersPerSecond);
 
@@ -446,10 +441,11 @@ public void set(SwerveModuleState[] states) {
   /**
    * Set swerve module direction and speed, automatically applying traction control
    * @param states Array of states for all swerve modules
-   * @param currentPose Current robot pose
+   * @param inertialVelocity Current inertial velocity
+   * @param rotateRate Current rotate rate
    */
-  public void set(SwerveModuleState[] states, Pose2d currentPose) {
-    set(states[m_location.index], currentPose);
+  public void set(SwerveModuleState[] states, Measure<Velocity<Distance>> inertialVelocity, Measure<Velocity<Angle>> rotateRate) {
+    set(states[m_location.index], inertialVelocity, rotateRate);
   }
 
   /**

src/main/java/org/lasarobotics/drive/TractionControlController.java

@@ -86,7 +86,7 @@ public TractionControlController(Measure<Dimensionless> staticCoF,
     this.m_mass = mass.divide(4).in(Units.Kilograms);
     this.m_maxLinearSpeed = Math.floor(maxLinearSpeed.in(Units.MetersPerSecond) * 1000) / 1000;
     this.m_maxAcceleration = m_staticCoF * GlobalConstants.GRAVITATIONAL_ACCELERATION.in(Units.MetersPerSecondPerSecond);
-    this.m_maxPredictedSlipRatio = (m_maxAcceleration * GlobalConstants.ROBOT_LOOP_HZ / 2)
+    this.m_maxPredictedSlipRatio = (m_maxAcceleration * GlobalConstants.ROBOT_LOOP_HZ)
       / (m_staticCoF * m_mass * GlobalConstants.GRAVITATIONAL_ACCELERATION.in(Units.MetersPerSecondPerSecond));
     this.m_isSlipping = false;
     this.m_slippingDebouncer = new Debouncer(MIN_SLIPPING_TIME.in(Units.Seconds), DebounceType.kRising);

src/test/java/org/lasarobotics/drive/TractionControlTest.java

@@ -25,7 +25,7 @@
 
 @TestMethodOrder(MethodOrderer.OrderAnnotation.class)
 public class TractionControlTest {
-  private final Measure<Dimensionless> SLIP_RATIO = Units.Percent.of(.0);
+  private final Measure<Dimensionless> SLIP_RATIO = Units.Percent.of(8.0);
   private final Measure<Dimensionless> STATIC_FRICTION_COEFFICIENT = Units.Value.of(1.1);
   private final Measure<Dimensionless> DYNAMIC_FRICTION_COEFFICIENT = Units.Value.of(0.8);
   private final Measure<Mass> MASS = Units.Pounds.of(110.0);