diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index cffea4f6..de9e5ce5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -22,7 +22,6 @@
 import frc.robot.autochooser.FieldLocation;
 import frc.robot.autochooser.chooser.AutoChooser2025;
 import frc.robot.autochooser.event.RealAutoEventProvider;
-import frc.robot.commands.UnitTester;
 import frc.robot.commands.byebye.ByeByeToFwrLimit;
 import frc.robot.commands.byebye.ByeByeToRevLimit;
 import frc.robot.commands.coral.IntakeCoral;
@@ -80,7 +79,6 @@
 import frc.robot.utils.logging.LoggableIO;
 import frc.robot.utils.motor.Gain;
 import frc.robot.utils.motor.PID;
-import frc.robot.utils.shuffleboard.SmartShuffleboard;
 import frc.robot.utils.simulation.RobotVisualizer;
 import frc.robot.utils.simulation.SwerveSimulationUtils;
 import java.util.function.Consumer;
@@ -445,7 +443,6 @@ public RobotVisualizer getRobotVisualizer() {
   }
 
   public void putShuffleboardCommands() {
-    SmartShuffleboard.putCommand("What Branch Unit Test", "Test 1", new UnitTester());
     if (Constants.CORAL_DEBUG) {
       SmartDashboard.putData(
           "Shoot Coral", new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED));
diff --git a/src/main/java/frc/robot/commands/UnitTester.java b/src/main/java/frc/robot/commands/UnitTester.java
deleted file mode 100644
index 1ba6c7cc..00000000
--- a/src/main/java/frc/robot/commands/UnitTester.java
+++ /dev/null
@@ -1,16 +0,0 @@
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.utils.shuffleboard.SmartShuffleboard;
-
-public class UnitTester extends Command {
-  @Override
-  public void execute() {
-    SmartShuffleboard.put("Branch", "Branch", GamePieceLocate.UnitTest1().toString());
-  }
-
-  @Override
-  public boolean isFinished() {
-    return true;
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java
index 04692537..da30f85c 100644
--- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java
+++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java
@@ -8,9 +8,9 @@
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.robot.Robot;
-import frc.robot.commands.GamePieceLocate;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.swervev3.SwerveDrivetrain;
+import frc.robot.utils.GamePieceLocate;
 import frc.robot.utils.diag.DiagLimelight;
 import java.util.Map;
 
diff --git a/src/main/java/frc/robot/commands/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java
similarity index 96%
rename from src/main/java/frc/robot/commands/GamePieceLocate.java
rename to src/main/java/frc/robot/utils/GamePieceLocate.java
index 230fd1ee..2d3dc80e 100644
--- a/src/main/java/frc/robot/commands/GamePieceLocate.java
+++ b/src/main/java/frc/robot/utils/GamePieceLocate.java
@@ -1,4 +1,4 @@
-package frc.robot.commands;
+package frc.robot.utils;
 
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
@@ -181,10 +181,4 @@ public static AlgaePositions findAlgaePos(Pose2d robotPos, Vector<N2> piecePos)
     }
     return closest;
   }
-
-  public static AlgaePositions UnitTest1() {
-    double x = 0.31119220563058896357;
-    double y = 0.06719517620178168871;
-    return findAlgaePos(new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y));
-  }
 }
diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java
new file mode 100644
index 00000000..631037a4
--- /dev/null
+++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java
@@ -0,0 +1,23 @@
+package frc.robot.utils;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.constants.AlgaePositions;
+import org.junit.jupiter.api.Test;
+
+class GamePieceLocateTest {
+
+  @Test
+  void findAlgaePos() {
+    double x = 0.31119220563058896357;
+    double y = 0.06719517620178168871;
+    AlgaePositions algaePos =
+        GamePieceLocate.findAlgaePos(
+            new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y));
+    assertEquals(5.144949195, algaePos.getPosition().getX());
+    assertEquals(4.0259, algaePos.getPosition().getY());
+  }
+}