add gyro sim support

MiamiBeachBots · Jan 17, 2025 · 6aa76f1 · 6aa76f1
1 parent 0fcbbbc
commit 6aa76f1
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -22,6 +22,8 @@
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.studica.frc.AHRS;
 import com.studica.frc.AHRS.NavXComType;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -116,6 +118,9 @@ public DriveSubsystem() {
     m_rightGearbox = DCMotor.getNEO(2);
     m_leftSim = new SparkMaxSim(m_backLeft, m_leftGearbox);
     m_rightSim = new SparkMaxSim(m_backRight, m_rightGearbox);
+    // setup simulation for gyro
+    int gyroID = SimDeviceDataJNI.getSimDeviceHandle("navX-Sensor[0]");
+    SimDouble SimGyroAngle = new SimDouble(SimDeviceDataJNI.getSimValueHandle(gyroID, "Yaw"));
     // invert right side
     m_backRightConfig.inverted(true);
     m_frontRightConfig.inverted(true);