From b3f3ae444324c4d339e4aa5dc59fe75bd3d56558 Mon Sep 17 00:00:00 2001
From: Shikhar <mj@mikovsky.com>
Date: Mon, 23 Feb 2026 23:28:25 -0700
Subject: [PATCH] Some changes

---
 .../java/frc4388/robot/RobotContainer.java    |  1 +
 .../java/frc4388/robot/subsystems/Lidar.java  | 20 +++++++++----------
 .../robot/subsystems/swerve/SwerveDrive.java  |  9 +--------
 3 files changed, 12 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index c3e51c2..49e9ec1 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/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(
diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java
index d6685d9..001d241 100644
--- a/src/main/java/frc4388/robot/subsystems/Lidar.java
+++ b/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);
             }
         }
 
diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
index 228429e..4d2c808 100644
--- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+++ b/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() {