diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d487b9c..a763ab7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 trenches = Set.of( + getTagPose(7), + getTagPose(12), + getTagPose(23), + getTagPose(28) + ); + + private static Pose2d getTagPose(int id) { + return layout.getTagPose(id).orElseThrow().toPose2d(); + } } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 826c26d..f911b86 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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() { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index eb9972f..5417c25 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/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,9 +13,11 @@ 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; @@ -22,6 +25,7 @@ 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 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 diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 580bfdb..bcc0216 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/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;