BALLING into main

src/main/java/frc/robot/Controls.java

@@ -8,13 +8,8 @@
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import com.ctre.phoenix6.swerve.SwerveRequest.SwerveDriveBrake;
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.wpilibj.DataLogManager;
@@ -33,7 +28,7 @@
 import frc.robot.subsystems.auto.AutoDriveRotate;
 import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode;
 import frc.robot.subsystems.launcher.TurretSubsystem;
-import frc.robot.util.AllianceUtils;
+import frc.robot.util.GetTargetFromPose;
 import frc.robot.util.HubShiftUtil;
 import frc.robot.util.robotType.RobotType;
 import frc.robot.util.robotType.RobotTypesEnum;
@@ -77,15 +72,6 @@ public class Controls {
   private final CommandXboxController visionTestController =
       new CommandXboxController(VISION_TEST_CONTROLLER_PORT);
 
-  AprilTagFieldLayout aprilTagFieldLayout =
-      AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
-  // Robot with bumpers is 36.875 inches by 30.750 inches
-  Transform2d robotOffsetFromTag =
-      new Transform2d(
-          new Translation2d(Units.inchesToMeters(30.750 / 2), 0), Rotation2d.fromDegrees(180));
-  Pose2d redHub = aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(robotOffsetFromTag);
-  Pose2d blueHub = aprilTagFieldLayout.getTagPose(26).get().toPose2d().plus(robotOffsetFromTag);
-
   private LEDMode ledsMode = LEDMode.DEFAULT;
   public static IntakeMode intakeMode = IntakeMode.RETRACTED;
 
@@ -148,7 +134,10 @@ private void configureIndexingBindings() {
     }
     connected(indexingTestController)
         .and(indexingTestController.leftTrigger())
-        .whileTrue(s.indexerSubsystem.runIndexer());
+        .whileTrue(
+            s.launcherSubsystem != null
+                ? s.indexerSubsystem.runIndexer(() -> s.flywheels.getTargetSpeed())
+                : s.indexerSubsystem.runIndexer());
   }
 
   private Command rumble(CommandXboxController controller, double vibration, Time duration) {
@@ -254,8 +243,7 @@ private void configureDrivebaseBindings() {
         .back()
         .onTrue(
             s.drivebaseSubsystem
-                .runOnce(
-                    () -> s.drivebaseSubsystem.resetPose(AllianceUtils.isRed() ? redHub : blueHub))
+                .runOnce(() -> s.drivebaseSubsystem.resetPose(GetTargetFromPose.getRestPose()))
                 .withName("Reset to Hub"));
   }
 
@@ -534,8 +522,7 @@ private void configureTurretBindings() {
         .and(turretTestController.rightBumper())
         .onTrue(
             s.drivebaseSubsystem
-                .runOnce(
-                    () -> s.drivebaseSubsystem.resetPose(AllianceUtils.isRed() ? redHub : blueHub))
+                .runOnce(() -> s.drivebaseSubsystem.resetPose(GetTargetFromPose.getRestPose()))
                 .withName("Reset to Hub"));
     driverController
         .rightStick()

src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java

@@ -28,6 +28,7 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.generated.CompTunerConstants;
 import frc.robot.util.AllianceUtils;
+import frc.robot.util.GetTargetFromPose;
 import java.util.function.Supplier;
 
 /**
@@ -298,6 +299,11 @@ public static double tau(double value) {
 
   /** Clamps the pose estimator to the field boundary. Does not affect driving. */
   private void clampPoseToField() {
+    if (GetTargetFromPose.BALLING.get()) {
+      // Don't clamp pose if balling, since we may intentionally drive outside the field boundaries
+      // to pick up balls
+      return;
+    }
     Pose2d current = getState().Pose;
     double clampedX = MathUtil.clamp(current.getX(), 0.0, FIELD_X_MAX);
     double clampedY = MathUtil.clamp(current.getY(), 0.0, FIELD_Y_MAX);

src/main/java/frc/robot/subsystems/index/IndexerSubsystem.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.util.GetTargetFromPose;
 import java.util.function.DoubleSupplier;
 
 public class IndexerSubsystem extends SubsystemBase {
@@ -34,6 +35,11 @@ public Command runIndexer(DoubleSupplier flywheelRPS) {
           // the flywheel to have a smooth handoff
           double fRPS = flywheelRPS.getAsDouble() * 1.12 + 15;
           double sRPS = Math.min(fRPS * 1.5, 70);
+          if (GetTargetFromPose.BALLING.get()) {
+            // Feeding speed reduced for balling so balls dont hit each other mid air
+            fRPS /= 2;
+            sRPS /= 2;
+          }
           feeder.setVelocity(fRPS);
           spindexerSubsystem.setVelocity(sRPS);
         },

src/main/java/frc/robot/util/GetTargetFromPose.java

@@ -1,24 +1,54 @@
 package frc.robot.util;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain;
+import frc.robot.util.tuning.NtTunableBoolean;
 
 public class GetTargetFromPose {
-  private static Translation2d pointLeftFieldTop = new Translation2d(2, 6);
-  private static Translation2d pointLeftFieldBottom = new Translation2d(2, 2);
-  private static Translation2d pointRightFieldTop = new Translation2d(14, 6);
-  private static Translation2d pointRightFieldBottom = new Translation2d(14, 2);
+  private static AprilTagFieldLayout aprilTagFieldLayout =
+      AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+  // Robot with bumpers is 36.875 inches by 30.750 inches
+  private static Transform2d robotOffsetFromTag =
+      new Transform2d(
+          new Translation2d(Units.inchesToMeters(30.750 / 2), 0), Rotation2d.fromDegrees(180));
+  private static Pose2d redHub =
+      aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(robotOffsetFromTag);
+  private static Pose2d blueHub =
+      aprilTagFieldLayout.getTagPose(26).get().toPose2d().plus(robotOffsetFromTag);
 
-  private static double fieldLength = Units.inchesToMeters(651.2);
-  private static double fieldWidth = Units.inchesToMeters(317.7);
-  private static double allianceLineX = Units.inchesToMeters(158.6);
-  private static double robotOffset = Units.inchesToMeters(15);
+  private static final Translation2d pointLeftFieldTop = new Translation2d(2, 6);
+  private static final Translation2d pointLeftFieldBottom = new Translation2d(2, 2);
+  private static final Translation2d pointRightFieldTop = new Translation2d(14, 6);
+  private static final Translation2d pointRightFieldBottom = new Translation2d(14, 2);
+
+  private static final double fieldLength = Units.inchesToMeters(651.2);
+  private static final double fieldWidth = Units.inchesToMeters(317.7);
+  private static final double allianceLineX = Units.inchesToMeters(158.6);
+  private static final double robotOffset = Units.inchesToMeters(15);
+
+  // BALLING INSTRUCTIONS
+  // ONLY USE THIS FOR A BASKETBALL HOOP. If you want to score into a cardboard box dont use this
+  // instead just zero infront of the box and the robot will score normally into the box
+  // Switch to red alliance (I did this from behind this the hoop if you are doing it from the front
+  // use blue) then go into advantage scope to turn on "Balling". After that go to the
+  // center grey line of the court and then line up the bumper gap with the gray line and the turret
+  // side of the robot on the edge of the thick black line
+  public static NtTunableBoolean BALLING = new NtTunableBoolean("/Balling", false);
+  private static Translation2d BALLING_POSE = new Translation2d(-0.9779, -0.2286);
+  private static Pose2d RIGHT_BASKETBALL_COURT_CORNER = new Pose2d(0, 0, new Rotation2d());
 
   public static Translation2d getTargetLocation(CommandSwerveDrivetrain drivetrain) {
+    if (BALLING.get()) {
+      return BALLING_POSE;
+    }
     if (AllianceUtils.isBlue()) {
       if (drivetrain.getState().Pose.getX() <= allianceLineX + robotOffset) {
         return AllianceUtils.getHubTranslation2d();
@@ -41,6 +71,9 @@ public static Translation2d getTargetLocation(CommandSwerveDrivetrain drivetrain
   }
 
   public static Translation2d getTargetLocation(Pose2d pose) {
+    if (BALLING.get()) {
+      return BALLING_POSE;
+    }
     if (AllianceUtils.isBlue()) {
       if (pose.getX() <= allianceLineX + robotOffset) {
         return AllianceUtils.getHubTranslation2d();
@@ -66,6 +99,8 @@ public static Trigger autoShoot(CommandSwerveDrivetrain drivetrain) {
     return new Trigger(
         () -> {
           if (DriverStation.isAutonomousEnabled()) return false;
+          if (BALLING.get()) return false;
+
           var shiftInfo = HubShiftUtil.getShiftedShiftInfo();
 
           boolean pastAllianceLine =
@@ -84,4 +119,9 @@ public static Trigger autoShoot(CommandSwerveDrivetrain drivetrain) {
           return false;
         });
   }
+
+  public static Pose2d getRestPose() {
+    if (BALLING.get()) return RIGHT_BASKETBALL_COURT_CORNER;
+    return AllianceUtils.isRed() ? redHub : blueHub;
+  }
 }

src/main/java/frc/robot/util/tuning/LauncherConstants.java

@@ -11,6 +11,7 @@
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StructPublisher;
 import frc.robot.util.AllianceUtils;
+import frc.robot.util.GetTargetFromPose;
 import frc.robot.util.robotType.RobotType;
 
 public class LauncherConstants {
@@ -25,6 +26,8 @@ public class LauncherConstants {
       table.getStructTopic("Turret Pose", Pose2d.struct).publish();
   private static final DoublePublisher turretToHubDistance =
       table.getDoubleTopic("Turret to hub distance").publish();
+  private static final DoublePublisher turretToTargetDistance =
+      table.getDoubleTopic("Turret to target distance").publish();
 
   private static double minTime = Double.POSITIVE_INFINITY;
   private static double maxTime = Double.NEGATIVE_INFINITY;
@@ -75,10 +78,24 @@ public String toString() {
     new LauncherDistanceDataPoint(8, 9, 90, 1.53)
   };
 
+  // These "work" but if you ever want to do this again I reccommend redoing the data map and add
+  // more points to shoot closer to the hoop
+  public static final LauncherDistanceDataPoint[] ballingDistanceData = {
+    new LauncherDistanceDataPoint(5.720, 5.7, 82, 1),
+    new LauncherDistanceDataPoint(4.354, 5, 70, 1),
+    new LauncherDistanceDataPoint(3.119, 3.7, 63, 1),
+    new LauncherDistanceDataPoint(2.319, 2.8, 60, 1)
+  };
+
   private static final InterpolatingDoubleTreeMap flywheelMap = new InterpolatingDoubleTreeMap();
   private static final InterpolatingDoubleTreeMap hoodMap = new InterpolatingDoubleTreeMap();
   private static final InterpolatingDoubleTreeMap timeMap = new InterpolatingDoubleTreeMap();
 
+  private static final InterpolatingDoubleTreeMap flywheelMapBalling =
+      new InterpolatingDoubleTreeMap();
+  private static final InterpolatingDoubleTreeMap hoodMapBalling = new InterpolatingDoubleTreeMap();
+  private static final InterpolatingDoubleTreeMap timeMapBalling = new InterpolatingDoubleTreeMap();
+
   static {
     LauncherDistanceDataPoint[] distanceData =
         RobotType.isAlpha() ? alphaDistanceData : compDistanceData;
@@ -89,6 +106,24 @@ public String toString() {
       maxTime = Math.max(maxTime, point.time);
       minTime = Math.min(minTime, point.time);
     }
+
+    for (var point : ballingDistanceData) {
+      flywheelMapBalling.put(point.distance, point.flywheelPower);
+      hoodMapBalling.put(point.distance, point.hoodAngle);
+      timeMapBalling.put(point.distance, point.time);
+    }
+  }
+
+  private static InterpolatingDoubleTreeMap activeFlywheelMap() {
+    return GetTargetFromPose.BALLING.get() ? flywheelMapBalling : flywheelMap;
+  }
+
+  private static InterpolatingDoubleTreeMap activeHoodMap() {
+    return GetTargetFromPose.BALLING.get() ? hoodMapBalling : hoodMap;
+  }
+
+  private static InterpolatingDoubleTreeMap activeTimeMap() {
+    return GetTargetFromPose.BALLING.get() ? timeMapBalling : timeMap;
   }
 
   // public static void update(Pose2d robot, CommandSwerveDrivetrain driveTrain) {
@@ -107,7 +142,7 @@ public String toString() {
   // }
 
   public static double getFlywheelSpeedFromDistance(double distance) {
-    return flywheelMap.get(distance);
+    return activeFlywheelMap().get(distance);
   }
 
   public static Translation2d launcherFromRobot(Pose2d robot) {
@@ -119,6 +154,8 @@ public static void UpdateNT(Pose2d robot) {
     turretPose.set(result);
     distToHub = AllianceUtils.getHubTranslation2d().minus(result.getTranslation()).getNorm();
     turretToHubDistance.set(distToHub);
+    turretToTargetDistance.set(
+        GetTargetFromPose.getTargetLocation(result).minus(result.getTranslation()).getNorm());
   }
 
   public static double getFlywheelSpeedFromPose2d(Translation2d target, Pose2d robot) {
@@ -138,7 +175,7 @@ public static double distToHub() {
   }
 
   public static double getHoodAngleFromDistance(double distance) {
-    return hoodMap.get(distance /*+ distanceOffset*/);
+    return activeHoodMap().get(distance /*+ distanceOffset*/);
   }
 
   public static double getHoodAngleFromPose2d(Translation2d target, Pose2d robot) {
@@ -147,7 +184,7 @@ public static double getHoodAngleFromPose2d(Translation2d target, Pose2d robot)
   }
 
   public static double getTimeFromDistance(double distance) {
-    return timeMap.get(distance /*+ distanceOffset*/);
+    return activeTimeMap().get(distance /*+ distanceOffset*/);
   }
 
   public static double minTimeOfFlight() {