Incorporate some of photonvision code from Lucas

hethann · hethann · commit 6e6437e16b7e · 2025-03-15T19:08:57.000-04:00
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -8,12 +8,15 @@
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.*;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.commands.MoveToPoseRelativeToAprilTagCommand;
 import frc.robot.commands.ledCommands.BlinkLEDCommand;
 import frc.robot.commands.ledCommands.SetLEDPatternCommand;
 import frc.robot.generated.TunerConstants;
@@ -29,7 +32,7 @@ public class RobotContainer {
     public final Drivetrain drivetrain = TunerConstants.createDrivetrain();
     public final EndEffector endEffector = new EndEffector();
     public final Climber climber = new Climber();
-    // public final VisionSubsystem vision = new VisionSubsystem(drivetrain);
+     public final VisionSubsystem vision = new VisionSubsystem(drivetrain);
     public final Elevator elevator = new Elevator();
     public final Pivot pivot = new Pivot();
     public final Ratchet ratchet = new Ratchet();
@@ -138,14 +141,14 @@ private void configureBindings() {
          * RIGHT JOYSTICK RIGHT - RESET FIELD CENTRIC HEADING
          */
 
-        driverJoystickLeft.leftButton().onTrue(
-            new InstantCommand(() -> Drivetrain.isRobotCentric = false));
-        driverJoystickLeft.rightButton().onTrue(
-            new InstantCommand(() -> Drivetrain.isRobotCentric = true));
-        driverJoystickRight.leftButton().onTrue(
-            new InstantCommand( () -> Drivetrain.isSlowMode = !Drivetrain.isSlowMode));
-        driverJoystickRight.rightButton().onTrue(
-            new InstantCommand(drivetrain::seedFieldCentric));
+        driverJoystickLeft.leftButton().whileTrue(
+            new MoveToPoseRelativeToAprilTagCommand(vision, drivetrain, 6, new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0))));
+        // driverJoystickLeft.rightButton().onTrue(
+        //     new InstantCommand(() -> Drivetrain.isRobotCentric = true));
+        // driverJoystickRight.leftButton().onTrue(
+        //     new InstantCommand( () -> Drivetrain.isSlowMode = !Drivetrain.isSlowMode));
+        // driverJoystickRight.rightButton().onTrue(
+        //     new InstantCommand(drivetrain::seedFieldCentric));
 
         // ==================== Climber Bindings ====================
 
diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
+import java.util.Collections;
 import java.util.List;
 import java.util.Optional;
 
@@ -23,8 +24,8 @@ public class VisionSubsystem extends SubsystemBase {
     private final PhotonCamera camera_2;
     private final PhotonCamera camera_1;
     private final Drivetrain drivetrain;
-    private List<PhotonPipelineResult> latestCamera1Results;
-    private List<PhotonPipelineResult> latestCamera2Results;
+    private List<PhotonPipelineResult> latestCamera1Results = Collections.emptyList();
+    private List<PhotonPipelineResult> latestCamera2Results = Collections.emptyList();
     AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
     PhotonPoseEstimator poseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, new Transform3d());
 
