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 index 9ca32029..33ae91af 100644 --- 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())};