Calculate real speed internally

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

@@ -121,6 +121,47 @@ public static ChassisSpeeds correctForDynamics(ChassisSpeeds requestedSpeeds) {
     return correctedSpeeds;
   }
 
+  public static SwerveModuleState getRealModuleSpeed(Translation2d moduleLocation, ChassisSpeeds desiredSpeed) {
+    Matrix<N3, N1> firstOrderInputMatrix = new Matrix<>(N3(),N1());
+    Matrix<N2, N3> firstOrderMatrix = new Matrix<>(N2(),N3());
+    Matrix<N2, N2> rotationMatrix = new Matrix<>(N2(),N2());
+
+    firstOrderInputMatrix.set(0, 0, desiredSpeed.vxMetersPerSecond);
+    firstOrderInputMatrix.set(1, 0, desiredSpeed.vyMetersPerSecond);
+    firstOrderInputMatrix.set(2, 0, desiredSpeed.omegaRadiansPerSecond);
+
+    firstOrderMatrix.set(0, 0, 1);
+    firstOrderMatrix.set(1, 1, 1);
+
+    var swerveModuleState = new SwerveModuleState();
+
+    // Angle that the module location vector makes with respect to the robot
+    Rotation2d moduleAngle = new Rotation2d(Math.atan2(moduleLocation.getY(), moduleLocation.getX()));
+    // Angle that the module location vector makes with respect to the field for field centric if applicable
+    moduleAngle = Rotation2d.fromRadians(moduleAngle.getRadians());
+    double moduleX = moduleLocation.getNorm() * Math.cos(moduleAngle.getRadians());
+    double moduleY = moduleLocation.getNorm() * Math.sin(moduleAngle.getRadians());
+    // -r_y
+    firstOrderMatrix.set(0, 2, -moduleY);
+    // +r_x
+    firstOrderMatrix.set(1, 2, +moduleX);
+
+    Matrix<N2, N1> firstOrderOutput = firstOrderMatrix.times(firstOrderInputMatrix);
+
+    double moduleHeading = Math.atan2(firstOrderOutput.get(1, 0), firstOrderOutput.get(0, 0));
+    double moduleSpeed = Math.sqrt(firstOrderOutput.elementPower(2).elementSum());
+
+    rotationMatrix.set(0, 0, +Math.cos(moduleHeading));
+    rotationMatrix.set(0, 1, +Math.sin(moduleHeading));
+    rotationMatrix.set(1, 0, -Math.sin(moduleHeading));
+    rotationMatrix.set(1, 1, +Math.cos(moduleHeading));
+
+    // Set swerve module state
+    swerveModuleState = new SwerveModuleState(moduleSpeed, Rotation2d.fromRadians(moduleHeading));
+
+    return swerveModuleState;
+  }
+
   /**
     * Convert chassis speed to states of individual modules using second order kinematics
     *

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

@@ -28,6 +28,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.units.Current;
@@ -400,13 +401,14 @@ public void set(SwerveModuleState state) {
   /**
    * Set swerve module direction and speed, automatically applying traction control
    * @param state Desired swerve module state representing desired speed
-   * @param realState Real swerve module state representing actual speed
+   * @param realSpeeds Real speeds of robot from IMU
    */
-  public void set(SwerveModuleState state, SwerveModuleState realState) {
+  public void set(SwerveModuleState state, ChassisSpeeds realSpeeds) {
+    var inertialVelocity = Units.MetersPerSecond.of(AdvancedSwerveKinematics.getRealModuleSpeed(m_moduleCoordinate, realSpeeds).speedMetersPerSecond);
     // Apply traction control
     state.speedMetersPerSecond = m_tractionControlController.calculate(
       Units.MetersPerSecond.of(state.speedMetersPerSecond),
-      Units.MetersPerSecond.of(realState.speedMetersPerSecond),
+      inertialVelocity,
       getDriveVelocity()
     ).in(Units.MetersPerSecond);
 
@@ -425,10 +427,10 @@ public void set(SwerveModuleState[] states) {
   /**
    * Set swerve module direction and speed, automatically applying traction control
    * @param states Array of states for all swerve modules representing desired speed
-   * @param realStates Array of states for all swerve modules representing actual speed
+   * @param realSpeeds Real speeds of robot from IMU
    */
-  public void set(SwerveModuleState[] states, SwerveModuleState[] realStates) {
-    set(states[m_location.index], realStates[m_location.index]);
+  public void set(SwerveModuleState[] states, ChassisSpeeds realSpeeds) {
+    set(states[m_location.index], realSpeeds);
   }
 
   /**