From 4ad5cd00efd88641d5837ff145fe6d7deaab22cb Mon Sep 17 00:00:00 2001
From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com>
Date: Thu, 20 Mar 2025 17:19:30 -0400
Subject: [PATCH] IT WORKS YES F YES

---
 src/main/java/frc/robot/RobotContainer.java            |  2 ++
 src/main/java/frc/robot/driver/DriverXbox.java         | 10 ++++------
 .../java/frc/robot/vision/PoseEstimatorSubsystem.java  |  4 ++++
 3 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 9149264..ba8ffb5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -59,6 +59,8 @@ public RobotContainer() {
 
         var mRushPivot = RushinatorPivot.getInstance();
 
+        var mPoseEstimator = PoseEstimatorSubsystem.getInstance();
+
         // ShuffleboardTab visionTab = Shuffleboard.getTab("Vision Pose Estimator");
         // PoseEstimatorSubsystem.getInstance().addDashboardWidgets(visionTab);
     }
diff --git a/src/main/java/frc/robot/driver/DriverXbox.java b/src/main/java/frc/robot/driver/DriverXbox.java
index 139bf25..e3303a3 100644
--- a/src/main/java/frc/robot/driver/DriverXbox.java
+++ b/src/main/java/frc/robot/driver/DriverXbox.java
@@ -13,8 +13,6 @@
 import frc.robot.drivetrain.CommandSwerveDrivetrain;
 import frc.robot.rushinator.RushinatorPivot;
 import frc.robot.rushinator.RushinatorWrist;
-import frc.robot.rushinator.commands.SetArmState;
-import frc.robot.rushinator.commands.SetWristState;
 
 
 public class DriverXbox extends XboxGamepad {
@@ -59,11 +57,11 @@ public void setupTeleopButtons() {
         controller.rightBumper().whileTrue(new InstantCommand(() -> RobotContainer.modeFast = false));
         controller.rightBumper().whileFalse(new InstantCommand(() -> RobotContainer.modeFast = true));
 
-        controller.x().onTrue(new SetWristState(RushinatorWrist.State.kScoreLeftWrist));
+        // controller.x().onTrue(new SetWristState(RushinatorWrist.State.kScoreLeftWrist));
         
-        controller.y().onTrue(new SetWristState(RushinatorWrist.State.kHumanPlayer));
-        controller.b().onTrue(new SetWristState(RushinatorWrist.State.kScoreRightWrist));
-        controller.a().onTrue(new SetWristState(RushinatorWrist.State.kGroundWrist));
+        // controller.y().onTrue(new SetWristState(RushinatorWrist.State.kHumanPlayer));
+        // controller.b().onTrue(new SetWristState(RushinatorWrist.State.kScoreRightWrist));
+        // controller.a().onTrue(new SetWristState(RushinatorWrist.State.kGroundWrist));
 
         // controller.a().onTrue(new SetArmState(RushinatorPivot.State.kTestPos));
         
diff --git a/src/main/java/frc/robot/vision/PoseEstimatorSubsystem.java b/src/main/java/frc/robot/vision/PoseEstimatorSubsystem.java
index 035bbab..aae66e0 100644
--- a/src/main/java/frc/robot/vision/PoseEstimatorSubsystem.java
+++ b/src/main/java/frc/robot/vision/PoseEstimatorSubsystem.java
@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.drivetrain.CommandSwerveDrivetrain;
 
@@ -81,6 +82,7 @@ public PoseEstimatorSubsystem(Supplier<Rotation2d> rotationSupplier,
     allNotifier.setName("runAll");
     allNotifier.startPeriodic(0.02);
 
+    SmartDashboard.putData("Field Pose Estimation", field2d);
     // backNotifier.setName("backRunnable");
     // backNotifier.startPeriodic(0.02);
 
@@ -149,6 +151,8 @@ public void periodic() {
       dashboardPose = flipAlliance(dashboardPose);
     }
     field2d.setRobotPose(dashboardPose);
+    SmartDashboard.putString("Pose Formatted", getFomattedPose());
+    
   }
 
   private String getFomattedPose() {