From 6501cef5c9f017cca3d250d92b403d997b148ad3 Mon Sep 17 00:00:00 2001
From: Kaya <95276965+kytpbs@users.noreply.github.com>
Date: Wed, 29 May 2024 16:47:07 +0300
Subject: [PATCH] add even more simulation support to driveSubsytem

---
 .../java/frc/robot/subsystems/DriveSubsystem.java     | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
index 6187519..2245191 100644
--- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -125,6 +125,10 @@ public DriveSubsystem() {
         this // Reference to this subsystem to set requirements
         );
     SmartDashboard.putNumber("Move By", 0);
+    if (RobotBase.isSimulation()) {
+      SmartDashboard.putNumber("X position", 0);
+      SmartDashboard.putNumber("Y position", 0);
+    }
   }
 
   @Override
@@ -163,6 +167,13 @@ public void simulationPeriodic() {
     SmartDashboard.putNumber("Rear Left Swerve Rotation", states[2].angle.getDegrees());
     SmartDashboard.putNumber("Rear Right Swerve Speed", states[3].speedMetersPerSecond);
     SmartDashboard.putNumber("Rear Right Swerve Rotation", states[3].angle.getDegrees());
+
+    var xPos = SmartDashboard.getNumber("X position", 0);
+    var yPos = SmartDashboard.getNumber("Y position", 0);
+
+    if (xPos != 0 && yPos != 0) {
+      resetOdometry(new Pose2d(xPos, yPos, getRotation2d()));
+    }
   }
 
   @Override