Auto drive onto ball

src/main/deploy/shooter/shooterconfig.json

@@ -1,71 +1,71 @@
 {
   "shooterConfigs" : [ {
-    "hoodEjectAngle" : 67.0,
-    "flywheelSpeed" : 1800.0,
+    "hoodEjectAngle" : 68.0,
+    "flywheelSpeed" : 1700.0,
     "distance" : 40.0
   }, {
     "hoodEjectAngle" : 62.0,
-    "flywheelSpeed" : 1750.0,
+    "flywheelSpeed" : 1700.0,
     "distance" : 60.0
   }, {
     "hoodEjectAngle" : 56.0,
-    "flywheelSpeed" : 1800.0,
+    "flywheelSpeed" : 1700.0,
     "distance" : 82.0
   }, {
     "hoodEjectAngle" : 56.0,
-    "flywheelSpeed" : 2000.0,
+    "flywheelSpeed" : 1800.0,
     "distance" : 106.0
   }, {
     "hoodEjectAngle" : 53.0,
-    "flywheelSpeed" : 2000.0,
+    "flywheelSpeed" : 1800.0,
     "distance" : 115.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2000.0,
+    "flywheelSpeed" : 1900.0,
     "distance" : 120.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2100.0,
+    "flywheelSpeed" : 2000.0,
     "distance" : 133.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2150.0,
+    "flywheelSpeed" : 2100.0,
     "distance" : 144.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2300.0,
+    "flywheelSpeed" : 2200.0,
     "distance" : 157.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2350.0,
+    "flywheelSpeed" : 2200.0,
     "distance" : 175.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2460.0,
+    "flywheelSpeed" : 2300.0,
     "distance" : 190.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2450.0,
+    "flywheelSpeed" : 2400.0,
     "distance" : 200.0
   }, {
     "hoodEjectAngle" : 51.0,
     "flywheelSpeed" : 2500.0,
     "distance" : 210.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2550.0,
+    "flywheelSpeed" : 2600.0,
     "distance" : 225.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2650.0,
+    "flywheelSpeed" : 2700.0,
     "distance" : 250.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 3000.0,
+    "flywheelSpeed" : 2800.0,
     "distance" : 270.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 3000.0,
+    "flywheelSpeed" : 2900.0,
     "distance" : 299.0
   } ]
 }

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

@@ -19,7 +19,9 @@
 import frc.auton.guiauto.serialization.reflection.ClassInformationSender;
 import frc.subsystem.*;
 import frc.subsystem.Climber.ClimbState;
+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;
@@ -436,7 +438,6 @@ public void teleopInit() {
     }
 
     private final Object driverForcingVisionOn = new Object();
-    private final Object buttonPanelForcingVisionOn = new Object();
     private final Object resettingPoseVisionOn = new Object();
 
     /**
@@ -464,7 +465,9 @@ public void teleopPeriodic() {
             shooter.setFiring(false);
             shooter.setSpeed(0);
             visionManager.unForceVisionOn(driverForcingVisionOn);
-            if (climber.getClimbState() == ClimbState.IDLE || climber.isPaused()) { // If we're climbing don't allow the robot to be
+            if (xbox.getRawAxis(XboxAxes.LEFT_TRIGGER) > 0.1 && intake.getIntakeSolState() == IntakeSolState.OPEN) {
+                drive.centerOntoBall(getControllerDriveInputs(), useFieldRelative);
+            } else if (climber.getClimbState() == ClimbState.IDLE || climber.isPaused()) { // If we're climbing don't allow the robot to be
                 // driven
                 doNormalDriving();
             } else {

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

@@ -28,6 +28,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;
@@ -65,7 +66,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;
@@ -573,6 +574,62 @@ 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() + 50)) * 20;
+
+            double distX = Math.tan(limelight.getHorizontalOffset()) * distY;
+
+            double allowedError = (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);
+            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);
+        }
+    }
+
     public void setAutoRotation(@NotNull Rotation2d rotation) {
         autoTargetHeading = rotation;
         System.out.println("new rotation" + rotation.getDegrees());
@@ -603,6 +660,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));
         }
     }
 
@@ -836,4 +907,8 @@ public void turnToAngle(double degrees) throws InterruptedException {
             Thread.onSpinWait();
         }
     }
+
+    public synchronized void setSearchForBall() {
+        setDriveState(DriveState.SEARCH_FOR_BALL);
+    }
 }

src/main/java/frc/utility/ControllerDriveInputs.java

@@ -74,8 +74,13 @@ public double getRotation() {
      * @return {@link ControllerDriveInputs}
      */
     public @NotNull ControllerDriveInputs squareInputs() {
-        x = Math.copySign(x * x, x);
-        y = Math.copySign(y * y, y);
+        double dist = Math.hypot(x, y);
+        double distSquared = dist * dist;
+        double angle = Math.atan2(y, x);
+
+
+        x = distSquared * Math.cos(angle);
+        y = distSquared * Math.sin(angle);
         rotation = Math.copySign(rotation * rotation, rotation);
         return this;
     }
@@ -86,8 +91,12 @@ public double getRotation() {
      * @return {@link ControllerDriveInputs}
      */
     public @NotNull ControllerDriveInputs cubeInputs() {
-        x = x * x * x;
-        y = y * y * y;
+        double dist = Math.hypot(x, y);
+        double distCubed = dist * dist * dist;
+        double angle = Math.atan2(y, x);
+
+        x = distCubed * Math.cos(angle);
+        y = distCubed * Math.sin(angle);
         rotation = rotation * rotation * rotation;
         return this;
     }