IT WORKS YES F YES

Author: pix-xl

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);
     }

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));
 

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() {