completed the improved version of swerve drive simulation

src/main/java/frc/robot/constants/DriveControlLoops.java

@@ -11,7 +11,7 @@ public class DriveControlLoops {
         Math.toRadians(60),
         0.02,
         Math.toRadians(3),
-        0.05,
+        0.12,
         true,
         0
 );

src/main/java/frc/robot/constants/DriveTrainConstants.java

@@ -8,8 +8,6 @@
 /**
  * stores the constants and PID configs for chassis
  * because we want an all-real simulation for the chassis, the numbers are required to be considerably precise
- * TODO: you need to change some of these numbers to fit your robot!
- * TODO: If you are using REV Chassis, please remove ALL the references to {@link TunerConstants} and enter your numbers by hand
  * */
 public class DriveTrainConstants {
     /* dead configs, don't change them */
@@ -29,6 +27,7 @@ public class DriveTrainConstants {
 
 
     /**
+     * numbers that needs to be changed to fit each robot
      * TODO: change these numbers to match your robot
      *  for REV chassis, replace ALL references to {@link TunerConstants} with actual numbers
      *  */
@@ -40,27 +39,12 @@ public class DriveTrainConstants {
             ROBOT_MASS_KG = 40, // with bumpers
             STEER_FRICTION_VOLTAGE = TunerConstants.kSteerFrictionVoltage,
             DRIVE_FRICTION_VOLTAGE = TunerConstants.kDriveFrictionVoltage,
+            DRIVE_INERTIA = 0.01,
             STEER_INERTIA = 0.01;
 
-    /* don't change the equations */
-    private static final double GRAVITY_CONSTANT = 9.8;
-    public static final double
-            DRIVE_BASE_RADIUS = Math.hypot(TunerConstants.kFrontLeftXPosInches, TunerConstants.kFrontLeftYPosInches),
-            /* friction_force = normal_force * coefficient_of_friction */
-            MAX_FRICTION_ACCELERATION = GRAVITY_CONSTANT * WHEEL_COEFFICIENT_OF_FRICTION,
-            /* force = torque / distance */
-            MAX_PROPELLING_FORCE_NEWTONS = DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS,
-            /* floor_speed = wheel_angular_velocity * wheel_radius */
-            CHASSIS_MAX_VELOCITY = DRIVE_MOTOR.freeSpeedRadPerSec / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS , // calculate using choreo
-            CHASSIS_MAX_ACCELERATION_MPS_SQ = Math.min(
-                    MAX_FRICTION_ACCELERATION, // cannot exceed max friction
-                    MAX_PROPELLING_FORCE_NEWTONS / ROBOT_MASS_KG
-            ),
-            CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC = CHASSIS_MAX_VELOCITY / DRIVE_BASE_RADIUS,
-            CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = CHASSIS_MAX_ACCELERATION_MPS_SQ / DRIVE_BASE_RADIUS,
-            CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION = MAX_FRICTION_ACCELERATION / DRIVE_BASE_RADIUS;
-
-    /* translations of the modules to the robot center, in FL, FR, BL, BR */
+    /**
+     * translations of the modules to the robot center, in FL, FR, BL, BR
+     * */
     public static final Translation2d[] MODULE_TRANSLATIONS = new Translation2d[] {
             new Translation2d(
                     Units.inchesToMeters(TunerConstants.kFrontLeftXPosInches),
@@ -80,6 +64,25 @@ public class DriveTrainConstants {
             )
     };
 
+    /* equations that calculates some constants for the simulator (don't modify) */
+    private static final double GRAVITY_CONSTANT = 9.8;
+    public static final double
+            DRIVE_BASE_RADIUS = MODULE_TRANSLATIONS[0].getNorm(),
+            /* friction_force = normal_force * coefficient_of_friction */
+            MAX_FRICTION_ACCELERATION = GRAVITY_CONSTANT * WHEEL_COEFFICIENT_OF_FRICTION,
+            MAX_FRICTION_FORCE_PER_MODULE = MAX_FRICTION_ACCELERATION * ROBOT_MASS_KG / MODULE_TRANSLATIONS.length,
+            /* force = torque / distance */
+            MAX_PROPELLING_FORCE_NEWTONS = DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS,
+            /* floor_speed = wheel_angular_velocity * wheel_radius */
+            CHASSIS_MAX_VELOCITY = DRIVE_MOTOR.freeSpeedRadPerSec / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS , // calculate using choreo
+            CHASSIS_MAX_ACCELERATION_MPS_SQ = Math.min(
+                    MAX_FRICTION_ACCELERATION, // cannot exceed max friction
+                    MAX_PROPELLING_FORCE_NEWTONS / ROBOT_MASS_KG
+            ),
+            CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC = CHASSIS_MAX_VELOCITY / DRIVE_BASE_RADIUS,
+            CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = CHASSIS_MAX_ACCELERATION_MPS_SQ / DRIVE_BASE_RADIUS,
+            CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION = MAX_FRICTION_ACCELERATION / DRIVE_BASE_RADIUS;
+
     public static final SwerveDriveKinematics DRIVE_KINEMATICS = new SwerveDriveKinematics(MODULE_TRANSLATIONS);
 
     /* for collision detection in simulation */

src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java

@@ -74,14 +74,35 @@ public void updateSim(double periodSecs) {
         steerSim.update(periodSecs);
     }
 
-    public double getDriveAppliedVolts() {
+    public double getSimulationTorque() {
+        return DRIVE_MOTOR.getTorque(DRIVE_MOTOR.getCurrent(
+                physicsSimulationResults.driveWheelFinalVelocityRadPerSec * DRIVE_GEAR_RATIO,
+                driveAppliedVolts
+        ));
+    }
+
+    public double getAppliedVolts() {
         return driveAppliedVolts;
     }
 
-    public Rotation2d getSteerFacing() {
+    public Rotation2d getSimulationSteerFacing() {
         return Rotation2d.fromRadians(steerSim.getAngularPositionRad());
     }
 
+    public SwerveModuleState getSimulationSwerveState() {
+        return new SwerveModuleState(
+                physicsSimulationResults.driveWheelFinalVelocityRadPerSec * WHEEL_RADIUS_METERS,
+                getSimulationSteerFacing()
+        );
+    }
+
+    public SwerveModuleState getDesiredSwerveState() {
+        return new SwerveModuleState(
+                driveAppliedVolts * CHASSIS_MAX_VELOCITY,
+                getSimulationSteerFacing()
+        );
+    }
+
     /**
      * this replaces DC Motor Sim for drive wheels
      * */

src/main/java/frc/robot/subsystems/drive/SwerveModule.java

@@ -17,6 +17,7 @@
 import frc.robot.utils.Alert;
 import frc.robot.utils.CustomMaths.SwerveStateProjection;
 import frc.robot.utils.CustomPIDs.MaplePIDController;
+import org.littletonrobotics.junction.Logger;
 
 import static frc.robot.constants.DriveControlLoops.*;
 
@@ -54,7 +55,7 @@ public SwerveModule(ModuleIO io, String name) {
 
     public void updateOdometryInputs() {
         io.updateInputs(inputs);
-        // Logger.processInputs("Drive/Module-" + name, inputs);
+        Logger.processInputs("Drive/Module-" + name, inputs);
         this.hardwareFaultAlert.setActivated(!inputs.hardwareConnected);
     }
 

src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java

@@ -93,16 +93,16 @@ protected void simulateTranslationalFrictionNoMotion() {
             super.setLinearVelocity(new Vector2());
     }
 
-    private void simulateChassisRotationalBehavior(double desiredRotationalMotionPercent) {
+    protected void simulateChassisRotationalBehavior(double desiredRotationalMotionPercent) {
         final double maximumTorque = this.profile.maxAngularAcceleration * super.getMass().getInertia();
-        if (Math.abs(desiredRotationalMotionPercent) > 0.03) {
+        if (Math.abs(desiredRotationalMotionPercent) > 0.01) {
             super.applyTorque(desiredRotationalMotionPercent * maximumTorque);
             return;
         }
 
         final double actualRotationalMotionPercent = Math.abs(getAngularVelocity() / profile.maxAngularVelocity),
                 frictionalTorqueMagnitude = this.profile.angularFrictionAcceleration * super.getMass().getInertia();
-        if (actualRotationalMotionPercent > 0.03)
+        if (actualRotationalMotionPercent > 0.01)
             super.applyTorque(Math.copySign(frictionalTorqueMagnitude, -super.getAngularVelocity()));
         else
             super.setAngularVelocity(0);
@@ -158,10 +158,10 @@ public RobotSimulationProfile(double robotMaxVelocity, double robotMaxAccelerati
             this.robotMass = robotMass;
             this.propellingForce = robotMaxAcceleration * robotMass;
             this.frictionForce = DriveTrainConstants.MAX_FRICTION_ACCELERATION * robotMass;
-            this.linearVelocityDamping = robotMaxAcceleration / robotMaxVelocity;
+            this.linearVelocityDamping = robotMaxAcceleration / robotMaxVelocity * 0.3;
             this.maxAngularVelocity = maxAngularVelocity;
             this.maxAngularAcceleration = robotMaxAcceleration / (Math.hypot(width, height) / 2);
-            this.angularDamping = maxAngularAcceleration / maxAngularVelocity;
+            this.angularDamping = maxAngularAcceleration / maxAngularVelocity * 0.3;
             this.angularFrictionAcceleration = DriveTrainConstants.CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION;
             this.width = width;
             this.height = height;

src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/SwerveDriveSimulation.java

@@ -1,16 +1,20 @@
 package frc.robot.utils.CompetitionFieldUtils.Simulations;
 
+import edu.wpi.first.math.MathUtil;
 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.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
 import frc.robot.Robot;
 import frc.robot.constants.LogPaths;
 import frc.robot.subsystems.drive.IO.GyroIOSim;
 import frc.robot.subsystems.drive.IO.ModuleIOSim;
 import frc.robot.subsystems.drive.IO.OdometryThread;
+import frc.robot.utils.CustomMaths.GeometryConvertor;
 import frc.robot.utils.MapleTimeUtils;
+import org.dyn4j.geometry.Vector2;
 
 import java.util.Arrays;
 import java.util.function.Consumer;
@@ -55,40 +59,112 @@ public void resetOdometryToActualRobotPose() {
     public void updateSimulationSubTick(int tickNum, double tickSeconds) {
         for (int i = 0; i < modules.length; i++)
             moduleSimulationSubTick(
+                    getObjectOnFieldPose2d(),
                     modules[i],
                     MODULE_TRANSLATIONS[i],
                     tickNum, tickSeconds
             );
 
-        final ChassisSpeeds chassisFreeSpeeds = DRIVE_KINEMATICS.toChassisSpeeds(Arrays.stream(modules).map((
-                moduleIOSim -> new SwerveModuleState(
-                        moduleIOSim.physicsSimulationResults.driveWheelFinalVelocityRadPerSec * WHEEL_RADIUS_METERS,
-                        moduleIOSim.getSteerFacing()
-                ))).toArray(SwerveModuleState[]::new));
-
-        // TODO: here we apply the friction force that brings the chassis from its floor speed to free speed
-
+        simulateFrictionForce();
 
         gyroSimulationSubTick(
-                gyroIOSim,
                 super.getObjectOnFieldPose2d().getRotation(),
                 super.getAngularVelocity(),
                 tickNum
         );
     }
 
-    private static void moduleSimulationSubTick(
+    private void moduleSimulationSubTick(
+            Pose2d robotWorldPose,
             ModuleIOSim module,
-            Translation2d moduleTranslation,
+            Translation2d moduleTranslationOnRobot,
             int tickNum,
             double tickPeriodSeconds
     ) {
-        // TODO: here, apply the propelling force of each module on the robot
-        //  and feed its odometry drive position and velocity back to the results
+        /* update the DC motor simulation of the steer */
+        module.updateSim(tickPeriodSeconds);
+
+        /* simulate the propelling force of the module */
+        final Rotation2d moduleWorldFacing = module.getSimulationSteerFacing().plus(robotWorldPose.getRotation());
+        final Vector2 moduleWorldPosition = GeometryConvertor.toDyn4jVector2(
+                robotWorldPose.getTranslation()
+                        .plus(moduleTranslationOnRobot.rotateBy(robotWorldPose.getRotation()))
+        );
+        double actualPropellingForceOnFloorNewtons = module.getSimulationTorque() / WHEEL_RADIUS_METERS;
+        final boolean skidding = Math.abs(actualPropellingForceOnFloorNewtons) > MAX_FRICTION_FORCE_PER_MODULE;
+        if (skidding)
+            actualPropellingForceOnFloorNewtons = Math.copySign(MAX_FRICTION_FORCE_PER_MODULE, actualPropellingForceOnFloorNewtons);
+        super.applyForce(
+                Vector2.create(actualPropellingForceOnFloorNewtons, moduleWorldFacing.getRadians()),
+                moduleWorldPosition
+        );
+
+
+        final Vector2 floorVelocity = super.getLinearVelocity(moduleWorldPosition);
+        final double floorVelocityProjectionOnWheelDirectionMPS = floorVelocity.getMagnitude() *
+                Math.cos(floorVelocity.getAngleBetween(new Vector2(moduleWorldFacing.getRadians())));
+
+        if (skidding)
+            /* if the chassis is skidding, the toque will cause the wheels to spin freely */
+            module.physicsSimulationResults.driveWheelFinalVelocityRadPerSec += module.getSimulationTorque() / DRIVE_INERTIA * tickPeriodSeconds;
+        else
+            /* otherwise, the floor velocity is projected to the wheel */
+            module.physicsSimulationResults.driveWheelFinalVelocityRadPerSec = floorVelocityProjectionOnWheelDirectionMPS / WHEEL_RADIUS_METERS;
+
+        module.physicsSimulationResults.odometrySteerPositions[tickNum] = module.getSimulationSteerFacing();
+        module.physicsSimulationResults.driveWheelFinalRevolutions += Units.radiansToRotations(
+                module.physicsSimulationResults.driveWheelFinalVelocityRadPerSec * tickPeriodSeconds
+        );
+        module.physicsSimulationResults.odometryDriveWheelRevolutions[tickNum] = module.physicsSimulationResults.driveWheelFinalRevolutions;
     }
 
-    private static void gyroSimulationSubTick(
-            GyroIOSim gyroIOSim,
+    private void simulateFrictionForce() {
+        final ChassisSpeeds speedsDifference = getDifferenceBetweenFloorAndFreeSpeed();
+        final Translation2d translationalSpeedsDifference = new Translation2d(speedsDifference.vxMetersPerSecond, speedsDifference.vyMetersPerSecond);
+        final double forceMultiplier = Math.min(translationalSpeedsDifference.getNorm() * 3, 1);
+        super.applyForce(Vector2.create(
+                forceMultiplier * MAX_FRICTION_ACCELERATION * ROBOT_MASS_KG,
+                translationalSpeedsDifference.getAngle().getRadians()
+        ));
+
+        if (Math.abs(getDesiredSpeedsFieldRelative().omegaRadiansPerSecond)
+                / CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC
+                < 0.01)
+            simulateChassisRotationalBehavior(0);
+    }
+
+    private ChassisSpeeds getDifferenceBetweenFloorAndFreeSpeed() {
+        ChassisSpeeds chassisFreeSpeedsFieldRelative = getFreeSpeedsFieldRelative();
+
+        final double freeSpeedMagnitude = Math.hypot(chassisFreeSpeedsFieldRelative.vxMetersPerSecond, chassisFreeSpeedsFieldRelative.vyMetersPerSecond),
+                floorSpeedMagnitude = Math.hypot(getMeasuredChassisSpeedsFieldRelative().vxMetersPerSecond, getMeasuredChassisSpeedsFieldRelative().vyMetersPerSecond);
+        if (freeSpeedMagnitude > floorSpeedMagnitude)
+            chassisFreeSpeedsFieldRelative = chassisFreeSpeedsFieldRelative.times(floorSpeedMagnitude / freeSpeedMagnitude);
+
+        return chassisFreeSpeedsFieldRelative.minus(getMeasuredChassisSpeedsFieldRelative());
+    }
+
+    private ChassisSpeeds getFreeSpeedsFieldRelative() {
+        return ChassisSpeeds.fromRobotRelativeSpeeds(
+                DRIVE_KINEMATICS.toChassisSpeeds(
+                        Arrays.stream(modules)
+                                .map(ModuleIOSim::getSimulationSwerveState)
+                                .toArray(SwerveModuleState[]::new)),
+                getObjectOnFieldPose2d().getRotation()
+        );
+    }
+
+    private ChassisSpeeds getDesiredSpeedsFieldRelative() {
+        return ChassisSpeeds.fromRobotRelativeSpeeds(
+                DRIVE_KINEMATICS.toChassisSpeeds(
+                        Arrays.stream(modules)
+                                .map(ModuleIOSim::getDesiredSwerveState)
+                                .toArray(SwerveModuleState[]::new)),
+                getObjectOnFieldPose2d().getRotation()
+        );
+    }
+
+    private void gyroSimulationSubTick(
             Rotation2d currentFacing,
             double angularVelocityRadPerSec,
             int tickNum