From 44c92ed1af6dab2eb054ecce544451ff0979ca79 Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Sat, 27 Jan 2024 11:00:19 -0600
Subject: [PATCH 01/19] Add intial Limelight subsystem

---
 .../java/frc/robot/subsystems/Limelight.java  | 71 +++++++++++++++++++
 1 file changed, 71 insertions(+)
 create mode 100644 src/main/java/frc/robot/subsystems/Limelight.java

diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
new file mode 100644
index 00000000..c528c37e
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Limelight extends SubsystemBase {
+  public final NetworkTable limelightTable;
+  private double tv, tx, ty, ta;
+
+  /** Creates a new limelight. */
+  public Limelight(String name) {
+    this.limelightTable = NetworkTableInstance.getDefault().getTable(name);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    tv = limelightTable.getEntry("tv").getDouble(0);
+    tx = limelightTable.getEntry("tx").getDouble(0);
+    ty = limelightTable.getEntry("ty").getDouble(0);
+    ta = limelightTable.getEntry("ta").getDouble(0);
+  }
+
+  /**
+   * Gets the horizontal offset from the crosshair to the currently active target.
+   *
+   * @return tx; horizontal offset from the crosshair to the currently active target.
+   */
+  public double getTX() {
+    return tx;
+  }
+
+  /**
+   * Gets the vertical offset from the crosshair to the currently active target.
+   *
+   * @return ty; vertical offset from the crosshair to the currently active target.
+   */
+  public double getTY() {
+    return ty;
+  }
+
+  /**
+   * Gets the area of the target 0% to 100% of image.
+   *
+   * @return ta; area of the target 0% to 100% of image.
+   */
+  public double getTA() {
+    return ta;
+  }
+
+  /**
+   * Returns true if the Limelight has a valid target.
+   *
+   * @return a boolean; true if the Limelight has a vlid target.
+   */
+  public boolean hasValidTarget() {
+    return tv = 1.0;
+  }
+}

From 5ae8517f12080bfe17621fcf7e342bfee3ba5cd1 Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Sat, 27 Jan 2024 11:18:32 -0600
Subject: [PATCH 02/19] Use last year's robot limelight implementation

---
 .../java/frc/robot/subsystems/Limelight.java  | 129 +++++++++++++++---
 1 file changed, 110 insertions(+), 19 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
index c528c37e..fafc6860 100644
--- a/src/main/java/frc/robot/subsystems/Limelight.java
+++ b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -11,32 +11,85 @@
 
 package frc.robot.subsystems;
 
-import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.networktables.DoubleArraySubscriber;
+import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
+/** Subsystem for the Limelight 2+ that we use for vision. */
 public class Limelight extends SubsystemBase {
-  public final NetworkTable limelightTable;
+
+  /* Note to future devs: we are NOT doing the whole setting values in NetworkTables to turn the Limelight
+   * LEDS on and off thing this year (turnOnLEDs() and turnOfflLEDs() in robot2022).
+   * For one, if we're not tracking reflective targets (which as of now we are not, we're just doing
+   * AprilTags), then we shouldn't need them on at all; for another, the correct way to turn the LEDs on
+   * and off is to make the default pipeline one with them off and then switch to a pipeline which has them
+   * on if we need to turn them on. This method prevents the whole "Limelight LEDs are stuck on until robot
+   * code starts!" thing.
+   */
+
+  /* This class may need logic to switch between pipelines based off of AprilTag pipeline restrictions. As
+   * of now, I'm not adding them, since in theory the Limelight should just handle getting a botpos with as
+   * many AprilTags in its FOV as can fit because MegaTag
+   * (https://docs.limelightvision.io/en/latest/apriltags_in_3d.html#robot-localization-botpose-and-megatag).
+   */
+  /* it would be so cool if we just didn't have to do multiple pipelines at all this year */
+
+  /* variable chaingun, I promise we use all of these */
+  private DoubleSubscriber tvSubscriber, txSubscriber, tySubscriber, taSubscriber;
+  private DoubleArraySubscriber botposSubscriber;
   private double tv, tx, ty, ta;
+  /* See https://docs.limelightvision.io/en/latest/apriltags_in_3d.html#robot-localization-botpose-and-megatag
+   * to understand what the heck the different indices in this array mean
+   */
+  private double[] botpos;
+  private String name;
+
+  private String fmtPath(String end) {
+    return "/" + name + "/" + end;
+  }
 
-  /** Creates a new limelight. */
+  /**
+   * Creates a new Limelight. Should be run once from {@link frc.robot.RobotContainer}.
+   *
+   * @param name The hostname of the limelight
+   */
   public Limelight(String name) {
-    this.limelightTable = NetworkTableInstance.getDefault().getTable(name);
+    this.name = name;
+    tvSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("tv")).subscribe(0.0);
+    txSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("tx")).subscribe(0.0);
+    tySubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("ty")).subscribe(0.0);
+    taSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("ta")).subscribe(0.0);
+    /* In theory this won't break. It got mad when I tried to insert the array into the
+     * method like .subscribe({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) so ¯\_(ツ)_/¯
+     */
+    double[] defaultBotpos = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    botposSubscriber =
+        NetworkTableInstance.getDefault()
+            .getDoubleArrayTopic(fmtPath("botpose"))
+            .subscribe(defaultBotpos);
   }
 
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-    tv = limelightTable.getEntry("tv").getDouble(0);
-    tx = limelightTable.getEntry("tx").getDouble(0);
-    ty = limelightTable.getEntry("ty").getDouble(0);
-    ta = limelightTable.getEntry("ta").getDouble(0);
+  /* now its time for getter method chaingun, which I have to write manually because VS Code */
+  /* nvm im making a python script do this for me */
+
+  /**
+   * Returns whether the Limelight has a valid target.
+   *
+   * @return Whether or not the Limelight has a valid target. True if it does, false if it doesn't.
+   */
+  public boolean hasValidTarget() {
+    return tv == 1.0;
   }
 
   /**
    * Gets the horizontal offset from the crosshair to the currently active target.
    *
-   * @return tx; horizontal offset from the crosshair to the currently active target.
+   * @return tx; the horizontal offset from the crosshair to the target.
    */
   public double getTX() {
     return tx;
@@ -45,27 +98,65 @@ public double getTX() {
   /**
    * Gets the vertical offset from the crosshair to the currently active target.
    *
-   * @return ty; vertical offset from the crosshair to the currently active target.
+   * @return ty; the vertical offset from the crosshair to the target.
    */
   public double getTY() {
     return ty;
   }
 
   /**
-   * Gets the area of the target 0% to 100% of image.
+   * Gets the area of the target, 0% to 100% of the image.
    *
-   * @return ta; area of the target 0% to 100% of image.
+   * @return ta; the area of the target.
    */
   public double getTA() {
     return ta;
   }
 
   /**
-   * Returns true if the Limelight has a valid target.
+   * Returns the robot's current pose relative to the field.
    *
-   * @return a boolean; true if the Limelight has a vlid target.
+   * <p>Units are meters; 0,0 is at the center of the field.
+   *
+   * <p>Will only work if the Limelight can see an AprilTag and read it correctly.
+   *
+   * <p>Notably, the Z, roll, and pitch values will always be zero, since we snap the bot to the
+   * floor on the Limelight.
+   *
+   * @return A {@link Pose}; the robot's current position relative to the field.
    */
-  public boolean hasValidTarget() {
-    return tv = 1.0;
+  public Pose3d getBotpose() {
+    return new Pose3d(
+        botpos[0], botpos[1], botpos[2], new Rotation3d(botpos[3], botpos[4], botpos[5]));
+  }
+
+  /**
+   * Gets the robot's current post relative to the field.
+   *
+   * <p>Units are meters; 0,0 is at the center of the field.
+   *
+   * <p>Will only work if the Limelight can see an AprilTag and read it correctly.
+   *
+   * @return A {@link Pose2d}; the robot's current position relative to the field in two dimensions.
+   */
+  public Pose2d getBotpose2d() {
+    return new Pose2d(botpos[0], botpos[1], Rotation2d.fromDegrees(botpos[5]));
+  }
+
+  /**
+   * Subsystem periodic; runs every scheduler run. Used to update Limelight data from NetworkTables
+   * in this subsystem.
+   */
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    tv = tvSubscriber.get();
+    tx = txSubscriber.get();
+    ty = tySubscriber.get();
+    ta = taSubscriber.get();
+    /* See https://docs.limelightvision.io/en/latest/apriltags_in_3d.html#robot-localization-botpose-and-megatag
+     * to understand what the heck the different indices in this array mean
+     */
+    botpos = botposSubscriber.get();
   }
 }

From a8851a81a5531114b655611877cd579ad973a066 Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Mon, 29 Jan 2024 15:45:47 -0600
Subject: [PATCH 03/19] Removed cached Limelight variables

---
 .../java/frc/robot/subsystems/Limelight.java  | 21 +++++++------------
 1 file changed, 7 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
index fafc6860..49f17a99 100644
--- a/src/main/java/frc/robot/subsystems/Limelight.java
+++ b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -42,11 +42,9 @@ public class Limelight extends SubsystemBase {
   /* variable chaingun, I promise we use all of these */
   private DoubleSubscriber tvSubscriber, txSubscriber, tySubscriber, taSubscriber;
   private DoubleArraySubscriber botposSubscriber;
-  private double tv, tx, ty, ta;
   /* See https://docs.limelightvision.io/en/latest/apriltags_in_3d.html#robot-localization-botpose-and-megatag
    * to understand what the heck the different indices in this array mean
    */
-  private double[] botpos;
   private String name;
 
   private String fmtPath(String end) {
@@ -83,7 +81,7 @@ public Limelight(String name) {
    * @return Whether or not the Limelight has a valid target. True if it does, false if it doesn't.
    */
   public boolean hasValidTarget() {
-    return tv == 1.0;
+    return tvSubscriber.get() == 1.0;
   }
 
   /**
@@ -92,7 +90,7 @@ public boolean hasValidTarget() {
    * @return tx; the horizontal offset from the crosshair to the target.
    */
   public double getTX() {
-    return tx;
+    return txSubscriber.get();
   }
 
   /**
@@ -101,7 +99,7 @@ public double getTX() {
    * @return ty; the vertical offset from the crosshair to the target.
    */
   public double getTY() {
-    return ty;
+    return tySubscriber.get();
   }
 
   /**
@@ -110,7 +108,7 @@ public double getTY() {
    * @return ta; the area of the target.
    */
   public double getTA() {
-    return ta;
+    return taSubscriber.get();
   }
 
   /**
@@ -126,6 +124,7 @@ public double getTA() {
    * @return A {@link Pose}; the robot's current position relative to the field.
    */
   public Pose3d getBotpose() {
+    double[] botpos = botposSubscriber.get();
     return new Pose3d(
         botpos[0], botpos[1], botpos[2], new Rotation3d(botpos[3], botpos[4], botpos[5]));
   }
@@ -140,6 +139,7 @@ public Pose3d getBotpose() {
    * @return A {@link Pose2d}; the robot's current position relative to the field in two dimensions.
    */
   public Pose2d getBotpose2d() {
+    double[] botpos = botposSubscriber.get();
     return new Pose2d(botpos[0], botpos[1], Rotation2d.fromDegrees(botpos[5]));
   }
 
@@ -150,13 +150,6 @@ public Pose2d getBotpose2d() {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    tv = tvSubscriber.get();
-    tx = txSubscriber.get();
-    ty = tySubscriber.get();
-    ta = taSubscriber.get();
-    /* See https://docs.limelightvision.io/en/latest/apriltags_in_3d.html#robot-localization-botpose-and-megatag
-     * to understand what the heck the different indices in this array mean
-     */
-    botpos = botposSubscriber.get();
+
   }
 }

From af1b3365a606ac527b11f86d88e16a15687391ee Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Thu, 1 Feb 2024 18:06:50 -0600
Subject: [PATCH 04/19] Add AimWithLimelight

---
 .../frc/robot/commands/AimWithLimelight.java  | 108 ++++++++++++++++++
 1 file changed, 108 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/AimWithLimelight.java

diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java
new file mode 100644
index 00000000..f7f96a5e
--- /dev/null
+++ b/src/main/java/frc/robot/commands/AimWithLimelight.java
@@ -0,0 +1,108 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Drive;
+import frc.robot.subsystems.Limelight;
+
+public class AimWithLimelight extends Command {
+  private Drive drive;
+  private Limelight limelight;
+
+  private boolean finished;
+  private int counter;
+
+  private double steerStrength,
+      distanceFromTarget,
+      mountHeight,
+      mountAngle,
+      driveStrength,
+      speedLimit,
+      turnDoneThreshold,
+      distanceDoneThreshold,
+      upperHubTapeHeight;
+
+  /** Creates a new AimWithLimelight. */
+  public AimWithLimelight(
+      Limelight limelight,
+      double steerStrength,
+      double distanceFromTarget,
+      double mountHeight,
+      double mountAngle,
+      double driveStrength,
+      double speedLimit,
+      double turnDoneThreshold,
+      double distanceDoneThreshold,
+      double hubTapeHeight) {
+    this.steerStrength = steerStrength;
+    this.distanceFromTarget = distanceFromTarget;
+    this.mountHeight = mountHeight;
+    this.mountAngle = mountAngle;
+    this.driveStrength = driveStrength;
+    this.speedLimit = speedLimit;
+    this.turnDoneThreshold = turnDoneThreshold;
+    this.distanceDoneThreshold = distanceDoneThreshold;
+    this.upperHubTapeHeight = hubTapeHeight;
+    this.drive = RobotContainer.drive;
+    this.limelight = limelight;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(drive, limelight);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.finished = false;
+    this.counter = 0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (limelight.hasValidTarget()) {
+      double z = limelight.getTX() * steerStrength;
+      double yComponent =
+          distanceFromTarget
+              - ((upperHubTapeHeight - mountHeight) / Math.tan((mountAngle + limelight.getTY())));
+      double y = yComponent * (Math.PI / 180.0) * driveStrength;
+      if (z > speedLimit) {
+        z = speedLimit;
+      }
+      drive.driveRobotOriented(y * -1.0, 0.0, z);
+      boolean isAngled = Math.abs(limelight.getTX()) < turnDoneThreshold;
+      boolean isDistanced =
+          Math.abs(
+                  (distanceFromTarget)
+                      - ((upperHubTapeHeight - mountHeight)
+                          / Math.tan((mountAngle + limelight.getTY()) * (Math.PI / 180.0))))
+              <= distanceDoneThreshold;
+      if (isAngled && isDistanced) {
+        counter++;
+        if (counter > 5) {
+          this.finished = true;
+        }
+      }
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return finished;
+  }
+}

From 6821575e00a02d883efee589ac1c5339802258c0 Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Sat, 3 Feb 2024 09:50:11 -0600
Subject: [PATCH 05/19] Add Limelight constants

---
 src/main/java/frc/robot/Constants.java | 32 ++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1ceaa0b6..c1c85680 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -74,4 +74,36 @@ public static class Intake {
     /** Speed we want to run the Intake at. */
     public static final double INTAKE_MOTOR_SPEED = 0.5;
   }
+
+  public static class Limelight {
+    public static class AimingLimelight {
+      /* The number of degrees the limelight is mounted back from perfectly vertical */
+      public static final double MOUNT_ANGLE = 0;
+
+      /* The number of inches from the center of the Limelight lens to the floor */
+      public static final double MOUNT_HEIGHT = 0;
+
+      /* The number of inches from the retroreflective tape on the upper hub to the floor */
+      /* This is 8'8", which is what the manual says the height from the floor to the top of the upper hub is. */
+      public static final double UPPER_HUB_TAPE_HEIGHT = 0;
+
+      /* How far in inches we want to be from the target when we shoot */
+      public static final double DISTANCE_FROM_TARGET = 0;
+
+      /* How hard to turn toward the target. Double between 0 and 1, standard way to drive a motor */
+      public static final double STEER_STRENGTH = 0;
+
+      /* How hard to drive toward the target. Same notation as above. */
+      public static final double DRIVE_STRENGTH = 0;
+
+      /* VERY BASIC speed limit to make sure we don't drive too fast towards the target. Will need to be changed when implementing PID. */
+      public static final double SPEED_LIMIT = 0;
+
+      /* When we're at or below this number of degrees from where we want to be, we'll consider the limelight's aim routine "done" */
+      public static final double TURN_DONE_THRESHOLD = 0;
+
+      /* When we're at or below this number of inches from the target distance, we'll consider the limelight's drive routine "done" */
+      public static final double DISTANCE_DONE_THRESHOLD = 0;
+    }
+  }
 }

From a2f298d757b94450af24fdda285f502723b535bf Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Sat, 3 Feb 2024 09:50:26 -0600
Subject: [PATCH 06/19] Add aimToAmp

---
 src/main/java/frc/robot/RobotContainer.java | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 2a8d483b..7ba9fd6b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -15,6 +15,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.commands.AimWithLimelight;
 import frc.robot.commands.DeployPneumatics;
 import frc.robot.commands.DriveFieldOriented;
 import frc.robot.commands.DriveRobotOriented;
@@ -23,6 +24,7 @@
 import frc.robot.commands.RunIntake;
 import frc.robot.subsystems.Drive;
 import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.Limelight;
 import frc.robot.subsystems.mailbox.MailboxBelts;
 import frc.robot.subsystems.mailbox.MailboxPneumatics;
 
@@ -51,6 +53,7 @@ public class RobotContainer {
   /** Singleton instance of {@link Intake} for the whole robot. */
   public static Intake intake = new Intake();
 
+  public static Limelight limelight = new Limelight("limelight");
   /*
    * ************
    * * COMMANDS *
@@ -63,6 +66,18 @@ public class RobotContainer {
   private DeployPneumatics deployPneumatics = new DeployPneumatics();
   private RunBelts runBelts = new RunBelts();
   private RunIntake runIntake = new RunIntake();
+  private AimWithLimelight aimToAmp =
+      new AimWithLimelight(
+          limelight,
+          Constants.Limelight.AimingLimelight.STEER_STRENGTH,
+          Constants.Limelight.AimingLimelight.DISTANCE_FROM_TARGET,
+          Constants.Limelight.AimingLimelight.MOUNT_HEIGHT,
+          Constants.Limelight.AimingLimelight.MOUNT_ANGLE,
+          Constants.Limelight.AimingLimelight.DRIVE_STRENGTH,
+          Constants.Limelight.AimingLimelight.SPEED_LIMIT,
+          Constants.Limelight.AimingLimelight.TURN_DONE_THRESHOLD,
+          Constants.Limelight.AimingLimelight.DISTANCE_DONE_THRESHOLD,
+          Constants.Limelight.AimingLimelight.UPPER_HUB_TAPE_HEIGHT);
 
   /*
    * ***********************

From 1d45d05ac18e86b73c323f0bd414d8cc080304fb Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Sat, 3 Feb 2024 10:43:20 -0600
Subject: [PATCH 07/19] Fix Limelight constants

---
 src/main/java/frc/robot/Constants.java      | 35 ++++++++++++++-------
 src/main/java/frc/robot/RobotContainer.java |  2 +-
 2 files changed, 25 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index c1c85680..c166a9ba 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -75,34 +75,47 @@ public static class Intake {
     public static final double INTAKE_MOTOR_SPEED = 0.5;
   }
 
+  /** Holds contstants for the limelights. */
   public static class Limelight {
+    /** Constants for aiming limelight. */
     public static class AimingLimelight {
-      /* The number of degrees the limelight is mounted back from perfectly vertical */
+      /** The number of degrees the limelight is mounted back from perfectly vertical */
       public static final double MOUNT_ANGLE = 0;
 
-      /* The number of inches from the center of the Limelight lens to the floor */
+      /** The number of inches from the center of the Limelight lens to the floor */
       public static final double MOUNT_HEIGHT = 0;
 
-      /* The number of inches from the retroreflective tape on the upper hub to the floor */
-      /* This is 8'8", which is what the manual says the height from the floor to the top of the upper hub is. */
-      public static final double UPPER_HUB_TAPE_HEIGHT = 0;
+      /** The number of inches from the retroreflective tape on the upper hub to the floor */
+      /**
+       * This is 8'8", which is what the manual says the height from the floor to the top of the
+       * upper hub is.
+       */
+      public static final double AMP_APRILTAG_HEIGHT = 0;
 
-      /* How far in inches we want to be from the target when we shoot */
+      /** How far in inches we want to be from the target when we shoot */
       public static final double DISTANCE_FROM_TARGET = 0;
 
-      /* How hard to turn toward the target. Double between 0 and 1, standard way to drive a motor */
+      /**
+       * How hard to turn toward the target. Double between 0 and 1, standard way to drive a motor
+       */
       public static final double STEER_STRENGTH = 0;
 
-      /* How hard to drive toward the target. Same notation as above. */
+      /** How hard to drive toward the target. Same notation as above. */
       public static final double DRIVE_STRENGTH = 0;
 
-      /* VERY BASIC speed limit to make sure we don't drive too fast towards the target. Will need to be changed when implementing PID. */
+      /** VERY BASIC speed limit to make sure we don't drive too fast towards the target. */
       public static final double SPEED_LIMIT = 0;
 
-      /* When we're at or below this number of degrees from where we want to be, we'll consider the limelight's aim routine "done" */
+      /**
+       * When we're at or below this number of degrees from where we want to be, we'll consider the
+       * limelight's aim routine "done"
+       */
       public static final double TURN_DONE_THRESHOLD = 0;
 
-      /* When we're at or below this number of inches from the target distance, we'll consider the limelight's drive routine "done" */
+      /**
+       * When we're at or below this number of inches from the target distance, we'll consider the
+       * limelight's drive routine "done"
+       */
       public static final double DISTANCE_DONE_THRESHOLD = 0;
     }
   }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 7ba9fd6b..28f7e819 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -77,7 +77,7 @@ public class RobotContainer {
           Constants.Limelight.AimingLimelight.SPEED_LIMIT,
           Constants.Limelight.AimingLimelight.TURN_DONE_THRESHOLD,
           Constants.Limelight.AimingLimelight.DISTANCE_DONE_THRESHOLD,
-          Constants.Limelight.AimingLimelight.UPPER_HUB_TAPE_HEIGHT);
+          Constants.Limelight.AimingLimelight.AMP_APRILTAG_HEIGHT);
 
   /*
    * ***********************

From 0e8fa9839c3ad5832e0e6b650eff45a7e89b34e0 Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Sat, 3 Feb 2024 10:56:11 -0600
Subject: [PATCH 08/19] Fix AimWithLimelight

---
 .../frc/robot/commands/AimWithLimelight.java  | 24 ++++++++++++++-----
 1 file changed, 18 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java
index f7f96a5e..986390cb 100644
--- a/src/main/java/frc/robot/commands/AimWithLimelight.java
+++ b/src/main/java/frc/robot/commands/AimWithLimelight.java
@@ -31,9 +31,21 @@ public class AimWithLimelight extends Command {
       speedLimit,
       turnDoneThreshold,
       distanceDoneThreshold,
-      upperHubTapeHeight;
+      targetHeight;
 
-  /** Creates a new AimWithLimelight. */
+  /**
+   * @param limelight The limelight to aim with.
+   * @param steerStrength How hard to turn towards the target; between 0 and 1.
+   * @param distanceFromTarget How far from in inches we want to be from the target when we
+   *     dispense.
+   * @param mountHeight The height of the limelight off the floor.
+   * @param mountAngle The number of degrees the limelight is mounted back from perfectly vertical.
+   * @param driveStrength How hard to drive towards the target.
+   * @param speedLimit Basic speed limit to make sure we don't drive too fast.
+   * @param turnDoneThreshold The threshold in angles when we consider the aiming done.
+   * @param distanceDoneThreshold The threshold in inches when we consider the aiming done.
+   * @param targetHeight The height of the target off the floor.
+   */
   public AimWithLimelight(
       Limelight limelight,
       double steerStrength,
@@ -44,7 +56,7 @@ public AimWithLimelight(
       double speedLimit,
       double turnDoneThreshold,
       double distanceDoneThreshold,
-      double hubTapeHeight) {
+      double targetHeight) {
     this.steerStrength = steerStrength;
     this.distanceFromTarget = distanceFromTarget;
     this.mountHeight = mountHeight;
@@ -53,7 +65,7 @@ public AimWithLimelight(
     this.speedLimit = speedLimit;
     this.turnDoneThreshold = turnDoneThreshold;
     this.distanceDoneThreshold = distanceDoneThreshold;
-    this.upperHubTapeHeight = hubTapeHeight;
+    this.targetHeight = targetHeight;
     this.drive = RobotContainer.drive;
     this.limelight = limelight;
     // Use addRequirements() here to declare subsystem dependencies.
@@ -74,7 +86,7 @@ public void execute() {
       double z = limelight.getTX() * steerStrength;
       double yComponent =
           distanceFromTarget
-              - ((upperHubTapeHeight - mountHeight) / Math.tan((mountAngle + limelight.getTY())));
+              - ((targetHeight - mountHeight) / Math.tan((mountAngle + limelight.getTY())));
       double y = yComponent * (Math.PI / 180.0) * driveStrength;
       if (z > speedLimit) {
         z = speedLimit;
@@ -84,7 +96,7 @@ public void execute() {
       boolean isDistanced =
           Math.abs(
                   (distanceFromTarget)
-                      - ((upperHubTapeHeight - mountHeight)
+                      - ((targetHeight - mountHeight)
                           / Math.tan((mountAngle + limelight.getTY()) * (Math.PI / 180.0))))
               <= distanceDoneThreshold;
       if (isAngled && isDistanced) {

From 292a002b05d8904677560e2acbc829ad26d90ade Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Sat, 3 Feb 2024 12:01:10 -0600
Subject: [PATCH 09/19] Fix Limelight subsystem

---
 .../java/frc/robot/subsystems/Limelight.java  | 45 ++++++++++++-------
 1 file changed, 28 insertions(+), 17 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
index 49f17a99..0715afee 100644
--- a/src/main/java/frc/robot/subsystems/Limelight.java
+++ b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -16,22 +16,15 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.networktables.DoubleArraySubscriber;
+import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.DoubleTopic;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 /** Subsystem for the Limelight 2+ that we use for vision. */
 public class Limelight extends SubsystemBase {
 
-  /* Note to future devs: we are NOT doing the whole setting values in NetworkTables to turn the Limelight
-   * LEDS on and off thing this year (turnOnLEDs() and turnOfflLEDs() in robot2022).
-   * For one, if we're not tracking reflective targets (which as of now we are not, we're just doing
-   * AprilTags), then we shouldn't need them on at all; for another, the correct way to turn the LEDs on
-   * and off is to make the default pipeline one with them off and then switch to a pipeline which has them
-   * on if we need to turn them on. This method prevents the whole "Limelight LEDs are stuck on until robot
-   * code starts!" thing.
-   */
-
   /* This class may need logic to switch between pipelines based off of AprilTag pipeline restrictions. As
    * of now, I'm not adding them, since in theory the Limelight should just handle getting a botpos with as
    * many AprilTags in its FOV as can fit because MegaTag
@@ -41,10 +34,12 @@ public class Limelight extends SubsystemBase {
 
   /* variable chaingun, I promise we use all of these */
   private DoubleSubscriber tvSubscriber, txSubscriber, tySubscriber, taSubscriber;
-  private DoubleArraySubscriber botposSubscriber;
   /* See https://docs.limelightvision.io/en/latest/apriltags_in_3d.html#robot-localization-botpose-and-megatag
    * to understand what the heck the different indices in this array mean
    */
+  private DoubleArraySubscriber botposSubscriber;
+  private DoubleSubscriber pipelineSubcriber;
+  private DoublePublisher pipelinePublisher;
   private String name;
 
   private String fmtPath(String end) {
@@ -62,9 +57,10 @@ public Limelight(String name) {
     txSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("tx")).subscribe(0.0);
     tySubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("ty")).subscribe(0.0);
     taSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("ta")).subscribe(0.0);
-    /* In theory this won't break. It got mad when I tried to insert the array into the
-     * method like .subscribe({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) so ¯\_(ツ)_/¯
-     */
+    DoubleTopic pipelineTopic =
+        NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("pipeline"));
+    this.pipelineSubcriber = pipelineTopic.subscribe(0.0);
+    this.pipelinePublisher = pipelineTopic.publish();
     double[] defaultBotpos = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
     botposSubscriber =
         NetworkTableInstance.getDefault()
@@ -121,7 +117,7 @@ public double getTA() {
    * <p>Notably, the Z, roll, and pitch values will always be zero, since we snap the bot to the
    * floor on the Limelight.
    *
-   * @return A {@link Pose}; the robot's current position relative to the field.
+   * @return A {@link Pose3d}; the robot's current position relative to the field.
    */
   public Pose3d getBotpose() {
     double[] botpos = botposSubscriber.get();
@@ -130,7 +126,7 @@ public Pose3d getBotpose() {
   }
 
   /**
-   * Gets the robot's current post relative to the field.
+   * Gets the robot's current pose relative to the field.
    *
    * <p>Units are meters; 0,0 is at the center of the field.
    *
@@ -144,9 +140,24 @@ public Pose2d getBotpose2d() {
   }
 
   /**
-   * Subsystem periodic; runs every scheduler run. Used to update Limelight data from NetworkTables
-   * in this subsystem.
+   * Returns the current limelight pipeline.
+   *
+   * @return the current limelight pipeline.
+   */
+  public double getLimelightPipeline() {
+    return pipelineSubcriber.get();
+  }
+
+  /**
+   * Sets the limelight pipeline.
+   *
+   * @param pipeline the pipeline.
    */
+  public void setLimelightPipeline(double pipeline) {
+    pipelinePublisher.set(pipeline);
+  }
+
+  /** Subsystem periodic; runs every scheduler run. */
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

From 812a824701e8fd51d5dd2241064f72dc4bc21d7e Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Sat, 3 Feb 2024 12:18:31 -0600
Subject: [PATCH 10/19] Fix Constants and RobotContainer

---
 src/main/java/frc/robot/Constants.java      | 6 +-----
 src/main/java/frc/robot/RobotContainer.java | 2 ++
 2 files changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index c166a9ba..7299fbb1 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -85,11 +85,7 @@ public static class AimingLimelight {
       /** The number of inches from the center of the Limelight lens to the floor */
       public static final double MOUNT_HEIGHT = 0;
 
-      /** The number of inches from the retroreflective tape on the upper hub to the floor */
-      /**
-       * This is 8'8", which is what the manual says the height from the floor to the top of the
-       * upper hub is.
-       */
+      /** The height to the Amp Apriltag off the ground. */
       public static final double AMP_APRILTAG_HEIGHT = 0;
 
       /** How far in inches we want to be from the target when we shoot */
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 28f7e819..b58dde0a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -53,7 +53,9 @@ public class RobotContainer {
   /** Singleton instance of {@link Intake} for the whole robot. */
   public static Intake intake = new Intake();
 
+  /** Singleton instance of {@link Limelight} for aiming. */
   public static Limelight limelight = new Limelight("limelight");
+
   /*
    * ************
    * * COMMANDS *

From 1a9a6f7fac2f329f0f44316913c006005c5685be Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Sat, 3 Feb 2024 12:24:02 -0600
Subject: [PATCH 11/19] Fix lowercase Limelight

---
 src/main/java/frc/robot/Constants.java                 | 10 +++++-----
 src/main/java/frc/robot/commands/AimWithLimelight.java |  8 +++++---
 2 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 7299fbb1..b92ee5c8 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -75,11 +75,11 @@ public static class Intake {
     public static final double INTAKE_MOTOR_SPEED = 0.5;
   }
 
-  /** Holds contstants for the limelights. */
+  /** Holds contstants for the Limelights. */
   public static class Limelight {
-    /** Constants for aiming limelight. */
+    /** Constants for aiming Limelight. */
     public static class AimingLimelight {
-      /** The number of degrees the limelight is mounted back from perfectly vertical */
+      /** The number of degrees the Limelight is mounted back from perfectly vertical */
       public static final double MOUNT_ANGLE = 0;
 
       /** The number of inches from the center of the Limelight lens to the floor */
@@ -104,13 +104,13 @@ public static class AimingLimelight {
 
       /**
        * When we're at or below this number of degrees from where we want to be, we'll consider the
-       * limelight's aim routine "done"
+       * Limelight's aim routine "done"
        */
       public static final double TURN_DONE_THRESHOLD = 0;
 
       /**
        * When we're at or below this number of inches from the target distance, we'll consider the
-       * limelight's drive routine "done"
+       * Limelight's drive routine "done"
        */
       public static final double DISTANCE_DONE_THRESHOLD = 0;
     }
diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java
index 986390cb..4a96a673 100644
--- a/src/main/java/frc/robot/commands/AimWithLimelight.java
+++ b/src/main/java/frc/robot/commands/AimWithLimelight.java
@@ -34,12 +34,14 @@ public class AimWithLimelight extends Command {
       targetHeight;
 
   /**
-   * @param limelight The limelight to aim with.
+   * Creates a new command to aim with the Limelight.
+   *
+   * @param limelight The Limelight to aim with.
    * @param steerStrength How hard to turn towards the target; between 0 and 1.
    * @param distanceFromTarget How far from in inches we want to be from the target when we
    *     dispense.
-   * @param mountHeight The height of the limelight off the floor.
-   * @param mountAngle The number of degrees the limelight is mounted back from perfectly vertical.
+   * @param mountHeight The height of the Limelight off the floor.
+   * @param mountAngle The number of degrees the Limelight is mounted back from perfectly vertical.
    * @param driveStrength How hard to drive towards the target.
    * @param speedLimit Basic speed limit to make sure we don't drive too fast.
    * @param turnDoneThreshold The threshold in angles when we consider the aiming done.

From 8dfa7649adc582c046e08fe4ea9f150f936753a8 Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Sat, 3 Feb 2024 12:36:21 -0600
Subject: [PATCH 12/19] Fix AimWithLimelight javadoc

---
 .../java/frc/robot/commands/AimWithLimelight.java    | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java
index 4a96a673..d10a8678 100644
--- a/src/main/java/frc/robot/commands/AimWithLimelight.java
+++ b/src/main/java/frc/robot/commands/AimWithLimelight.java
@@ -36,15 +36,17 @@ public class AimWithLimelight extends Command {
   /**
    * Creates a new command to aim with the Limelight.
    *
-   * @param limelight The Limelight to aim with.
+   * @param limelight The Limelight subsystem to aim with.
    * @param steerStrength How hard to turn towards the target; between 0 and 1.
    * @param distanceFromTarget How far from in inches we want to be from the target when we
    *     dispense.
-   * @param mountHeight The height of the Limelight off the floor.
+   * @param mountHeight The height of the center of the Limelight lense off the floor.
    * @param mountAngle The number of degrees the Limelight is mounted back from perfectly vertical.
-   * @param driveStrength How hard to drive towards the target.
-   * @param speedLimit Basic speed limit to make sure we don't drive too fast.
-   * @param turnDoneThreshold The threshold in angles when we consider the aiming done.
+   *     Positive means backwards.
+   * @param driveStrength How hard to drive towards the target; between 0 and 1.
+   * @param speedLimit Basic speed limit to make sure we don't drive too fast. Percentage of max
+   *     speed the robot can go.
+   * @param turnDoneThreshold The threshold in degrees when we consider the aiming done.
    * @param distanceDoneThreshold The threshold in inches when we consider the aiming done.
    * @param targetHeight The height of the target off the floor.
    */

From eb34deb419334f0ebf9de169232a866c3c9c0542 Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Sat, 3 Feb 2024 12:38:40 -0600
Subject: [PATCH 13/19] Fix Limelight javadoc

---
 .../java/frc/robot/subsystems/Limelight.java  | 19 ++++++-------------
 1 file changed, 6 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
index 0715afee..a2f6d016 100644
--- a/src/main/java/frc/robot/subsystems/Limelight.java
+++ b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -25,13 +25,6 @@
 /** Subsystem for the Limelight 2+ that we use for vision. */
 public class Limelight extends SubsystemBase {
 
-  /* This class may need logic to switch between pipelines based off of AprilTag pipeline restrictions. As
-   * of now, I'm not adding them, since in theory the Limelight should just handle getting a botpos with as
-   * many AprilTags in its FOV as can fit because MegaTag
-   * (https://docs.limelightvision.io/en/latest/apriltags_in_3d.html#robot-localization-botpose-and-megatag).
-   */
-  /* it would be so cool if we just didn't have to do multiple pipelines at all this year */
-
   /* variable chaingun, I promise we use all of these */
   private DoubleSubscriber tvSubscriber, txSubscriber, tySubscriber, taSubscriber;
   /* See https://docs.limelightvision.io/en/latest/apriltags_in_3d.html#robot-localization-botpose-and-megatag
@@ -47,9 +40,9 @@ private String fmtPath(String end) {
   }
 
   /**
-   * Creates a new Limelight. Should be run once from {@link frc.robot.RobotContainer}.
+   * Creates a new Limelight.
    *
-   * @param name The hostname of the limelight
+   * @param name The hostname of the Limelight
    */
   public Limelight(String name) {
     this.name = name;
@@ -140,18 +133,18 @@ public Pose2d getBotpose2d() {
   }
 
   /**
-   * Returns the current limelight pipeline.
+   * Returns the current Limelight pipeline number.
    *
-   * @return the current limelight pipeline.
+   * @return the current Limelight pipeline number.
    */
   public double getLimelightPipeline() {
     return pipelineSubcriber.get();
   }
 
   /**
-   * Sets the limelight pipeline.
+   * Sets the Limelight pipeline number.
    *
-   * @param pipeline the pipeline.
+   * @param pipeline the pipeline number.
    */
   public void setLimelightPipeline(double pipeline) {
     pipelinePublisher.set(pipeline);

From 8d9d61e7e077324fd44d4b1dd602566697abdce3 Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Sat, 3 Feb 2024 12:53:19 -0600
Subject: [PATCH 14/19] Fix javadoc

---
 src/main/java/frc/robot/Constants.java                 | 2 ++
 src/main/java/frc/robot/RobotContainer.java            | 3 ++-
 src/main/java/frc/robot/commands/AimWithLimelight.java | 7 +++----
 3 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index b92ee5c8..5091e3e4 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -79,6 +79,8 @@ public static class Intake {
   public static class Limelight {
     /** Constants for aiming Limelight. */
     public static class AimingLimelight {
+      public static final String LIMELIGHT_NAME = "aiming_limelight";
+
       /** The number of degrees the Limelight is mounted back from perfectly vertical */
       public static final double MOUNT_ANGLE = 0;
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index b58dde0a..6ee1ec76 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -54,7 +54,8 @@ public class RobotContainer {
   public static Intake intake = new Intake();
 
   /** Singleton instance of {@link Limelight} for aiming. */
-  public static Limelight limelight = new Limelight("limelight");
+  public static Limelight limelight =
+      new Limelight(Constants.Limelight.AimingLimelight.LIMELIGHT_NAME);
 
   /*
    * ************
diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java
index d10a8678..aafc5eae 100644
--- a/src/main/java/frc/robot/commands/AimWithLimelight.java
+++ b/src/main/java/frc/robot/commands/AimWithLimelight.java
@@ -38,11 +38,10 @@ public class AimWithLimelight extends Command {
    *
    * @param limelight The Limelight subsystem to aim with.
    * @param steerStrength How hard to turn towards the target; between 0 and 1.
-   * @param distanceFromTarget How far from in inches we want to be from the target when we
-   *     dispense.
-   * @param mountHeight The height of the center of the Limelight lense off the floor.
+   * @param distanceFromTarget How far from in inches we want to be from the target.
+   * @param mountHeight The height of the center of the Limelight lens off the floor.
    * @param mountAngle The number of degrees the Limelight is mounted back from perfectly vertical.
-   *     Positive means backwards.
+   *     Positive means rotated such that the lens is facing up, and not down.
    * @param driveStrength How hard to drive towards the target; between 0 and 1.
    * @param speedLimit Basic speed limit to make sure we don't drive too fast. Percentage of max
    *     speed the robot can go.

From cb49dbc3175f4dfc4663236f281cea7d7117c64a Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Mon, 5 Feb 2024 14:53:12 -0600
Subject: [PATCH 15/19] Clarify Range

---
 src/main/java/frc/robot/commands/AimWithLimelight.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java
index aafc5eae..0b022836 100644
--- a/src/main/java/frc/robot/commands/AimWithLimelight.java
+++ b/src/main/java/frc/robot/commands/AimWithLimelight.java
@@ -44,7 +44,7 @@ public class AimWithLimelight extends Command {
    *     Positive means rotated such that the lens is facing up, and not down.
    * @param driveStrength How hard to drive towards the target; between 0 and 1.
    * @param speedLimit Basic speed limit to make sure we don't drive too fast. Percentage of max
-   *     speed the robot can go.
+   *     speed the robot can go. 0 to 1.
    * @param turnDoneThreshold The threshold in degrees when we consider the aiming done.
    * @param distanceDoneThreshold The threshold in inches when we consider the aiming done.
    * @param targetHeight The height of the target off the floor.

From bacdc4a9aab4a23e2469861034e861e87e90461a Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Mon, 5 Feb 2024 16:03:38 -0600
Subject: [PATCH 16/19] Remove out-dated comment

---
 src/main/java/frc/robot/subsystems/Limelight.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
index a2f6d016..56b3ef4f 100644
--- a/src/main/java/frc/robot/subsystems/Limelight.java
+++ b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -62,7 +62,6 @@ public Limelight(String name) {
   }
 
   /* now its time for getter method chaingun, which I have to write manually because VS Code */
-  /* nvm im making a python script do this for me */
 
   /**
    * Returns whether the Limelight has a valid target.

From d404706de42e24325cd01f0eecfeb29331c77e55 Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Mon, 5 Feb 2024 16:04:47 -0600
Subject: [PATCH 17/19] Fix aiming limelight name

---
 src/main/java/frc/robot/Constants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 5091e3e4..72cac176 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -79,7 +79,7 @@ public static class Intake {
   public static class Limelight {
     /** Constants for aiming Limelight. */
     public static class AimingLimelight {
-      public static final String LIMELIGHT_NAME = "aiming_limelight";
+      public static final String LIMELIGHT_NAME = "limelight";
 
       /** The number of degrees the Limelight is mounted back from perfectly vertical */
       public static final double MOUNT_ANGLE = 0;

From 71c3096ecdcfdcd9665c1fa6bc3080e4eda8deca Mon Sep 17 00:00:00 2001
From: MarissaKoglesby <156854363+MarissaKoglesby@users.noreply.github.com>
Date: Mon, 5 Feb 2024 16:16:20 -0600
Subject: [PATCH 18/19] Fix AimWithLimelight

---
 src/main/java/frc/robot/RobotContainer.java            | 1 -
 src/main/java/frc/robot/commands/AimWithLimelight.java | 3 ++-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index cc807fae..3f167ee5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -55,7 +55,6 @@ public class RobotContainer {
   /** Singleton instance of {@link Intake} for the whole robot. */
   public static Intake intake = new Intake();
 
-
   /** Singleton instance of {@link Limelight} for aiming. */
   public static Limelight limelight =
       new Limelight(Constants.Limelight.AimingLimelight.LIMELIGHT_NAME);
diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java
index 0b022836..2735a64f 100644
--- a/src/main/java/frc/robot/commands/AimWithLimelight.java
+++ b/src/main/java/frc/robot/commands/AimWithLimelight.java
@@ -11,6 +11,7 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.RobotContainer;
 import frc.robot.subsystems.Drive;
@@ -94,7 +95,7 @@ public void execute() {
       if (z > speedLimit) {
         z = speedLimit;
       }
-      drive.driveRobotOriented(y * -1.0, 0.0, z);
+      drive.driveRobotOriented(new Translation2d(y * -1.0, 0.0), z);
       boolean isAngled = Math.abs(limelight.getTX()) < turnDoneThreshold;
       boolean isDistanced =
           Math.abs(

From 618d3c1da24df8fc18bc020080fe4ee3d92cb065 Mon Sep 17 00:00:00 2001
From: quinnnnnnnnnnnnnn <81661589+QuackitsQuinn@users.noreply.github.com>
Date: Mon, 5 Feb 2024 16:24:25 -0600
Subject: [PATCH 19/19] Fix merge bug

---
 src/main/java/frc/robot/RobotContainer.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 62877c65..8d20513b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -16,7 +16,6 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.AimWithLimelight;
-import frc.robot.commands.DeployPneumatics;
 import frc.robot.commands.DeployUrMom;
 import frc.robot.commands.EnterXMode;
 import frc.robot.commands.RunIntake;