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