Some changes

src/main/java/frc4388/robot/RobotContainer.java

@@ -101,6 +101,7 @@ public class RobotContainer {
     );
 
     private Command LidarIntake = new SequentialCommandGroup(
+        // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball
         RobotIntakeDown,
         new RunCommand(
         () -> m_robotSwerveDrive.driveWithInputRotation(

src/main/java/frc4388/robot/subsystems/Lidar.java

@@ -64,6 +64,11 @@ public class Lidar extends SubsystemBase implements ScanListener {
     public Rotation2d getLatestBallAngle() {
         return latestBallAngleDeg;
     }
+
+    public boolean outOfBounds(Translation2d closestBall){
+        // Make sure robot doesn't go off the earth
+        return true;
+    }
     
 
     @Override
@@ -237,19 +242,14 @@ public class Lidar extends SubsystemBase implements ScanListener {
         }
 
         if (closestBallPrior != null) {
-            if (closestBall.getDistance(closestBallPrior) < 0.1){
+            if (closestBall.getDistance(closestBallPrior) < 0.1 && outOfBounds(closestBall)){
 
-                Point scaledPoint = new Point( (WIDTH/2) + (closestBall.getX() / scale), (WIDTH/2) + (closestBall.getY() / scale));
-
-                Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1);
-
-                // System.out.println("Drive "+ Units.metersToInches(closestBall.x) + " inches forward and " + Units.metersToInches(closestBall.y) + "inches to the right");
+                // Point scaledPoint = new Point( (WIDTH/2) + (closestBall.getX() / scale), (WIDTH/2) + (closestBall.getY() / scale));
+                // Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1);
                 latestBallAngleDeg = new Rotation2d(Math.atan((closestBall.getY())/(closestBall.getX()))/Math.PI*180);
-                System.out.println("!!" + latestBallAngleDeg);
-
             } else {
-                Point scaledPoint = new Point( (WIDTH/2) + (closestBallPrior.getX() / scale), (WIDTH/2) + (closestBallPrior.getX() / scale));
-                Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1);
+                // Point scaledPoint = new Point( (WIDTH/2) + (closestBallPrior.getX() / scale), (WIDTH/2) + (closestBallPrior.getX() / scale));
+                // Imgproc.circle(mat, scaledPoint, (int) (RADIUS_OFFSET / scale), new Scalar(200, 25, 52), -1);
             }
         }
 

src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java

@@ -405,20 +405,13 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     }
 
     public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
-        // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the
-        // swerve drive is still going:
-        // stopModules(); // stop the swerve
-
-        // if (leftStick.getNorm() < 0.05) //if no imput
-        // return; // don't bother doing swerve drive math and return early.
-
+    
         leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
         io.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(leftStick.getX() * -speedAdjust)
                 .withVelocityY(leftStick.getY() * speedAdjust)
                 .withTargetDirection(rot));
-        // double
     }
 
     public double getGyroAngle() {