Add Maple-Sim

build.gradle

@@ -100,3 +100,7 @@ wpi.java.configureTestTasks(test)
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
 }
+
+repositories {
+    mavenLocal()
+}

src/main/java/frc/robot/RobotContainer.java

@@ -96,6 +96,10 @@ public RobotContainer()
    */
   private void configureBindings()
   {
+    if (Robot.isSimulation())
+    {
+      driverXbox.start().onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
+    }
     if (DriverStation.isTest())
     {
       driverXbox.b().whileTrue(drivebase.sysIdDriveMotorCommand());

src/main/java/frc/robot/subsystems/swervedrive/Vision.java

@@ -131,7 +131,7 @@ public void updatePoseEstimation(SwerveDrive swerveDrive)
   {
     if (SwerveDriveTelemetry.isSimulation)
     {
-      visionSim.update(swerveDrive.getPose());
+      visionSim.update(swerveDrive.getSimulationDriveTrainPose().orElse(swerveDrive.getPose()));
 
     }
     for (Cameras camera : Cameras.values())

src/main/java/swervelib/SwerveDrive.java

@@ -18,10 +18,8 @@
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.*;
 import edu.wpi.first.wpilibj.Alert.AlertType;
-import edu.wpi.first.wpilibj.Notifier;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.util.ArrayList;
@@ -31,6 +29,8 @@
 import java.util.Optional;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
+
+import frc.robot.Robot;
 import org.ironmaple.simulation.SimulatedArena;
 import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
 import org.ironmaple.simulation.drivesims.SwerveModuleSimulation;
@@ -48,6 +48,8 @@
 import swervelib.telemetry.SwerveDriveTelemetry;
 import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
 
+import static edu.wpi.first.units.Units.*;
+
 /**
  * Swerve Drive class representing and controlling the swerve drive.
  */
@@ -196,48 +198,50 @@ public SwerveDrive(
     kinematics = new SwerveDriveKinematics(config.moduleLocationsMeters);
     odometryThread = new Notifier(this::updateOdometry);
 
+    this.swerveModules = config.modules;
+
     // Create an integrator for angle if the robot is being simulated to emulate an IMU
     // If the robot is real, instantiate the IMU instead.
     if (SwerveDriveTelemetry.isSimulation)
     {
-      simIMU = new SwerveIMUSimulation();
-      imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L);
+      DriveTrainSimulationConfig simulationConfig = DriveTrainSimulationConfig.Default()
+              .withBumperSize(
+                      Meters.of(config.getTracklength()).plus(Inches.of(5)),
+                      Meters.of(config.getTrackwidth()).plus(Inches.of(5)))
+              .withRobotMass(Kilograms.of(config.physicalCharacteristics.robotMassKg))
+              .withCustomModuleTranslations(config.moduleLocationsMeters)
+              .withGyro(config.getGyroSim())
+              .withSwerveModule(() -> new SwerveModuleSimulation(
+                      config.getDriveMotorSim(),
+                      config.getAngleMotorSim(),
+                      config.physicalCharacteristics.driveMotorCurrentLimit,
+                      config.physicalCharacteristics.conversionFactor.drive.gearRatio,
+                      config.physicalCharacteristics.conversionFactor.angle.gearRatio,
+                      config.physicalCharacteristics.driveFrictionVoltage,
+                      config.physicalCharacteristics.angleFrictionVoltage,
+                      config.physicalCharacteristics.wheelGripCoefficientOfFriction,
+                      Units.inchesToMeters(config.physicalCharacteristics.conversionFactor.drive.diameter/2),
+                      0.02));
+
+      mapleSimDrive = new SwerveDriveSimulation(simulationConfig, startingPose);
+
+      // feed module simulation instances to modules
+      for (int i = 0; i < swerveModules.length; i++)
+      {
+        this.swerveModules[i].getSimModule().setMapleSimModule(mapleSimDrive.getModules()[i]);
+      }
 
-      // Setup MapleSim
-      SwerveModuleSimulation simModule = new SwerveModuleSimulation(config.getDriveMotorSim(),
-                                                                    config.getAngleMotorSim(),
-                                                                    config.physicalCharacteristics.driveMotorCurrentLimit,
-                                                                    config.physicalCharacteristics.conversionFactor.drive.gearRatio,
-                                                                    config.physicalCharacteristics.conversionFactor.angle.gearRatio,
-                                                                    config.physicalCharacteristics.driveFrictionVoltage,
-                                                                    config.physicalCharacteristics.angleFrictionVoltage,
-                                                                    config.physicalCharacteristics.wheelGripCoefficientOfFriction,
-                                                                    config.physicalCharacteristics.conversionFactor.drive.diameter /
-                                                                    2,
-                                                                    config.physicalCharacteristics.steerRotationalInertia);
-      DriveTrainSimulationConfig simCfg = new DriveTrainSimulationConfig(config.physicalCharacteristics.robotMassKg,
-                                                                         config.getTracklength() +
-                                                                         Units.inchesToMeters(5),
-                                                                         config.getTrackwidth() +
-                                                                         Units.inchesToMeters(5),
-                                                                         config.getTracklength(),
-                                                                         config.getTrackwidth(),
-                                                                         () -> {
-                                                                           return simModule;
-                                                                         },
-                                                                         config.getGyroSim());
-      mapleSimDrive = new SwerveDriveSimulation(simCfg, startingPose);
       // register the drivetrain simulation
       SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive);
-    } else
-    {
+
+      simIMU = new SwerveIMUSimulation(mapleSimDrive.getGyroSimulation());
+      imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L);
+    } else {
       imu = config.imu;
       imu.factoryDefault();
       imuReadingCache = new Cache<>(imu::getRotation3d, 5L);
     }
 
-    this.swerveModules = config.modules;
-
     //    odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
     swerveDrivePoseEstimator =
         new SwerveDrivePoseEstimator(
@@ -285,6 +289,10 @@ public SwerveDrive(
     }
 
     odometryThread.startPeriodic(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02);
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      SimulatedArena.overrideSimulationTimings(0.01, 2);
+    }
 
     checkIfTunerXCompatible();
   }
@@ -338,6 +346,7 @@ private void checkIfTunerXCompatible()
   public void setOdometryPeriod(double period)
   {
     odometryThread.stop();
+    SimulatedArena.overrideSimulationTimings(period, 2);
     odometryThread.startPeriodic(period);
   }
 
@@ -347,6 +356,7 @@ public void setOdometryPeriod(double period)
   public void stopOdometryThread()
   {
     odometryThread.stop();
+    SimulatedArena.overrideSimulationTimings(TimedRobot.kDefaultPeriod, 5);
   }
 
   /**
@@ -693,7 +703,7 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
   }
 
   /**
-   * Gets the current pose (position and rotation) of the robot, as reported by odometry.
+   * Gets the measured pose (position and rotation) of the robot, as reported by odometry.
    *
    * @return The robot's pose
    */
@@ -707,7 +717,25 @@ public Pose2d getPose()
   }
 
   /**
-   * Gets the current field-relative velocity (x, y and omega) of the robot
+   * Gets the actual pose of the drivetrain during simulation
+   *
+   * @return an optional Pose2d, representing the drivetrain pose during simulation, or an empty optional when running on real robot
+   * */
+  public Optional<Pose2d> getSimulationDriveTrainPose()
+  {
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      odometryLock.lock();
+      Pose2d simulationPose = mapleSimDrive.getSimulatedDriveTrainPose();
+      odometryLock.unlock();
+      return Optional.of(simulationPose);
+    }
+
+    return Optional.empty();
+  }
+
+  /**
+   * Gets the measured field-relative robot velocity (x, y and omega)
    *
    * @return A ChassisSpeeds object of the current field-relative velocity
    */
@@ -721,6 +749,24 @@ public ChassisSpeeds getFieldVelocity()
         kinematics.toChassisSpeeds(getStates()), getOdometryHeading().unaryMinus());
   }
 
+  /**
+   * Gets the actual field-relative robot velocity (x, y and omega) during simulation
+   *
+   * @return An optional ChassisSpeeds representing the actual field-relative velocity of the robot, or an empty optional when running on real robot
+   */
+  public Optional<ChassisSpeeds> getSimulationFieldVelocity()
+  {
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      odometryLock.lock();
+      ChassisSpeeds simulationFieldRelativeVelocity = mapleSimDrive.getDriveTrainSimulatedChassisSpeedsFieldRelative();
+      odometryLock.unlock();
+      return Optional.of(simulationFieldRelativeVelocity);
+    }
+
+    return Optional.empty();
+  }
+
   /**
    * Gets the current robot-relative velocity (x, y and omega) of the robot
    *
@@ -731,6 +777,24 @@ public ChassisSpeeds getRobotVelocity()
     return kinematics.toChassisSpeeds(getStates());
   }
 
+  /**
+   * Gets the actual robot-relative robot velocity (x, y and omega) during simulation
+   *
+   * @return An optional ChassisSpeeds representing the actual robot-relative velocity of the robot, or an empty optional when running on real robot
+   */
+  public Optional<ChassisSpeeds> getSimulationRobotVelocity()
+  {
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      odometryLock.lock();
+      ChassisSpeeds simulationFieldRelativeVelocity = mapleSimDrive.getDriveTrainSimulatedChassisSpeedsRobotRelative();
+      odometryLock.unlock();
+      return Optional.of(simulationFieldRelativeVelocity);
+    }
+
+    return Optional.empty();
+  }
+
   /**
    * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this
    * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to
@@ -744,6 +808,14 @@ public void resetOdometry(Pose2d pose)
     swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose);
     odometryLock.unlock();
     kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, getYaw()));
+
+    // teleport the robot to that pose if it's a simulation
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      odometryLock.lock();
+      mapleSimDrive.setSimulationWorldPose(pose);
+      odometryLock.unlock();
+    }
   }
 
   /**
@@ -1041,13 +1113,17 @@ public void updateOdometry()
       // Update odometry
       swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
 
+      if (SwerveDriveTelemetry.isSimulation)
+      {
+        SimulatedArena.getInstance().simulationPeriodic();
+      }
+
       // Update angle accumulator if the robot is simulated
       if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
       {
         Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition());
         if (SwerveDriveTelemetry.isSimulation)
         {
-          SimulatedArena.getInstance().simulationPeriodic();
           simIMU.updateOdometry(
               kinematics,
               getStates(),
@@ -1066,7 +1142,14 @@ public void updateOdometry()
 
       if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
       {
-        field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
+        if (SwerveDriveTelemetry.isSimulation)
+        {
+          field.setRobotPose(mapleSimDrive.getSimulatedDriveTrainPose());
+          field.getObject("OdometryPose").setPose(swerveDrivePoseEstimator.getEstimatedPosition());
+        } else
+        {
+          field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
+        }
       }
 
       double sumVelocity = 0;

src/main/java/swervelib/SwerveModule.java

@@ -366,7 +366,7 @@ public void setAnglePIDF(PIDFConfig config)
    */
   public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force)
   {
-    desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition()));
+    desiredState = SwerveModuleState.optimize(desiredState, getState().angle);
 
     // If we are forcing the angle
     if (!force && antiJitterEnabled)
@@ -699,4 +699,9 @@ public void updateTelemetry()
     SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition());
     SmartDashboard.putNumber(absoluteEncoderIssueName, getAbsoluteEncoderReadIssue() ? 1 : 0);
   }
+
+  public SwerveModuleSimulation getSimModule()
+  {
+    return simModule;
+  }
 }

src/main/java/swervelib/simulation/SwerveIMUSimulation.java

@@ -8,6 +8,8 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import org.ironmaple.simulation.drivesims.GyroSimulation;
+
 import java.util.Optional;
 
 /**
@@ -16,27 +18,13 @@
 public class SwerveIMUSimulation
 {
 
-  /**
-   * Main timer to control movement estimations.
-   */
-  private final Timer  timer;
-  /**
-   * The last time the timer was read, used to determine position changes.
-   */
-  private       double lastTime;
-  /**
-   * Heading of the robot.
-   */
-  private       double angle;
+  private final GyroSimulation gyroSimulation;
 
   /**
    * Create the swerve drive IMU simulation.
    */
-  public SwerveIMUSimulation()
-  {
-    timer = new Timer();
-    timer.start();
-    lastTime = timer.get();
+  public SwerveIMUSimulation(GyroSimulation gyroSimulation) {
+      this.gyroSimulation = gyroSimulation;
   }
 
   /**
@@ -46,7 +34,7 @@ public SwerveIMUSimulation()
    */
   public Rotation2d getYaw()
   {
-    return new Rotation2d(angle);
+    return gyroSimulation.getGyroReading();
   }
 
   /**
@@ -76,7 +64,7 @@ public Rotation2d getRoll()
    */
   public Rotation3d getGyroRotation3d()
   {
-    return new Rotation3d(0, 0, angle);
+    return new Rotation3d(0, 0, getYaw().getRadians());
   }
 
   /**
@@ -104,9 +92,6 @@ public void updateOdometry(
       Pose2d[] modulePoses,
       Field2d field)
   {
-
-    angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (timer.get() - lastTime);
-    lastTime = timer.get();
     field.getObject("XModules").setPoses(modulePoses);
   }
 
@@ -117,6 +102,6 @@ public void updateOdometry(
    */
   public void setAngle(double angle)
   {
-    this.angle = angle;
+    this.gyroSimulation.setRotation(Rotation2d.fromRadians(angle));
   }
 }