Auto drive onto ball

src/main/java/frc/robot/Constants.java

@@ -195,6 +195,16 @@ public enum AccelerationLimits {
      */
     public static final double MAX_ANGULAR_ACCELERATION = Math.toRadians(360 * 22);
 
+    /**
+     * Angular offset of the intake limelight (degrees)
+     */
+    public static final double INTAKE_LIMELIGHT_ANGLE_OFFSET = 50;
+    /**
+     * Height offset of the intake limelight from the ground (inches)
+     */
+    public static final double INTAKE_LIMELIGHT_HEIGHT_OFFSET = 0.75;
+    public static final double MAX_CENTERING_SPEED = 0.5;
+
     //field/Vision Manager constants
     public static final Translation2d GOAL_POSITION = new Translation2d(8.25, 0);
     public static final double VISION_PREDICT_AHEAD_TIME = 0.5;

src/main/java/frc/robot/Robot.java

@@ -20,7 +20,9 @@
 import frc.subsystem.*;
 import frc.subsystem.Climber.ClimbState;
 import frc.subsystem.Hopper.HopperState;
+import frc.subsystem.Intake.IntakeSolState;
 import frc.utility.*;
+import frc.utility.Controller.XboxAxes;
 import frc.utility.Controller.XboxButtons;
 import frc.utility.Limelight.LedMode;
 import frc.utility.Limelight.StreamingMode;
@@ -525,7 +527,6 @@ public void teleopInit() {
     }
 
     private final Object driverForcingVisionOn = new Object();
-    private final Object buttonPanelForcingVisionOn = new Object();
     private final Object resettingPoseVisionOn = new Object();
 
     /*
@@ -546,8 +547,7 @@ public void teleopPeriodic() {
 
 
         // Will terminate climb auto thread if any stick movement happens
-        if (autoThread != null && autoThread.isAlive() &&
-                !getControllerDriveInputs().equals(NO_MOTION_CONTROLLER_INPUTS)) {
+        if (autoThread != null && autoThread.isAlive() && !getControllerDriveInputs().equals(NO_MOTION_CONTROLLER_INPUTS)) {
             killAuto();
         }
 
@@ -607,12 +607,18 @@ public void teleopPeriodic() {
             }
 
             visionManager.unForceVisionOn(driverForcingVisionOn);
+
+
             if (GRAPPLE_CLIMB || Climber.getInstance().getClimbState() == ClimbState.IDLE || Climber.getInstance().isPaused()) {
                 // If we're climbing don't allow the robot to be driven
                 if (usingDPad) {
                     drive.updateTurn(getControllerDriveInputs(), Constants.CLIMB_LINEUP_ANGLE, useFieldRelative, 0);
                 } else {
-                    doNormalDriving();
+                    if (xbox.getRawAxis(XboxAxes.LEFT_TRIGGER) > 0.1 && intake.getIntakeSolState() == IntakeSolState.OPEN) {
+                        drive.centerOntoBall(getControllerDriveInputs(), useFieldRelative);
+                    } else {
+                        doNormalDriving();
+                    }
                 }
             } else {
                 // Stop the robot from moving if we're not issuing other commands to the drivebase
@@ -870,7 +876,7 @@ public void testPeriodic() {
         final Hopper hopper = Hopper.getInstance();
         final Intake intake = Intake.getInstance();
         final Shooter shooter = Shooter.getInstance();
-        
+
         if (!Constants.GRAPPLE_CLIMB) {
             Climber climber = Climber.getInstance();
 

src/main/java/frc/subsystem/Drive.java

@@ -27,6 +27,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants;
 import frc.utility.ControllerDriveInputs;
+import frc.utility.Limelight;
 import frc.utility.Timer;
 import frc.utility.controllers.LazyTalonFX;
 import frc.utility.wpimodified.HolonomicDriveController;
@@ -76,7 +77,7 @@ public void resetAuto() {
     }
 
     public enum DriveState {
-        TELEOP, TURN, HOLD, DONE, RAMSETE, STOP
+        TELEOP, TURN, HOLD, DONE, RAMSETE, SEARCH_FOR_BALL, STOP
     }
 
     public boolean useRelativeEncoderPosition = false;
@@ -678,6 +679,67 @@ private void updateRamsete() {
         }
     }
 
+    PIDController centerOntoBallPID = new PIDController(1, 0, 0, 0.02);
+
+    public void centerOntoBall(ControllerDriveInputs inputs, boolean fieldRelativeEnabled) {
+        Limelight limelight = Limelight.getInstance("limelight-intake");
+        if (limelight.isTargetVisible()) {
+
+            /* Top Down View
+            [] - ball
+                        X
+                    []------
+                     \     |
+                      \    |
+                       \   | Y
+                        \  |
+                         \ |
+                          \|
+              |---|----|----|----|----| Intake
+             */
+
+            double distY = Math.tan(Math.toRadians(limelight.getVerticalOffset() + INTAKE_LIMELIGHT_ANGLE_OFFSET))
+                    * INTAKE_LIMELIGHT_HEIGHT_OFFSET;
+
+            double distX = Math.tan(limelight.getHorizontalOffset()) * distY;
+
+
+            double allowedError = DriverStation.isAutonomous() ? 0 : (distY * 0.2) + 20;
+
+            double pidError;
+            if (Math.abs(distX) < allowedError) {
+                pidError = 0;
+            } else {
+                pidError = distX - (Math.signum(distX) * allowedError);
+            }
+
+            double strafeCommand = centerOntoBallPID.calculate(pidError, 0);
+            strafeCommand = Math.signum(strafeCommand) * Math.min(Math.abs(strafeCommand), MAX_CENTERING_SPEED);
+            // Ensure that the driver can always override the PID
+
+            Translation2d correction = new Translation2d(0, strafeCommand);
+
+            ChassisSpeeds chassisSpeeds;
+            if (useFieldRelative && fieldRelativeEnabled) {
+                correction = correction.rotateBy(RobotTracker.getInstance().getGyroAngle()); //Make it perpendicular to the robot
+                chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+                        DRIVE_HIGH_SPEED_M * inputs.getX() + correction.getX(),
+                        DRIVE_HIGH_SPEED_M * inputs.getY() + correction.getY(),
+                        inputs.getRotation() * 7,
+                        RobotTracker.getInstance().getGyroAngle());
+            } else {
+                chassisSpeeds = new ChassisSpeeds(
+                        DRIVE_HIGH_SPEED_M * inputs.getX() + correction.getX(),
+                        DRIVE_HIGH_SPEED_M * inputs.getY() + correction.getY(),
+                        inputs.getRotation() * 7);
+            }
+
+            swerveDrive(chassisSpeeds);
+        } else {
+            swerveDriveFieldRelative(inputs);
+        }
+    }
+
     private double getTurnPidDeltaSpeed(@NotNull TrapezoidProfile.State autoAimingRotationGoal) {
         turnPID.setSetpoint(autoAimingRotationGoal.position);
 
@@ -725,6 +787,20 @@ public void update() {
                 break;
             case STOP:
                 swerveDrive(new ChassisSpeeds(0, 0, 0));
+                break;
+            case SEARCH_FOR_BALL:
+                searchForBall();
+                break;
+        }
+    }
+
+    private void searchForBall() {
+        Limelight intakeLimelight = Limelight.getInstance("limelight-intake");
+        if (intakeLimelight.isTargetVisible()) {
+            Translation2d movement = new Translation2d(0.25, 0);
+            centerOntoBall(new ControllerDriveInputs(movement.getX(), movement.getY(), 0), false);
+        } else {
+            swerveDrive(new ChassisSpeeds(0, 0, 0));
         }
     }
 
@@ -859,6 +935,15 @@ public void logData() {
             logData("Swerve Motor " + i + " Current", swerveMotors[i].getStatorCurrent());
         }
         logData("Drive State", driveState.toString());
+
+        Limelight limelight = Limelight.getInstance("limelight-intake");
+        if (limelight.isTargetVisible()) {
+            double distY = Math.tan(Math.toRadians(limelight.getVerticalOffset() + INTAKE_LIMELIGHT_ANGLE_OFFSET))
+                    * INTAKE_LIMELIGHT_HEIGHT_OFFSET;
+            double distX = Math.tan(limelight.getHorizontalOffset()) * distY;
+            logData("Limelight Distance X", distX);
+            logData("Limelight Distance Y", distY);
+        }
     }
 
 
@@ -946,4 +1031,8 @@ public void turnToAngle(double degrees) throws InterruptedException {
             Thread.sleep(20);
         }
     }
+
+    public synchronized void setSearchForBall() {
+        setDriveState(DriveState.SEARCH_FOR_BALL);
+    }
 }

src/main/java/frc/subsystem/VisionManager.java

@@ -7,7 +7,6 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.drive.Vector2d;
 import frc.robot.Constants;
 import frc.subsystem.BlinkinLED.BlinkinLedMode;
 import frc.subsystem.BlinkinLED.LedStatus;
@@ -86,17 +85,8 @@ public void logData() {
 
         logData("Distance to Target", Units.metersToInches(getDistanceToTarget()));
         logData("Rotation Target", getAngleToTarget().getDegrees());
-
-        Vector3D correctVector = limelight.getCorrectTargetVector();
-        logData("New Distance", Math.hypot(correctVector.getX(), correctVector.getZ()));
-
         logData("Old Distance ", limelight.getDistance() + Constants.GOAL_RADIUS_IN + 23);
 
-        Vector2d targetPx = limelight.getTargetPosInCameraPixels();
-
-        logData("py", targetPx.y);
-        logData("px", targetPx.x);
-
 //        Vector2d newRelGoalPos = limelight.getCorrectGoalPos();
 //        logData("New Z", newRelGoalPos.x);
 //        logData("New X", newRelGoalPos.y);

src/main/java/frc/utility/Limelight.java

@@ -340,6 +340,7 @@ public Vector2d getTargetPosInCameraPixels() {
     );
 
     private static final MatBuilder<N3, N1> THREE_BY_ONE_MAT_BUILDER = new MatBuilder<>(Nat.N3(), Nat.N1());
+
     /**
      * @return The Correct distance from the limelight to the target in IN. This correctly compensates for the angle of the camera
      * to ensure a consistent distance as the robot rotates.