Add shooter default command that lowers hood under trench

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

@@ -13,8 +13,11 @@
 
 package frc.robot;
 
+import java.util.Set;
+
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.RobotBase;
@@ -43,6 +46,17 @@ public static class DriveConstants {
 
     public static class FieldConstants {
         public static final AprilTagFieldLayout layout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+
+        public static final Set<Pose2d> trenches = Set.of(
+            getTagPose(7),
+            getTagPose(12),
+            getTagPose(23),
+            getTagPose(28)
+        );
+
+        private static Pose2d getTagPose(int id) {
+            return layout.getTagPose(id).orElseThrow().toPose2d();
+        }
     }
 
     /**

src/main/java/frc/robot/RobotContainer.java

@@ -323,6 +323,9 @@ private void configureBindings() {
 
         // When the hopper isnt shooting, set it to run its idle velocity.
         hopper_.setDefaultCommand(hopper_.idleScrambler());
+
+        // When the shooter isnt shooting, get it ready to shoot.
+        shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose));
     }
 
     private void configureDriveBindings() {

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems.shooter;
 
 import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.RPM;
 import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
@@ -12,16 +13,19 @@
 
 import org.littletonrobotics.junction.Logger;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
+import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.Mode;
 import frc.robot.subsystems.hopper.Hopper;
 import frc.robot.util.MapleSimUtil;
@@ -98,31 +102,11 @@ public Current getShooterCurrent() {
             .plus(shooterInputs.shooter4Current);
     }
 
-    public Command runToVelocityCmd(AngularVelocity vel) {
-        return runOnce(() -> setShooterVelocity(vel))
-            .andThen(Commands.waitUntil(this::isShooterReady)).withName("Set Shooter Velocity");
-    }
-
-    public Command stopCmd() {
-        return runOnce(() -> stopShooter())
-            .andThen(Commands.waitUntil(this::isShooterReady)).withName("Stop Shooter");
-    }
-
-    public Command runVoltageCmd(Voltage vol) {
-        return runOnce(() -> setShooterVoltage(vol)).withName("Set Shooter Voltage");
-    }
-
-    // Hood Methods
     private void setHoodAngle(Angle pos) {
         hoodTarget = pos;
         hoodIO.gotoAngle(pos);
     }
 
-    public Command hoodToPosCmd(Angle pos) {
-        return runOnce(() -> setHoodAngle(pos)).withName("Set Hood Position");
-    }
-
-    // Both
     private void setSetpoints(AngularVelocity vel, Angle pos) {
         shooterTarget = vel;
         setShooterVelocity(shooterTarget);
@@ -141,10 +125,48 @@ public Command shootCmd(Hopper hopper) {
         );
     }
 
+    /**
+     * The command that the shooter can run whenever its not shooting to manage
+     * things like going to different hood angles to get ready to shoot,
+     * or lowering the hood under the trench.
+     * @return A command that does so.
+     */
+    public Command awaitShooting(Supplier<Pose2d> robotPose) {
+        return runDynamicSetpoints(() -> RadiansPerSecond.zero(), () -> {
+            Pose2d pose = robotPose.get();
+            Pose2d nearestTrench = pose.nearest(FieldConstants.trenches);
+            Distance nearestDistance = Meters.of(pose.getTranslation().getDistance(nearestTrench.getTranslation()));
+
+            if (nearestDistance.lte(ShooterConstants.allowedTrenchDistance)) {
+                return Degrees.zero();
+            }
+
+            return Degrees.of(45); // TODO: replace this with whatever determines shooter angle
+        });
+    }
+
     public Command runToSetpointsCmd(AngularVelocity vel, Angle pos) {
         return runOnce(() -> setSetpoints(vel, pos)).andThen(Commands.waitUntil(this::isShooterReady));
     }
 
+    public Command runToVelocityCmd(AngularVelocity vel) {
+        return runOnce(() -> setShooterVelocity(vel))
+            .andThen(Commands.waitUntil(this::isShooterReady)).withName("Set Shooter Velocity");
+    }
+
+    public Command stopCmd() {
+        return runOnce(() -> stopShooter())
+            .andThen(Commands.waitUntil(this::isShooterReady)).withName("Stop Shooter");
+    }
+
+    public Command runVoltageCmd(Voltage vol) {
+        return runOnce(() -> setShooterVoltage(vol)).withName("Set Shooter Voltage");
+    }
+
+    public Command hoodToPosCmd(Angle pos) {
+        return runOnce(() -> setHoodAngle(pos)).withName("Set Hood Position");
+    }
+
     /**
      * Runs supplied setpoints until the command ends, then stops.
      * @param vel

src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java

@@ -1,9 +1,11 @@
 package frc.robot.subsystems.shooter;
 
+import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 import static edu.wpi.first.units.Units.Seconds;
 
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.Time;
 
 public class ShooterConstants {
@@ -19,6 +21,8 @@ public class ShooterConstants {
 
     public static final AngularVelocity shooterTolerance = RotationsPerSecond.of(1.0);
 
+    public static final Distance allowedTrenchDistance = Meters.of(1.0);
+
     public class PID {
         // shooter
         public static final double shooterkP = 0.0;