Added vision to get distance instead of manually putting it in.

Author: EJainDev

src/main/java/frc/robot/Constants.java

@@ -14,4 +14,5 @@
  */
 public final class Constants {
   static public final boolean tuningMode = true;
-}
+  static public final String camName = "somename"; // TODO: Change
+}

src/main/java/frc/robot/Orchestrator.java

@@ -2,11 +2,13 @@
 
 import edu.wpi.first.units.DistanceUnit;
 import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.vision.Vision;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
@@ -19,10 +21,12 @@ public class Orchestrator {
     @AutoLogOutput
     private double Distance  = 300.0;
     @AutoLogOutput private final double shootingPower = 0.0;
+    private final Vision vision;
 
-    public Orchestrator(Indexer indexer, Shooter shooter) {
+    public Orchestrator(Indexer indexer, Shooter shooter, Vision vision) {
         this.indexer = indexer;
         this.shooter = shooter;
+        this.vision = vision;
     }
     public Command spinUpDistance(Double distance) {
         Distance = distance;
@@ -94,7 +98,7 @@ public Command shootCyclePID(double setpointRPM) {
 
     public Command shootDistance(Measure<DistanceUnit> distance) {
         return Commands.parallel(
-            shooter.toSetpoint(distance),
+            shooter.toSetpoint(Units.Meters.of(vision.distanceFromTarget())),
             Commands.sequence(
                 intakeIfNeeded(),
                 Commands.waitUntil(shooter::atSetpoint),

src/main/java/frc/robot/RobotContainer.java

@@ -15,6 +15,10 @@
 import frc.robot.subsystems.indexer.IndexerIOSparkMax;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterIOSparkMax;
+import frc.robot.subsystems.vision.Vision;
+import frc.robot.subsystems.vision.VisionIOPhotonVision;
+
+import static frc.robot.Constants.camName;
 
 
 /**
@@ -29,7 +33,8 @@ public class RobotContainer {
     // The robot's subsystems and commands are defined here...
    private final Indexer indexer = new Indexer(new IndexerIOSparkMax());
     private final Shooter shooter = new Shooter(new ShooterIOSparkMax());
-    private final Orchestrator orchestrator = new Orchestrator(indexer, shooter);
+    private final Vision vision = new Vision(new VisionIOPhotonVision(camName));
+    private final Orchestrator orchestrator = new Orchestrator(indexer, shooter, vision);
     private final CommandXboxController driverController = new CommandXboxController(0);
     private boolean fastMode = false;
     private final Trigger fastModeTrigger = new Trigger(() -> fastMode);

src/main/java/frc/robot/subsystems/vision/VisionIO.java

@@ -1,35 +1,34 @@
 package frc.robot.subsystems.vision;
 
-import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.geometry.Translation2d;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface VisionIO {
   @AutoLog
-  public static class VisionIOInputs {
+  class VisionIOInputs {
     public boolean connected = false;
     public PoseObservation latestTargetObservation =
-        new PoseObservation(0, new Translation3d(), 0);
+        new PoseObservation(0, new Translation2d(), 0);
     public int lastestTagID = 0 ;
     public double distanceFromTarget = 0;
     public double yaw = 0;
   }
 
 
   /** Represents the angle to a simple target, not used for pose estimation. */
-  public static record TargetObservation(Rotation2d tx, Rotation2d ty) {}
+  record TargetObservation(Rotation2d tx, Rotation2d ty) {}
 
   /** Represents a robot pose sample used for pose estimation. */
-  public static record PoseObservation (
+  record PoseObservation (
           int tagID,
-          Translation3d translation3d,
+          Translation2d translation2d,
           double yaw
   ){}
 
-  public static enum PoseObservationType {
+  enum PoseObservationType {
     PHOTONVISION
   }
 
    default void updateInputs(VisionIOInputs inputs) {}
-}
+}

src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java

@@ -1,11 +1,6 @@
 package frc.robot.subsystems.vision;
-import edu.wpi.first.math.geometry.*;
-
-import java.util.HashSet;
-import java.util.LinkedList;
-import java.util.List;
-import java.util.Set;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import org.photonvision.PhotonCamera;
 public class VisionIOPhotonVision implements VisionIO{
     protected final PhotonCamera camera;
@@ -20,16 +15,14 @@ public void updateInputs (VisionIOInputs inputs) {
         inputs.connected = camera.isConnected();
         var result = camera.getLatestResult();
         if (result.hasTargets()) {
-            inputs.latestTargetObservation = new PoseObservation(result.getBestTarget().fiducialId,result.getBestTarget().bestCameraToTarget.getTranslation(), result.getBestTarget().getYaw());
+            inputs.latestTargetObservation =
+                new PoseObservation(result.getBestTarget().fiducialId,result.getBestTarget().bestCameraToTarget.getTranslation().toTranslation2d(), result.getBestTarget().getYaw());
         }else{
-            inputs.latestTargetObservation = new PoseObservation(0,new Translation3d(),0);
+            inputs.latestTargetObservation = new PoseObservation(0, new Translation2d(), 0);
             }
         inputs.lastestTagID = inputs.latestTargetObservation.tagID();
-        inputs.distanceFromTarget = inputs.latestTargetObservation.translation3d().getNorm();
+        inputs.distanceFromTarget = inputs.latestTargetObservation.translation2d().getNorm();
         inputs.yaw = inputs.latestTargetObservation.yaw();
-
-
-
         }
 
 }