diff --git a/src/main/java/frc/lib/utils/TunableNumber.java b/src/main/java/frc/lib/utils/TunableNumber.java
new file mode 100644
index 0000000..66ef59b
--- /dev/null
+++ b/src/main/java/frc/lib/utils/TunableNumber.java
@@ -0,0 +1,118 @@
+
+package frc.lib.utils;
+
+import frc.robot.Constants;
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.Consumer;
+import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
+
+import static frc.robot.Constants.tuningMode;
+
+/**
+ * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
+ * value not in dashboard.
+ */
+public class TunableNumber implements DoubleSupplier {
+    private static final String tableKey = "/Tuning";
+
+    private final String key;
+    private boolean hasDefault = false;
+    private double defaultValue;
+    private LoggedNetworkNumber dashboardNumber;
+    private Map<Integer, Double> lastHasChangedValues = new HashMap<>();
+
+    /**
+     * Create a new LoggedTunableNumber
+     *
+     * @param dashboardKey Key on dashboard
+     */
+    public TunableNumber(String dashboardKey) {
+        this.key = tableKey + "/" + dashboardKey;
+    }
+
+    /**
+     * Create a new LoggedTunableNumber with the default value
+     *
+     * @param dashboardKey Key on dashboard
+     * @param defaultValue Default value
+     */
+    public TunableNumber(String dashboardKey, double defaultValue) {
+        this(dashboardKey);
+        initDefault(defaultValue);
+    }
+
+    /**
+     * Set the default value of the number. The default value can only be set once.
+     *
+     * @param defaultValue The default value
+     */
+    public void initDefault(double defaultValue) {
+        if (!hasDefault) {
+            hasDefault = true;
+            this.defaultValue = defaultValue;
+            if (tuningMode) {
+                dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
+            }
+        }
+    }
+
+    /**
+     * Get the current value, from dashboard if available and in tuning mode.
+     *
+     * @return The current value
+     */
+    public double get() {
+        if (!hasDefault) {
+            return 0.0;
+        } else {
+            return tuningMode ? dashboardNumber.get() : defaultValue;
+        }
+    }
+
+    /**
+     * Checks whether the number has changed since our last check
+     *
+     * @param id Unique identifier for the caller to avoid conflicts when shared between multiple
+     *     objects. Recommended approach is to pass the result of "hashCode()"
+     * @return True if the number has changed since the last time this method was called, false
+     *     otherwise.
+     */
+    public boolean hasChanged(int id) {
+        double currentValue = get();
+        Double lastValue = lastHasChangedValues.get(id);
+        if (lastValue == null || currentValue != lastValue) {
+            lastHasChangedValues.put(id, currentValue);
+            return true;
+        }
+
+        return false;
+    }
+
+    /**
+     * Runs action if any of the tunableNumbers have changed
+     *
+     * @param id Unique identifier for the caller to avoid conflicts when shared between multiple *
+     *     objects. Recommended approach is to pass the result of "hashCode()"
+     * @param action Callback to run when any of the tunable numbers have changed. Access tunable
+     *     numbers in order inputted in method
+     * @param tunableNumbers All tunable numbers to check
+     */
+    public static void ifChanged(int id, Consumer<double[]> action, TunableNumber... tunableNumbers) {
+        if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
+            action.accept(Arrays.stream(tunableNumbers).mapToDouble(TunableNumber::get).toArray());
+        }
+    }
+
+    /** Runs action if any of the tunableNumbers have changed */
+    public static void ifChanged(int id, Runnable action, TunableNumber... tunableNumbers) {
+        ifChanged(id, values -> action.run(), tunableNumbers);
+    }
+
+    @Override
+    public double getAsDouble() {
+        return get();
+    }
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index fc763af..c37107b 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -13,4 +13,6 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-}
+  static public final boolean tuningMode = true;
+  static public final String camName = "Arucam OV9281(Back)";
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java
index f1d1b2d..ebfe2dd 100644
--- a/src/main/java/frc/robot/Orchestrator.java
+++ b/src/main/java/frc/robot/Orchestrator.java
@@ -1,27 +1,53 @@
 package frc.robot;
 
-import java.util.function.BooleanSupplier;
-
+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;
+
+import java.util.function.BooleanSupplier;
 
 
 public class Orchestrator {
     private final Indexer indexer;
     private final Shooter shooter;
+    @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;
+        double percent = (8.331 * Math.pow(10, -5) * Math.pow(distance, 2) + 0.0734 * distance + 15.709) / 100;
+        Logger.recordOutput("Shooter/Percent", percent);
+        return shooter.runPercent(percent);
+    }
+    public Command shootCycleDistance() {
+        return Commands.parallel(
+                spinUpDistance(420.0),
+                Commands.sequence(
+                                intakeIfNeeded(),
+                                Commands.waitSeconds(ShooterConstants.SPINUP_SECONDS),
+                                indexer.indexIntoShooter())
+                        .repeatedly());
     }
 
     public Command spinUp(BooleanSupplier fastMode) {
         return Commands.either(
                 shooter.fullSpeed(),
-                shooter.speedUp(),
+                shooter.runPercent(0.8),
                  fastMode);
     }
 
@@ -51,6 +77,35 @@ public Command shootCycle(BooleanSupplier fastMode) {
                         .repeatedly());
     }
 
+    public Command shootOncePID(double setpointRPM) {
+        return Commands.parallel(
+                shooter.toSetpoint(setpointRPM),
+                Commands.sequence(
+                        intakeIfNeeded(),
+                        Commands.waitUntil(shooter::atSetpoint),
+                        indexer.indexIntoShooter()));
+    }
+
+    public Command shootCyclePID(double setpointRPM) {
+        return Commands.parallel(
+                shooter.toSetpoint(setpointRPM),
+                Commands.sequence(
+                                intakeIfNeeded(),
+                                Commands.waitUntil(shooter::atSetpoint),
+                                indexer.indexIntoShooter())
+                        .repeatedly());
+    }
+
+    public Command shootDistance(Measure<DistanceUnit> distance) {
+        return Commands.parallel(
+            shooter.toSetpoint(Units.Meters.of(vision.distanceFromTarget())),
+            Commands.sequence(
+                intakeIfNeeded(),
+                Commands.waitUntil(shooter::atSetpoint),
+                indexer.indexIntoShooter()).repeatedly());
+    }
+
+
     public Command reverseAll() {
         return Commands.parallel(shooter.reverse(), indexer.reverse());
     }
@@ -58,4 +113,4 @@ public Command reverseAll() {
     public Command stopAll() {
         return Commands.parallel(shooter.stop(), indexer.stop());
     }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5274408..ac1d9b9 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,22 +4,22 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.commands.Autos;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.drive.*;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.drive.DriveIOSparkMax;
 import frc.robot.subsystems.indexer.Indexer;
 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.VisionIO;
 import frc.robot.subsystems.vision.VisionIOPhotonVision;
 
+import static frc.robot.Constants.camName;
+
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -31,10 +31,10 @@ public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final Drive drive;
     // The robot's subsystems and commands are defined here...
-    private final Vision vision;
    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);
@@ -44,7 +44,6 @@ public class RobotContainer {
   public RobotContainer() {
     // Configure the trigger bindings
     drive = new Drive(new DriveIOSparkMax());
-    vision =  new Vision(new VisionIOPhotonVision("VGA_USB_Camera"){});
     configureBindings();
   }
 
@@ -75,16 +74,27 @@ private void configureBindings() {
                       .rightTrigger()
                       .whileTrue(orchestrator.shootCycle(() -> fastMode))
                       .onFalse(orchestrator.stopAll());
-              driverController.y().whileTrue(shooter.speedUp()).onFalse(shooter.stop());
-              driverController
-                      .leftTrigger()
-                      .whileTrue(orchestrator.reverseAll())
-                      .onFalse(orchestrator.stopAll());
-      
+
               driverController
                       .rightBumper()
                       .onTrue(orchestrator.shootOnce(() -> fastMode))
                       .onFalse(orchestrator.stopAll());
+
+              //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop());
+              driverController.x().whileTrue(orchestrator.shootCyclePID(2000)).onFalse(shooter.stop()); // TODO: change the RPM
+              driverController.a().whileTrue(orchestrator.shootDistance(Units.Inches.of(168.5))).onFalse(shooter.stop()); // TODO: Test other distances
+              driverController.leftTrigger().onTrue(indexer.indexUntilSwitch());
+
+              driverController.y().onTrue(indexer.indexIntoShooter());
+
+
+              driverController
+                      .leftBumper()
+                      .whileTrue(orchestrator.reverseAll())
+                      .onFalse(orchestrator.stopAll());
+      
+
+
     }
 
     /**
@@ -97,4 +107,4 @@ public Command getAutonomousCommand() {
         // An example command will be run in autonomous
         return Commands.none();
     }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 52e56b4..e1dd42f 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -1,15 +1,17 @@
 package frc.robot.subsystems.shooter;
 
+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 edu.wpi.first.wpilibj2.command.SubsystemBase;
-import java.util.function.DoubleSupplier;
-import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
-import frc.robot.subsystems.shooter.ShooterConstants;
+
+import java.util.function.DoubleSupplier;
 
 public class Shooter extends SubsystemBase {
-    private ShooterIO io;
+    private final ShooterIO io;
     private ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
 
     public Shooter(ShooterIO io) {
@@ -21,39 +23,55 @@ public Shooter(ShooterIO io) {
     public void periodic() {
         io.updateInputs(inputs);
         Logger.processInputs("Shooter", inputs);
+        io.goToSetpoint();
     }
 
     public Command stop() {
-        return Commands.runOnce(() ->{io.stop();}, this);
+        return Commands.runOnce(() -> {
+            io.stop();
+        }, this);
     }
 
-
     public Command reverse() {
-        return Commands.startEnd(
-            () -> io.setPercent(ShooterConstants.REVERSE_SHOOTER_PERCENT),
-            () -> io.setPercent(0),
-            this
-         );
-}
+        return Commands.startEnd(() -> io.setPercent(ShooterConstants.REVERSE_SHOOTER_PERCENT),
+                                 () -> io.setPercent(0), this);
+    }
+
+    public boolean atSetpoint() {
+        return inputs.atSetpoint;
+    }
 
+    public double getVelocity() {
+        return inputs.velocity;
+    }
+
+    public Command toSetpoint(double setpointRPM) {
+        return Commands.run(() -> {
+            io.setVelocity(setpointRPM);
+        }, this);
+    }
 
-    public Command speedUp() {
-        return Commands.run(
-                () -> {
-                    io.setPercent(ShooterConstants.SPEED);
-                },
-                this
-        );
+    public Command toSetpoint(Measure<DistanceUnit> distance) {
+        return Commands.run(() -> io.setDistance(distance.in(Units.Inches)));
+    }
+
+    public Command toSetpoint(DoubleSupplier setpointRPM) {
+        return Commands.run(() -> {
+            io.setVelocity(setpointRPM.getAsDouble());
+        }, this);
     }
 
     public Command fullSpeed() {
-        return Commands.run(
-                () -> {
-                    io.setPercent(1);
-                },
-                this
-        );
+        return Commands.run(() -> {
+            io.setPercent(1);
+        }, this);
+    }
+
+    public Command runPercent(double percent) {
+        return Commands.run(() -> {
+            io.setPercent(percent);
+        }, this);
     }
 
 
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index de81527..9bd01ae 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -1,16 +1,27 @@
 package frc.robot.subsystems.shooter;
 
-public class ShooterConstants {
+import edu.wpi.first.math.MathShared;
+import edu.wpi.first.math.MathUtil;
+import frc.lib.utils.TunableNumber;
 
-    public static final double SPEED = 0.8;
+public class ShooterConstants {
 
     public static final double REVERSE_SHOOTER_PERCENT = -0.5;
 
     public static final int SHOOTER_MOTOR_ID = 8;
 
-    public static final double ENCODER_POSITION_CONVERSION = 1.0;
+    public static final double ENCODER_VELOCITY_CONVERSION = 1;
+
+    public static final double SPINUP_SECONDS = 0.6;
+
+    public static final double TOLERANCE = 3; // TODO: change tolerance
+
 
-    public static final double ENCODER_VELOCITY_CONVERSION = 1.0;
+    public static final TunableNumber PID_P = new TunableNumber("Shooter/P", 0.00005); //0.0000009
+    public static final TunableNumber PID_I = new TunableNumber("Shooter/I",0.0000001); //0.0000003
+    public static final TunableNumber PID_D = new TunableNumber("Shooter/D", 0.0000001); // 0.0001
 
-    public static final double SPINUP_SECONDS = 0.1;
-}
\ No newline at end of file
+}
+// 0.00005
+//0.0000001
+//0.0000001
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
index 2a6a301..9703e66 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
@@ -1,6 +1,5 @@
 package frc.robot.subsystems.shooter;
 
-import org.ejml.dense.row.factory.DecompositionFactory_CDRM;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface ShooterIO {
@@ -8,7 +7,7 @@ public interface ShooterIO {
     @AutoLog
     class ShooterIOInputs {
 
-        public double velocity;
+        public double velocity; // RPM
 
         public double appliedVolts;
 
@@ -16,7 +15,11 @@ class ShooterIOInputs {
 
         public double temperature;
 
+        public double setpointRPM = 0.0;
+
         public boolean readyToShoot = false;
+
+        public boolean atSetpoint = false;
     }
 
     default void updateInputs(ShooterIOInputs inputs) {}
@@ -25,5 +28,11 @@ default void setPercent(double percent) {}
 
     default void setVoltage(double voltage) {}
 
+    default void setVelocity(double RPM) {}
+
+    default void setDistance(double distance) {}
+
+  default void goToSetpoint() {}
+
     default void stop() {}
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
index 70c2267..4f5b2cb 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java
@@ -1,13 +1,16 @@
 package frc.robot.subsystems.shooter;
 
+import static com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder;
 import static frc.robot.subsystems.shooter.ShooterConstants.*;
 
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkLimitSwitch;
+import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.ClosedLoopConfig;
 import com.revrobotics.spark.config.EncoderConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
@@ -17,24 +20,33 @@ public class ShooterIOSparkMax implements ShooterIO {
     private final SparkMax motor;
     private final RelativeEncoder encoder;
 
+    private final SparkClosedLoopController controller;
+
+    private double setpointRPM = 3000;
+
     public ShooterIOSparkMax() {
         motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless);
-
-        var config = new SparkMaxConfig();
+        controller = motor.getClosedLoopController();
+        var config  = new SparkMaxConfig();
         config.inverted(false)
                 .idleMode(IdleMode.kBrake)
                 .voltageCompensation(12)
                 .smartCurrentLimit(30);
 
+        var loopConfig = new ClosedLoopConfig();
+        loopConfig
+                .feedbackSensor(kPrimaryEncoder)
+                .positionWrappingEnabled(false)
+                .pid(PID_P.get(), PID_I.get(), PID_D.get());// TODO: tune PID values
+
+
         EncoderConfig enc = new EncoderConfig();
-        enc.positionConversionFactor(ENCODER_POSITION_CONVERSION);
         enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION);
         config.apply(enc);
+        config.apply(loopConfig);
 
         motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-
         encoder = motor.getEncoder();
-
     }
 
     @Override
@@ -42,7 +54,10 @@ public void updateInputs(ShooterIOInputs inputs) {
         inputs.temperature = motor.getMotorTemperature();
         inputs.appliedVolts = motor.getBusVoltage() * motor.getAppliedOutput();
         inputs.currentAmps = motor.getOutputCurrent();
-        inputs.velocity = encoder.getVelocity();
+        inputs.velocity = encoder.getVelocity();// RPM
+        inputs.setpointRPM = setpointRPM;
+        inputs.atSetpoint = Math.abs(setpointRPM - inputs.velocity) < TOLERANCE; // TODO: change tolerance
+
     }
 
     public void setPercent(double percent) {
@@ -52,6 +67,23 @@ public void setPercent(double percent) {
     public void setVoltage(double volts) {
         motor.setVoltage(volts);
     }
+    public double returnVelocity(){
+        return this.encoder.getVelocity();
+    }
+
+    @Override
+    public void setVelocity(double setpointRPM) {
+        this.setpointRPM = setpointRPM;
+    }
+
+    @Override
+    public void setDistance(double distance) {this.setpointRPM = 113.29128 * Math.pow(distance,
+                                                                                      0.639824);}
+
+    @Override
+    public void goToSetpoint(){
+        controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity);
+    }
 
     public void stop() {
         motor.set(0);
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java
index ed64e78..0e4b591 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java
@@ -1,16 +1,15 @@
 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;
@@ -18,18 +17,18 @@ public static class VisionIOInputs {
 
 
   /** 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) {}
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java
index ea62a36..e03bcb6 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java
+++ b/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();
-
-
-
         }
 
 }
\ No newline at end of file