Switch example swerve sim to DCMotorSim

Mechanical-Advantage · Oct 29, 2023 · 0db94f5 · 0db94f5
1 parent 7a99352
commit 0db94f5
Showing 1 changed file with 8 additions and 14 deletions.
diff --git a/example_projects/swerve_drive/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/example_projects/swerve_drive/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
@@ -3,7 +3,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 
 /**
  * Physics sim implementation of module IO.
@@ -15,11 +15,10 @@
 public class ModuleIOSim implements ModuleIO {
   private static final double LOOP_PERIOD_SECS = 0.02;
 
-  private FlywheelSim driveSim = new FlywheelSim(DCMotor.getNEO(1), 6.75, 0.025);
-  private FlywheelSim turnSim = new FlywheelSim(DCMotor.getNEO(1), 150.0 / 7.0, 0.004);
+  private DCMotorSim driveSim = new DCMotorSim(DCMotor.getNEO(1), 6.75, 0.025);
+  private DCMotorSim turnSim = new DCMotorSim(DCMotor.getNEO(1), 150.0 / 7.0, 0.004);
 
-  private double turnRelativePositionRad = 0.0;
-  private double turnAbsolutePositionRad = Math.random() * 2.0 * Math.PI;
+  private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI);
   private double driveAppliedVolts = 0.0;
   private double turnAppliedVolts = 0.0;
 
@@ -28,19 +27,14 @@ public void updateInputs(ModuleIOInputs inputs) {
     driveSim.update(LOOP_PERIOD_SECS);
     turnSim.update(LOOP_PERIOD_SECS);
 
-    double angleDiffRad = turnSim.getAngularVelocityRadPerSec() * LOOP_PERIOD_SECS;
-    turnRelativePositionRad += angleDiffRad;
-    turnAbsolutePositionRad += angleDiffRad;
-    turnAbsolutePositionRad = MathUtil.angleModulus(turnAbsolutePositionRad);
-
-    inputs.drivePositionRad =
-        inputs.drivePositionRad + (driveSim.getAngularVelocityRadPerSec() * LOOP_PERIOD_SECS);
+    inputs.drivePositionRad = driveSim.getAngularPositionRad();
     inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec();
     inputs.driveAppliedVolts = driveAppliedVolts;
     inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())};
 
-    inputs.turnAbsolutePosition = new Rotation2d(turnAbsolutePositionRad);
-    inputs.turnPosition = new Rotation2d(turnRelativePositionRad);
+    inputs.turnAbsolutePosition =
+        new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition);
+    inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad());
     inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec();
     inputs.turnAppliedVolts = turnAppliedVolts;
     inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())};