feat: functional camera

flamingchickens1540 · Sep 24, 2024 · a481a2c · a481a2c
1 parent 0a41223
commit a481a2c
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 3 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -135,7 +135,7 @@ public static class AprilTag {
         }
 
         public static class Gamepiece {
-            public static final String CAMERA_NAME = "limelight-note";
+            public static final String CAMERA_NAME = "limelight-vision";
             public static final int PIPELINE_INDEX = 0;
             public static final Pose3d CAMERA_POSE = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); //TODO FIND THIS POSE
         }

diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -175,10 +175,15 @@ private void configureButtonBindings() {
 //            drivetrain.getRotation();
 //            driver.leftTrigger().whileTrue(new DriveWithCorrectionCommand(drivetrain, driver, ()-> LimelightHelpers.getTX(Constants.Vision.VISION_CAMERA_NAME)));
 //            driver.leftTrigger().whileTrue(new SpitShoot(shooter, indexer));
-            driver.leftTrigger().whileTrue(new DriveWithCorrectionCommand2(drivetrain, driver,
-        ()-> Constants.Targeting.getSpeakerPose().getTranslation().minus(drivetrain.getPose().getTranslation()).getAngle().getDegrees()));
+//            driver.leftTrigger().whileTrue(new DriveWithCorrectionCommand2(drivetrain, driver,
+//        ()-> Constants.Targeting.getSpeakerPose().getTranslation().minus(drivetrain.getPose().getTranslation()).getAngle().getDegrees()));
+
+//            driver.leftTrigger().whileTrue(
+//                    new DriveWithCorrectionCommand(drivetrain, driver, ()->noteVision.getLatestDetection().rotation().toRotation2d().getDegrees())
+//            );
         }
 
+
         driver.povDown().and(() -> !DriverStation.isFMSAttached()).onTrue(Commands.runOnce(() -> drivetrain.setPose(new Pose2d(Units.inchesToMeters(260), Units.inchesToMeters(161.62), Rotation2d.fromRadians(0)))).ignoringDisable(true));
 
         driver.rightTrigger(0.95).toggleOnTrue(counterShuffleDrive);

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java
@@ -1,6 +1,7 @@
 package org.team1540.robot2024.subsystems.vision.gamepiece;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
 import org.team1540.robot2024.util.vision.GamepieceDetection;
 
 import static org.team1540.robot2024.Constants.Vision.Gamepiece.*;
@@ -20,6 +21,7 @@ private NoteVision(NoteVisionIO io) {
     @Override
     public void periodic() {
         io.updateInputs(inputs);
+        Logger.processInputs("NoteVision", inputs);
     }
 
     public GamepieceDetection getLatestDetection() {