made unit tests

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));

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;
 

src/main/java/frc/robot/commands/GamePieceLocate.java → 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));
-  }
 }

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());
+  }
+}