Update libraries and documentation

README.md

@@ -67,11 +67,11 @@ Custom library for 418 Purple Haze
 ### Required dependencies
 * AdvantageKit - https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/docs/INSTALLATION.md
 * URCL - https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json
-* NavX - https://dev.studica.com/releases/2024/NavX.json
-* REVLib - https://software-metadata.revrobotics.com/REVLib-2024.json
-* ReduxLib - https://frcsdk.reduxrobotics.com/ReduxLib_2024.json
-* CTRE Phoenix5 - https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json
-* CTRE Phoenix6 - https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json
+* NavX - https://dev.studica.com/releases/2025/Studica-2025.1.1-alpha-14.json
+* REVLib - https://software-metadata.revrobotics.com/REVLib-2025.json
+* ReduxLib - https://frcsdk.reduxrobotics.com/ReduxLib_2025.json
+* CTRE Phoenix5 - https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-beta-latest.json
+* CTRE Phoenix6 - https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json
 * PhotonLib - https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json
 
 ### Online installation
@@ -101,7 +101,35 @@ Change the file path as needed.
 
 ## Robot project integration
 
-
+1. Follow AdvantageKit setup instructions, and make your main `Robot.java` class extend `LoggedRobot`.
+2. In your `build.gradle` add the following:
+  * To the `plugins` section: `id "com.peterabeles.gversion" version "1.10"`
+  * Paste this before the `deploy` section:
+  ```
+  project.compileJava.dependsOn(createVersionFile)
+  gversion {
+    srcDir       = "src/main/java/"
+    classPackage = "frc.robot"
+    className    = "BuildConstants"
+    dateFormat   = "yyyy-MM-dd HH:mm:ss z"
+    timeZone     = "America/Chicago" // Use preferred time zone
+    indent       = "  "
+  }
+  ```
+
+3. In your `Robot.java` constructor, initialize the `PurpleManager`.
+```
+PurpleManager.initialize(
+  this,
+  AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
+  Path.of("/media/sda1"),
+  BuildConstants.MAVEN_NAME,
+  BuildConstants.GIT_SHA,
+  BuildConstants.BUILD_DATE,
+  true
+);
+```
+4. Call `PurpleManager.update()` in the beginning of the `robotPeriodic()` method of `Robot.java`.
 
 ## Releasing
 Create a release in GitHub. JitPack does the rest.

src/main/java/org/lasarobotics/drive/swerve/SwerveDrive.java

@@ -583,6 +583,7 @@ public void periodic() {
   @Override
   public void simulationPeriodic() {
     // This method will be called once per scheduler run in simulation
+    m_imu.updateSim(getPose().getRotation(), getDesiredChassisSpeeds(), m_controlCentricity);
     m_simCallbacks.stream().forEach(Runnable::run);
 
     smartDashboard();

src/main/java/org/lasarobotics/hardware/IMU.java

@@ -4,7 +4,10 @@
 
 package org.lasarobotics.hardware;
 
+import org.lasarobotics.drive.swerve.AdvancedSwerveKinematics.ControlCentricity;
+
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
@@ -79,5 +82,15 @@ public default LinearVelocity getVelocityZ() {
     return Units.MetersPerSecond.zero();
   }
 
+  /**
+   * Update IMU simulation
+   * <p>
+   * Offers a primitive IMU simulation in order to make swerve drives function correctly in the simulator
+   * @param orientation Orientation of robot on the field (typically provided by pose estimator)
+   * @param desiredSpeeds Desired speeds commanded by user
+   * @param controlCentricity Control centricity of desired
+   */
+  public void updateSim(Rotation2d orientation, ChassisSpeeds desiredSpeeds, ControlCentricity controlCentricity);
+
   public void close();
 }

src/main/java/org/lasarobotics/hardware/ctre/Pigeon2.java

@@ -4,6 +4,11 @@
 
 package org.lasarobotics.hardware.ctre;
 
+import java.time.Duration;
+import java.time.Instant;
+import java.util.concurrent.ThreadLocalRandom;
+
+import org.lasarobotics.drive.swerve.AdvancedSwerveKinematics.ControlCentricity;
 import org.lasarobotics.hardware.IMU;
 import org.lasarobotics.hardware.LoggableHardware;
 import org.lasarobotics.hardware.PurpleManager;
@@ -15,6 +20,7 @@
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
@@ -58,7 +64,10 @@ public static class Pigeon2Inputs {
 
   private static final Frequency DEFAULT_UPDATE_FREQUENCY = Units.Hertz.of(100);
 
+  private static final AngularVelocity PIGEON2_YAW_DRIFT_RATE = Units.DegreesPerSecond.of(0.25 / 60);
+
   private Notifier m_inputThread;
+  private Instant m_lastUpdateTime;
 
   private com.ctre.phoenix6.hardware.Pigeon2 m_pigeon;
 
@@ -86,7 +95,7 @@ public Pigeon2(ID id, Frequency updateRate) {
     this.m_pigeon = new com.ctre.phoenix6.hardware.Pigeon2(id.deviceID, id.bus.name);
     this.m_inputs = new Pigeon2InputsAutoLogged();
     this.m_inputThread = new Notifier(this::updateInputs);
-    this.
+    this.m_lastUpdateTime = Instant.now();
 
     m_pigeon.getRoll().setUpdateFrequency(updateRate);
     m_pigeon.getPitch().setUpdateFrequency(updateRate);
@@ -195,6 +204,23 @@ public Rotation2d getRotation2d() {
     synchronized (m_inputs) { return m_inputs.rotation2d; }
   }
 
+  @Override
+  public void updateSim(Rotation2d orientation, ChassisSpeeds desiredSpeeds, ControlCentricity controlCentricity) {
+    var currentTime = Instant.now();
+    double randomNoise = ThreadLocalRandom.current().nextDouble(0.9, 1.0);
+    double dt = Duration.between(currentTime, m_lastUpdateTime).toMillis() / 1000.0;
+
+    if (controlCentricity.equals(ControlCentricity.FIELD_CENTRIC))
+      desiredSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(desiredSpeeds, orientation);
+
+    int yawDriftDirection = ThreadLocalRandom.current().nextDouble(1.0) < 0.5 ? -1 : +1;
+    double angle = m_inputs.yawAngle.in(Units.Degrees) + Math.toDegrees(desiredSpeeds.omegaRadiansPerSecond * randomNoise) * dt
+                   + (PIGEON2_YAW_DRIFT_RATE.in(Units.DegreesPerSecond) * dt * yawDriftDirection);
+    m_pigeon.getSimState().setRawYaw(Units.Degrees.of(angle));
+
+    m_lastUpdateTime = currentTime;
+  }
+
   @Override
   public void close() {
     PurpleManager.remove(this);

src/main/java/org/lasarobotics/hardware/kauailabs/NavX2.java

@@ -4,6 +4,11 @@
 
 package org.lasarobotics.hardware.kauailabs;
 
+import java.time.Duration;
+import java.time.Instant;
+import java.util.concurrent.ThreadLocalRandom;
+
+import org.lasarobotics.drive.swerve.AdvancedSwerveKinematics.ControlCentricity;
 import org.lasarobotics.hardware.IMU;
 import org.lasarobotics.hardware.LoggableHardware;
 import org.lasarobotics.hardware.PurpleManager;
@@ -14,15 +19,19 @@
 import com.studica.frc.AHRS.NavXComType;
 import com.studica.frc.AHRS.NavXUpdateRate;
 
+import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.units.measure.MutAngle;
 import edu.wpi.first.units.measure.MutAngularVelocity;
 import edu.wpi.first.units.measure.MutLinearAcceleration;
 import edu.wpi.first.units.measure.MutLinearVelocity;
 import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
 
 /** NavX2 */
 public class NavX2 extends LoggableHardware implements IMU {
@@ -58,8 +67,19 @@ public static class NavX2Inputs {
     public Rotation2d rotation2d = Rotation2d.kZero;
   }
 
+  private static final AngularVelocity NAVX2_YAW_DRIFT_RATE = Units.DegreesPerSecond.of(0.5 / 60);
+
   private AHRS m_navx;
   private Notifier m_inputThread;
+  private ChassisSpeeds m_previousSpeeds;
+  private Instant m_lastUpdateTime;
+
+  private final SimDouble m_simPitch;
+  private final SimDouble m_simRoll;
+  private final SimDouble m_simYaw;
+  private final SimDouble m_simAccelX;
+  private final SimDouble m_simAccelY;
+  private final SimDouble m_simAccelZ;
 
   private String m_name;
   private NavX2InputsAutoLogged m_inputs;
@@ -76,6 +96,17 @@ public NavX2(ID id) {
     this.m_inputs = new NavX2InputsAutoLogged();
     this.m_inputThread = new Notifier(this::updateInputs);
     this.m_fieldCentricVelocities = false;
+
+    SimDeviceSim simNavX2 = new SimDeviceSim("navX-Sensor", m_navx.getPort());
+    this.m_simPitch = simNavX2.getDouble("Pitch");
+    this.m_simRoll = simNavX2.getDouble("Roll");
+    this.m_simYaw = simNavX2.getDouble("Yaw");
+    this.m_simAccelX = simNavX2.getDouble("LinearWorldAccelX");
+    this.m_simAccelY = simNavX2.getDouble("LinearWorldAccelY");
+    this.m_simAccelZ = simNavX2.getDouble("LinearWorldAccelZ");
+    this.m_previousSpeeds = new ChassisSpeeds();
+    this.m_lastUpdateTime = Instant.now();
+
     System.out.println();
 
     // Update inputs on init
@@ -205,6 +236,40 @@ public Rotation2d getRotation2d() {
     synchronized (m_inputs) { return m_inputs.rotation2d; }
   }
 
+  @Override
+  public LinearVelocity getVelocityX() {
+    synchronized (m_inputs) { return m_inputs.xVelocity; }
+  }
+
+  @Override
+  public LinearVelocity getVelocityY() {
+    synchronized (m_inputs) { return m_inputs.yVelocity; }
+  }
+
+  @Override
+  public void updateSim(Rotation2d orientation, ChassisSpeeds desiredSpeeds, ControlCentricity controlCentricity) {
+    var currentTime = Instant.now();
+    double randomNoise = ThreadLocalRandom.current().nextDouble(0.9, 1.0);
+    double dt = Duration.between(currentTime, m_lastUpdateTime).toMillis() / 1000.0;
+
+    if (controlCentricity.equals(ControlCentricity.FIELD_CENTRIC))
+      desiredSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(desiredSpeeds, orientation);
+
+    m_inputs.xVelocity.mut_replace(desiredSpeeds.vxMetersPerSecond, Units.MetersPerSecond);
+    m_inputs.yVelocity.mut_replace(desiredSpeeds.vyMetersPerSecond, Units.MetersPerSecond);
+
+    int yawDriftDirection = ThreadLocalRandom.current().nextDouble(1.0) < 0.5 ? -1 : +1;
+    double angle = m_simYaw.get() + Math.toDegrees(desiredSpeeds.omegaRadiansPerSecond * randomNoise) * dt
+                   + (NAVX2_YAW_DRIFT_RATE.in(Units.DegreesPerSecond) * dt * yawDriftDirection);
+    m_simYaw.set(angle);
+
+    m_simAccelX.set((desiredSpeeds.vxMetersPerSecond - m_previousSpeeds.vxMetersPerSecond) / dt);
+    m_simAccelY.set((desiredSpeeds.vyMetersPerSecond - m_previousSpeeds.vyMetersPerSecond) / dt);
+
+    m_previousSpeeds = desiredSpeeds;
+    m_lastUpdateTime = currentTime;
+  }
+
   /**
    * Closes the NavX
    */

src/main/java/org/lasarobotics/hardware/kauailabs/NavX2Sim.java

@@ -54,12 +54,12 @@ public void update(Rotation2d orientation, ChassisSpeeds desiredSpeeds, ControlC
     double randomNoise = ThreadLocalRandom.current().nextDouble(0.9, 1.0);
     double dt = Duration.between(currentTime, m_lastUpdateTime).toMillis() / 1000.0;
 
-    m_navx.getInputs().xVelocity.mut_replace(desiredSpeeds.vxMetersPerSecond, Units.MetersPerSecond);
-    m_navx.getInputs().yVelocity.mut_replace(desiredSpeeds.vyMetersPerSecond, Units.MetersPerSecond);
-
     if (controlCentricity.equals(ControlCentricity.FIELD_CENTRIC))
       desiredSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(desiredSpeeds, orientation);
 
+    m_navx.getInputs().xVelocity.mut_replace(desiredSpeeds.vxMetersPerSecond, Units.MetersPerSecond);
+    m_navx.getInputs().yVelocity.mut_replace(desiredSpeeds.vyMetersPerSecond, Units.MetersPerSecond);
+
     int yawDriftDirection = ThreadLocalRandom.current().nextDouble(1.0) < 0.5 ? -1 : +1;
     double angle = m_yaw.get() + Math.toDegrees(desiredSpeeds.omegaRadiansPerSecond * randomNoise) * dt
                    + (NAVX2_YAW_DRIFT_RATE.in(Units.DegreesPerSecond) * dt * yawDriftDirection);

src/main/java/org/lasarobotics/hardware/reduxrobotics/Canandgyro.java

@@ -4,6 +4,11 @@
 
 package org.lasarobotics.hardware.reduxrobotics;
 
+import java.time.Duration;
+import java.time.Instant;
+import java.util.concurrent.ThreadLocalRandom;
+
+import org.lasarobotics.drive.swerve.AdvancedSwerveKinematics.ControlCentricity;
 import org.lasarobotics.hardware.IMU;
 import org.lasarobotics.hardware.LoggableHardware;
 import org.lasarobotics.hardware.PurpleManager;
@@ -15,6 +20,7 @@
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Quaternion;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
@@ -65,18 +71,23 @@ public static class CanandgyroInputs {
 
   private static final Time INITIAL_FRAME_WAIT_TIME = Units.Seconds.of(5.0);
 
+  private static final AngularVelocity CANANDGYRO_YAW_DRIFT_RATE = Units.DegreesPerSecond.of(0.25 / 60);
+
   private com.reduxrobotics.sensors.canandgyro.Canandgyro m_gyro;
 
   private String m_name;
   private CanandgyroInputsAutoLogged m_inputs;
 
+  private Instant m_lastUpdateTime;
+
   private double m_prevAccelerationTimestamp;
 
   public Canandgyro(ID id) {
     this.m_name = id.name;
     this.m_gyro = new com.reduxrobotics.sensors.canandgyro.Canandgyro(id.deviceID);
     this.m_inputs = new CanandgyroInputsAutoLogged();
     this.m_prevAccelerationTimestamp = 0.0;
+    this.m_lastUpdateTime = Instant.now();
 
     m_gyro.getAccelerationFrame().addCallback(this::updateAcceleration);
     m_gyro.getMultiturnYawFrame().addCallback(this::updateYaw);
@@ -172,6 +183,25 @@ public Rotation2d getRotation2d() {
     synchronized (m_inputs) { return m_inputs.rotation2d; }
   }
 
+  @Override
+  public void updateSim(Rotation2d orientation, ChassisSpeeds desiredSpeeds, ControlCentricity controlCentricity) {
+    var currentTime = Instant.now();
+    double randomNoise = ThreadLocalRandom.current().nextDouble(0.9, 1.0);
+    double dt = Duration.between(currentTime, m_lastUpdateTime).toMillis() / 1000.0;
+    if (controlCentricity.equals(ControlCentricity.FIELD_CENTRIC))
+    desiredSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(desiredSpeeds, orientation);
+
+    m_inputs.xVelocity.mut_replace(desiredSpeeds.vxMetersPerSecond, Units.MetersPerSecond);
+    m_inputs.yVelocity.mut_replace(desiredSpeeds.vyMetersPerSecond, Units.MetersPerSecond);
+
+    int yawDriftDirection = ThreadLocalRandom.current().nextDouble(1.0) < 0.5 ? -1 : +1;
+    double angle = m_inputs.yawAngle.in(Units.Degrees) + Math.toDegrees(desiredSpeeds.omegaRadiansPerSecond * randomNoise) * dt
+                    + (CANANDGYRO_YAW_DRIFT_RATE.in(Units.DegreesPerSecond) * dt * yawDriftDirection);
+    m_inputs.yawAngle.mut_replace(Units.Degrees.of(angle));
+
+    m_inputs.rotation2d = orientation;
+  }
+
   @Override
   public void close() {
     PurpleManager.remove(this);

src/main/java/org/lasarobotics/vision/AprilTagCamera.java

@@ -15,7 +15,6 @@
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.simulation.PhotonCameraSim;
 import org.photonvision.simulation.SimCameraProperties;
-import org.photonvision.targeting.PhotonPipelineResult;
 
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
@@ -93,7 +92,7 @@ public AprilTagCamera(String name, Transform3d transform, Resolution resolution,
     this.m_fieldLayout = fieldLayout;
     // PV estimates will always be blue
     m_fieldLayout.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
-    this.m_poseEstimator = new PhotonPoseEstimator(m_fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, m_camera, m_transform);
+    this.m_poseEstimator = new PhotonPoseEstimator(m_fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, m_transform);
     m_poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
 
     this.m_latestResult = new AtomicReference<AprilTagCamera.Result>();
@@ -164,7 +163,7 @@ private void run() {
     if (!m_camera.isConnected()) return;
 
     // Update and log inputs
-    PhotonPipelineResult pipelineResult = m_camera.getLatestResult();
+    var pipelineResult = m_camera.getLatestResult();
 
     // Return if result is non-existent or invalid
     if (!pipelineResult.hasTargets()) {