From 480eb864d4c9ac35a47efb5918e10559dc410ad3 Mon Sep 17 00:00:00 2001 From: Michael Date: Mon, 16 Feb 2026 20:47:42 -0800 Subject: [PATCH 01/48] i would try it if i had an xbox crontroller but for now it might work --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 82 +++++++++++++++---- .../frc/robot/subsystems/drive/Drive.java | 17 +++- .../frc/robot/subsystems/shooter/Shooter.java | 10 +-- .../subsystems/shooter/ShooterConstants.java | 2 + 5 files changed, 85 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a763ab79..2f65d697 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -39,6 +39,7 @@ public final class Constants { private static final RobotType robotType = RobotType.COMPETITION; public static final boolean spawnLessFuelInSim = true; + public static final boolean shootOnMove = true; public static class DriveConstants { public static final double slowModeJoystickMultiplier = 0.4; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5aaae067..86f0b72d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -324,28 +324,76 @@ private void configureBindings() { // While the left trigger is held, we will run the intake. If the intake is stowed, it will also deploy it. gamepad_.leftTrigger().whileTrue(intake_.intakeSequence()); - // While the right trigger is held, we will shoot into the hub. - gamepad_.rightTrigger().whileTrue(Commands.parallel(DriveCommands.joystickDriveAtAngle(drivebase_, - () -> 0, - () -> 0, - () -> { - Translation2d hub = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueHubPose - : ShooterConstants.Positions.redHubPose; - - var hubTranslation = hub.minus(drivebase_.getPose().getTranslation()); - var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); - - return rotation; - }), - shooter_.shooterSetpointSupplier(() -> drivebase_.getPose(), hopper_))); - // 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)); + + if(Constants.shootOnMove) { + Translation2d target = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueHubPose + : ShooterConstants.Positions.redHubPose; + + Translation2d virtualTarget = + target.plus( + new Translation2d( + MetersPerSecond.of(drivebase_.getChassisSpeeds().vxMetersPerSecond).times(ShooterConstants.hangTimeOnShot), + MetersPerSecond.of(drivebase_.getChassisSpeeds().vyMetersPerSecond).times(ShooterConstants.hangTimeOnShot) + ) + ); + // while in alliance zone, point drivebase at virtual target, but still allow translational driving + + drivebase_.whenRobotIsInAllianceZone(DriverStation.getAlliance().get()).whileTrue( + DriveCommands.joystickDriveAtAngle( + drivebase_, + () -> gamepad_.getLeftY(), + () -> gamepad_.getLeftX(), + () -> { + var hubTranslation = virtualTarget.minus(drivebase_.getPose().getTranslation()); + var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); + + return rotation; + })); + + // While the right trigger is held, we will shoot into the hub. + // i wish i knew if this shoot command would interrrupt the other aim scommand, so this is kind of a "just in case", but if it is unnesecary it will be removed. + gamepad_.rightTrigger().whileTrue( + Commands.parallel( + DriveCommands.joystickDriveAtAngle( + drivebase_, + () -> gamepad_.getLeftY(), + () -> gamepad_.getLeftX(), + () -> { + var hubTranslation = virtualTarget.minus(drivebase_.getPose().getTranslation()); + var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); + + return rotation; + }), + shooter_.shooterSetpointSupplier(() -> virtualTarget.minus(drivebase_.getPose().getTranslation()), hopper_)) + ); + + + } else { + + Translation2d target = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueHubPose + : ShooterConstants.Positions.redHubPose; + // While the right trigger is held, we will shoot into the hub. + gamepad_.rightTrigger().whileTrue(Commands.parallel(DriveCommands.joystickDriveAtAngle(drivebase_, + () -> 0, + () -> 0, + () -> { + var hubTranslation = target.minus(drivebase_.getPose().getTranslation()); + var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); + + return rotation; + }), + shooter_.shooterSetpointSupplier(() -> target.minus(drivebase_.getPose().getTranslation()), hopper_))); + + } } private void configureDriveBindings() { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 7737b458..57534145 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -13,6 +13,7 @@ package frc.robot.subsystems.drive; +import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; @@ -59,6 +60,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; @@ -219,10 +221,8 @@ public void periodic() { for (var module : modules) { module.stop(); } - } - // Log empty setpoint states when disabled - if (DriverStation.isDisabled()) { + // Log empty setpoint states when disabled Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } @@ -472,6 +472,17 @@ public double getMaxAngularSpeedRadPerSec() { public RobotConfig getPathplannerConfig() { return PP_CONFIG; } + + public Trigger whenRobotIsInAllianceZone(Alliance alliance) { + return new Trigger(() -> { + Pose2d pose = getPose(); + if (alliance == Alliance.Blue) { + return pose.getMeasureX().lt(Inches.of(156.0)); + } else { + return pose.getMeasureX().gt(Inches.of(651.22 - 156.0)); + } + }); + } /** Returns an array of module translations. */ public Translation2d[] getModuleTranslations() { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 7fe866a0..76bff45a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -194,21 +194,17 @@ public Command hoodToPosCmd(Angle pos) { * * */ - public Command shooterSetpointSupplier(Supplier pose, Hopper hopper) { + public Command shooterSetpointSupplier(Supplier dist, Hopper hopper) { return Commands.parallel( defer(() -> { // constructing - Translation2d hubTranslation = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueHubPose - : ShooterConstants.Positions.redHubPose; ShooterConstants.Positions.initMap(); return runDynamicSetpoints(() -> { - Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); + Distance distanceToHub = Meters.of(Math.sqrt(dist.get().getX() * dist.get().getX() + dist.get().getY() * dist.get().getY())); var vel = ShooterConstants.Positions.distMap.get(distanceToHub.baseUnitMagnitude()); return RotationsPerSecond.of(vel); @@ -216,7 +212,7 @@ public Command shooterSetpointSupplier(Supplier pose, Hopper hopper) { () -> { // periodic - Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); + Distance distanceToHub = Meters.of(Math.sqrt(dist.get().getX() * dist.get().getX() + dist.get().getY() * dist.get().getY())); switch(HubDistance.fromDistance(distanceToHub)) { case LOW: diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 11137a18..ed6ab1e7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -25,6 +25,8 @@ public class ShooterConstants { public static final Distance allowedTrenchDistance = Meters.of(1.0); + public static final Time hangTimeOnShot = Seconds.of(7/4.5); + public class PID { // shooter public static final double shooterkP = 0.5; From efc771346feb7d22aaf224a01d28ab09395ccfd1 Mon Sep 17 00:00:00 2001 From: Michael Date: Mon, 16 Feb 2026 21:32:09 -0800 Subject: [PATCH 02/48] its being rlly wierd but ill fix it later it does seem to kinda work tho --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 55 ++++++++++++--------- 2 files changed, 33 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2f65d697..6c8363bb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -36,7 +36,7 @@ public final class Constants { // Sets the currently running robot. Change to SIMBOT when running the // desktop physics simulation so AdvantageKit runs in SIM mode instead of // falling back to REPLAY. - private static final RobotType robotType = RobotType.COMPETITION; + private static final RobotType robotType = RobotType.SIMBOT; public static final boolean spawnLessFuelInSim = true; public static final boolean shootOnMove = true; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 86f0b72d..3169e8fd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import com.ctre.phoenix6.CANBus; +import com.fasterxml.jackson.core.util.BufferRecycler.Gettable; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -310,6 +311,27 @@ public RobotContainer() { configureDriveBindings(); } + private Translation2d getTarget(){ + Translation2d target = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueHubPose + : ShooterConstants.Positions.redHubPose; + return target; + } + + // ONLY USE IN CONFIGURE BINDINGS + private Translation2d getVirtualTarget() { + Translation2d virtualTarget = + getTarget().minus( + new Translation2d( + MetersPerSecond.of(drivebase_.getChassisSpeeds().vxMetersPerSecond).times(ShooterConstants.hangTimeOnShot), + MetersPerSecond.of(drivebase_.getChassisSpeeds().vyMetersPerSecond).times(ShooterConstants.hangTimeOnShot) + ) + ); + + return virtualTarget; + } + // Bind robot actions to commands here. private void configureBindings() { // Manually deploying and undeploying the intake. @@ -331,27 +353,16 @@ private void configureBindings() { shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose)); if(Constants.shootOnMove) { - Translation2d target = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueHubPose - : ShooterConstants.Positions.redHubPose; - - Translation2d virtualTarget = - target.plus( - new Translation2d( - MetersPerSecond.of(drivebase_.getChassisSpeeds().vxMetersPerSecond).times(ShooterConstants.hangTimeOnShot), - MetersPerSecond.of(drivebase_.getChassisSpeeds().vyMetersPerSecond).times(ShooterConstants.hangTimeOnShot) - ) - ); + // while in alliance zone, point drivebase at virtual target, but still allow translational driving - drivebase_.whenRobotIsInAllianceZone(DriverStation.getAlliance().get()).whileTrue( + drivebase_.whenRobotIsInAllianceZone(DriverStation.getAlliance().isPresent() ? DriverStation.getAlliance().get() : Alliance.Blue).whileTrue( DriveCommands.joystickDriveAtAngle( drivebase_, () -> gamepad_.getLeftY(), () -> gamepad_.getLeftX(), () -> { - var hubTranslation = virtualTarget.minus(drivebase_.getPose().getTranslation()); + var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); return rotation; @@ -359,39 +370,37 @@ private void configureBindings() { // While the right trigger is held, we will shoot into the hub. // i wish i knew if this shoot command would interrrupt the other aim scommand, so this is kind of a "just in case", but if it is unnesecary it will be removed. - gamepad_.rightTrigger().whileTrue( + //CHANGE BACK TO RIGHT TRIGGER! + gamepad_.a().whileTrue( Commands.parallel( DriveCommands.joystickDriveAtAngle( drivebase_, () -> gamepad_.getLeftY(), () -> gamepad_.getLeftX(), () -> { - var hubTranslation = virtualTarget.minus(drivebase_.getPose().getTranslation()); + var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); return rotation; }), - shooter_.shooterSetpointSupplier(() -> virtualTarget.minus(drivebase_.getPose().getTranslation()), hopper_)) + shooter_.shooterSetpointSupplier(() -> getVirtualTarget().minus(drivebase_.getPose().getTranslation()), hopper_)) ); } else { - Translation2d target = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueHubPose - : ShooterConstants.Positions.redHubPose; + // While the right trigger is held, we will shoot into the hub. gamepad_.rightTrigger().whileTrue(Commands.parallel(DriveCommands.joystickDriveAtAngle(drivebase_, () -> 0, () -> 0, () -> { - var hubTranslation = target.minus(drivebase_.getPose().getTranslation()); + var hubTranslation = getTarget().minus(drivebase_.getPose().getTranslation()); var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); return rotation; }), - shooter_.shooterSetpointSupplier(() -> target.minus(drivebase_.getPose().getTranslation()), hopper_))); + shooter_.shooterSetpointSupplier(() -> getTarget().minus(drivebase_.getPose().getTranslation()), hopper_))); } } From dd7cd94d615446501d792d935e4c542dd3ca15a7 Mon Sep 17 00:00:00 2001 From: Michael Date: Tue, 17 Feb 2026 16:24:01 -0800 Subject: [PATCH 03/48] kk I got it working just it needs some tuning on the hang time --- src/main/java/frc/robot/RobotContainer.java | 32 ++++++++++++------- .../frc/robot/subsystems/drive/Drive.java | 9 ++---- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3169e8fd..04993b32 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import java.util.Arrays; +import java.util.logging.Logger; import org.ironmaple.simulation.drivesims.COTS; import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; @@ -29,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Mode; import frc.robot.Constants.RobotType; @@ -324,8 +326,8 @@ private Translation2d getVirtualTarget() { Translation2d virtualTarget = getTarget().minus( new Translation2d( - MetersPerSecond.of(drivebase_.getChassisSpeeds().vxMetersPerSecond).times(ShooterConstants.hangTimeOnShot), - MetersPerSecond.of(drivebase_.getChassisSpeeds().vyMetersPerSecond).times(ShooterConstants.hangTimeOnShot) + MetersPerSecond.of(drivebase_.getFieldChassisSpeeds().vxMetersPerSecond).times(ShooterConstants.hangTimeOnShot), + MetersPerSecond.of(drivebase_.getFieldChassisSpeeds().vyMetersPerSecond).times(ShooterConstants.hangTimeOnShot) ) ); @@ -356,27 +358,33 @@ private void configureBindings() { // while in alliance zone, point drivebase at virtual target, but still allow translational driving - drivebase_.whenRobotIsInAllianceZone(DriverStation.getAlliance().isPresent() ? DriverStation.getAlliance().get() : Alliance.Blue).whileTrue( - DriveCommands.joystickDriveAtAngle( + new Trigger(() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? drivebase_.getPose().getMeasureX().lt(Inches.of(156.0)) : drivebase_.getPose().getMeasureX().gt(Inches.of(651.22 - 156.0))).whileTrue( + Commands.parallel( + DriveCommands.joystickDriveAtAngle( drivebase_, - () -> gamepad_.getLeftY(), - () -> gamepad_.getLeftX(), + () -> -gamepad_.getLeftY(), + () -> -gamepad_.getLeftX(), () -> { var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); return rotation; - })); + }), + Commands.run(() -> { + org.littletonrobotics.junction.Logger.recordOutput("Shooter/Target", getVirtualTarget()); + }) + ) + ); // While the right trigger is held, we will shoot into the hub. - // i wish i knew if this shoot command would interrrupt the other aim scommand, so this is kind of a "just in case", but if it is unnesecary it will be removed. + // i wish i knew if this shoot command would interrrupt the other aim command, so this is kind of a "just in case", but if it is unnesecary it will be removed. //CHANGE BACK TO RIGHT TRIGGER! gamepad_.a().whileTrue( Commands.parallel( DriveCommands.joystickDriveAtAngle( drivebase_, - () -> gamepad_.getLeftY(), - () -> gamepad_.getLeftX(), + () -> -gamepad_.getLeftY(), + () -> -gamepad_.getLeftX(), () -> { var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); @@ -386,12 +394,11 @@ private void configureBindings() { shooter_.shooterSetpointSupplier(() -> getVirtualTarget().minus(drivebase_.getPose().getTranslation()), hopper_)) ); - } else { // While the right trigger is held, we will shoot into the hub. - gamepad_.rightTrigger().whileTrue(Commands.parallel(DriveCommands.joystickDriveAtAngle(drivebase_, + gamepad_.a().whileTrue(Commands.parallel(DriveCommands.joystickDriveAtAngle(drivebase_, () -> 0, () -> 0, () -> { @@ -403,6 +410,7 @@ private void configureBindings() { shooter_.shooterSetpointSupplier(() -> getTarget().minus(drivebase_.getPose().getTranslation()), hopper_))); } + } private void configureDriveBindings() { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 57534145..04dec108 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -17,6 +17,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; +import java.util.Optional; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.BiFunction; @@ -473,14 +474,10 @@ public RobotConfig getPathplannerConfig() { return PP_CONFIG; } - public Trigger whenRobotIsInAllianceZone(Alliance alliance) { + public Trigger whenRobotIsInAllianceZone() { return new Trigger(() -> { Pose2d pose = getPose(); - if (alliance == Alliance.Blue) { - return pose.getMeasureX().lt(Inches.of(156.0)); - } else { - return pose.getMeasureX().gt(Inches.of(651.22 - 156.0)); - } + return pose.getMeasureX().lt(Inches.of(156.0)) || pose.getMeasureX().gt(Inches.of(651.22 - 156.0)); }); } From 4fa0eb453de309725815704a2730bccf652afc4f Mon Sep 17 00:00:00 2001 From: Michael Date: Tue, 17 Feb 2026 16:30:47 -0800 Subject: [PATCH 04/48] updated max speed and fixed things from sim --- src/main/java/frc/robot/Constants.java | 6 ++++-- src/main/java/frc/robot/RobotContainer.java | 12 ++++++------ 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6c8363bb..02cdc403 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -36,10 +36,12 @@ public final class Constants { // Sets the currently running robot. Change to SIMBOT when running the // desktop physics simulation so AdvantageKit runs in SIM mode instead of // falling back to REPLAY. - private static final RobotType robotType = RobotType.SIMBOT; + private static final RobotType robotType = RobotType.COMPETITION; public static final boolean spawnLessFuelInSim = true; - public static final boolean shootOnMove = true; + public static final boolean shootOnMove = false; // if true, we will allow the driver to shoot while moving, but with reduced max speed. if false, we will not allow the driver to shoot while moving. + public static final double shootOnMoveMaxSpeed = 1.0/3.0; + public static final double aimOnMoveMaxSpeed = 2.0/3.0; public static class DriveConstants { public static final double slowModeJoystickMultiplier = 0.4; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 04993b32..67d1b30d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -362,8 +362,8 @@ private void configureBindings() { Commands.parallel( DriveCommands.joystickDriveAtAngle( drivebase_, - () -> -gamepad_.getLeftY(), - () -> -gamepad_.getLeftX(), + () -> -gamepad_.getLeftY() * Constants.aimOnMoveMaxSpeed, + () -> -gamepad_.getLeftX() * Constants.aimOnMoveMaxSpeed, () -> { var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); @@ -379,12 +379,12 @@ private void configureBindings() { // While the right trigger is held, we will shoot into the hub. // i wish i knew if this shoot command would interrrupt the other aim command, so this is kind of a "just in case", but if it is unnesecary it will be removed. //CHANGE BACK TO RIGHT TRIGGER! - gamepad_.a().whileTrue( + gamepad_.rightTrigger().whileTrue( Commands.parallel( DriveCommands.joystickDriveAtAngle( drivebase_, - () -> -gamepad_.getLeftY(), - () -> -gamepad_.getLeftX(), + () -> -gamepad_.getLeftY() * Constants.shootOnMoveMaxSpeed, + () -> -gamepad_.getLeftX() * Constants.shootOnMoveMaxSpeed, () -> { var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); @@ -398,7 +398,7 @@ private void configureBindings() { // While the right trigger is held, we will shoot into the hub. - gamepad_.a().whileTrue(Commands.parallel(DriveCommands.joystickDriveAtAngle(drivebase_, + gamepad_.rightTrigger().whileTrue(Commands.parallel(DriveCommands.joystickDriveAtAngle(drivebase_, () -> 0, () -> 0, () -> { From 0cb7fedd0376837183120dcc3ab34912c2ee61a8 Mon Sep 17 00:00:00 2001 From: Michael Date: Tue, 17 Feb 2026 17:47:38 -0800 Subject: [PATCH 05/48] commented tracking mode out --- src/main/java/frc/robot/RobotContainer.java | 34 ++++++++++----------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d6d761d1..3e3d84ac 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -358,23 +358,23 @@ private void configureBindings() { // while in alliance zone, point drivebase at virtual target, but still allow translational driving - new Trigger(() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? drivebase_.getPose().getMeasureX().lt(Inches.of(156.0)) : drivebase_.getPose().getMeasureX().gt(Inches.of(651.22 - 156.0))).whileTrue( - Commands.parallel( - DriveCommands.joystickDriveAtAngle( - drivebase_, - () -> -gamepad_.getLeftY() * Constants.aimOnMoveMaxSpeed, - () -> -gamepad_.getLeftX() * Constants.aimOnMoveMaxSpeed, - () -> { - var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); - var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); - - return rotation; - }), - Commands.run(() -> { - org.littletonrobotics.junction.Logger.recordOutput("Shooter/Target", getVirtualTarget()); - }) - ) - ); + // new Trigger(() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? drivebase_.getPose().getMeasureX().lt(Inches.of(156.0)) : drivebase_.getPose().getMeasureX().gt(Inches.of(651.22 - 156.0))).whileTrue( + // Commands.parallel( + // DriveCommands.joystickDriveAtAngle( + // drivebase_, + // () -> -gamepad_.getLeftY() * Constants.aimOnMoveMaxSpeed, + // () -> -gamepad_.getLeftX() * Constants.aimOnMoveMaxSpeed, + // () -> { + // var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); + // var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); + + // return rotation; + // }), + // Commands.run(() -> { + // org.littletonrobotics.junction.Logger.recordOutput("Shooter/Target", getVirtualTarget()); + // }) + // ) + // ); // While the right trigger is held, we will shoot into the hub. // i wish i knew if this shoot command would interrrupt the other aim command, so this is kind of a "just in case", but if it is unnesecary it will be removed. From d002a06a286195e520e97a7de767cad82859ad12 Mon Sep 17 00:00:00 2001 From: Michael Date: Tue, 17 Feb 2026 17:51:41 -0800 Subject: [PATCH 06/48] added delay to shot so db can rotate correctly this should probs be in the other shoot command too --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3e3d84ac..1bd88b06 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -391,7 +391,7 @@ private void configureBindings() { return rotation; }), - shooter_.shooterSetpointSupplier(() -> getVirtualTarget().minus(drivebase_.getPose().getTranslation()), hopper_)) + Commands.sequence(Commands.waitTime(ShooterConstants.dbRotationDelay), shooter_.shooterSetpointSupplier(() -> getVirtualTarget().minus(drivebase_.getPose().getTranslation()), hopper_))) ); } else { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index ed6ab1e7..12d533bd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -26,6 +26,7 @@ public class ShooterConstants { public static final Distance allowedTrenchDistance = Meters.of(1.0); public static final Time hangTimeOnShot = Seconds.of(7/4.5); + public static final Time dbRotationDelay = Seconds.of(0.5); public class PID { // shooter From 2395d261986917af9f6c6fd6b0de9bc9c7362bda Mon Sep 17 00:00:00 2001 From: Michael Date: Wed, 18 Feb 2026 16:31:28 -0800 Subject: [PATCH 07/48] Commented stuff, got rid of base shoot command, updated hangtime constant just need to reverse the hopper after shooting is done and I will be happy --- src/main/java/frc/robot/Constants.java | 8 +- src/main/java/frc/robot/RobotContainer.java | 87 ++++++++----------- .../frc/robot/subsystems/hopper/Hopper.java | 3 + .../subsystems/shooter/ShooterConstants.java | 2 +- 4 files changed, 43 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 02cdc403..bdf1963e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -39,9 +39,11 @@ public final class Constants { private static final RobotType robotType = RobotType.COMPETITION; public static final boolean spawnLessFuelInSim = true; - public static final boolean shootOnMove = false; // if true, we will allow the driver to shoot while moving, but with reduced max speed. if false, we will not allow the driver to shoot while moving. - public static final double shootOnMoveMaxSpeed = 1.0/3.0; - public static final double aimOnMoveMaxSpeed = 2.0/3.0; + //public static final boolean shootOnMove = false; // if true, we will allow the driver to shoot while moving, but with reduced max speed. if false, we will not allow the driver to shoot while moving. + //change to 0 if it really doesnt work, bc the db velocity will go to 0 and the target will just be the hub + //but I think it will work so yeah trust me butch + public static final double shootOnMoveMaxSpeed = 1.0/3.0; + public static final double aimOnMoveMaxSpeed = 2.0/3.0; // obsolete rn, but change if we ever add a aim mode again public static class DriveConstants { public static final double slowModeJoystickMultiplier = 0.4; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1bd88b06..2166ec7d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -353,63 +353,44 @@ private void configureBindings() { // When the shooter isnt shooting, get it ready to shoot. shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose)); - - if(Constants.shootOnMove) { - - // while in alliance zone, point drivebase at virtual target, but still allow translational driving - - // new Trigger(() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? drivebase_.getPose().getMeasureX().lt(Inches.of(156.0)) : drivebase_.getPose().getMeasureX().gt(Inches.of(651.22 - 156.0))).whileTrue( - // Commands.parallel( - // DriveCommands.joystickDriveAtAngle( - // drivebase_, - // () -> -gamepad_.getLeftY() * Constants.aimOnMoveMaxSpeed, - // () -> -gamepad_.getLeftX() * Constants.aimOnMoveMaxSpeed, - // () -> { - // var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); - // var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); - - // return rotation; - // }), - // Commands.run(() -> { - // org.littletonrobotics.junction.Logger.recordOutput("Shooter/Target", getVirtualTarget()); - // }) - // ) - // ); - - // While the right trigger is held, we will shoot into the hub. - // i wish i knew if this shoot command would interrrupt the other aim command, so this is kind of a "just in case", but if it is unnesecary it will be removed. - //CHANGE BACK TO RIGHT TRIGGER! - gamepad_.rightTrigger().whileTrue( - Commands.parallel( - DriveCommands.joystickDriveAtAngle( - drivebase_, - () -> -gamepad_.getLeftY() * Constants.shootOnMoveMaxSpeed, - () -> -gamepad_.getLeftX() * Constants.shootOnMoveMaxSpeed, - () -> { - var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); - var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); - - return rotation; - }), - Commands.sequence(Commands.waitTime(ShooterConstants.dbRotationDelay), shooter_.shooterSetpointSupplier(() -> getVirtualTarget().minus(drivebase_.getPose().getTranslation()), hopper_))) - ); - - } else { - - - // While the right trigger is held, we will shoot into the hub. - gamepad_.rightTrigger().whileTrue(Commands.parallel(DriveCommands.joystickDriveAtAngle(drivebase_, - () -> 0, - () -> 0, + // while in alliance zone, point drivebase at virtual target, but still allow translational driving + //lowkey we are not gonna need this but like maybe so idk imma just keep it + + // new Trigger(() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? drivebase_.getPose().getMeasureX().lt(Inches.of(156.0)) : drivebase_.getPose().getMeasureX().gt(Inches.of(651.22 - 156.0))).whileTrue( + // Commands.parallel( + // DriveCommands.joystickDriveAtAngle( + // drivebase_, + // () -> -gamepad_.getLeftY() * Constants.aimOnMoveMaxSpeed, + // () -> -gamepad_.getLeftX() * Constants.aimOnMoveMaxSpeed, + // () -> { + // var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); + // var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); + + // return rotation; + // }), + // Commands.run(() -> { + // org.littletonrobotics.junction.Logger.recordOutput("Shooter/Target", getVirtualTarget()); + // }) + // ) + // ); + + // While the right trigger is held, we will shoot into the hub. + // i wish i knew if this shoot command would interrrupt the other aim command, so this is kind of a "just in case", but if it is unnesecary it will be removed. + //CHANGE BACK TO RIGHT TRIGGER! + gamepad_.rightTrigger().whileTrue( + Commands.parallel( + DriveCommands.joystickDriveAtAngle( + drivebase_, + () -> -gamepad_.getLeftY() * Constants.shootOnMoveMaxSpeed, + () -> -gamepad_.getLeftX() * Constants.shootOnMoveMaxSpeed, () -> { - var hubTranslation = getTarget().minus(drivebase_.getPose().getTranslation()); + var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); return rotation; - }), - shooter_.shooterSetpointSupplier(() -> getTarget().minus(drivebase_.getPose().getTranslation()), hopper_))); - - } + }), + Commands.sequence(Commands.waitTime(ShooterConstants.dbRotationDelay), shooter_.shooterSetpointSupplier(() -> getVirtualTarget().minus(drivebase_.getPose().getTranslation()), hopper_))) + ); //While the A button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it. gamepad_.a().whileTrue(intake_.ejectSequence()); diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index fd832b45..cc443ce6 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -123,6 +123,9 @@ public Command feed(AngularVelocity feeder, AngularVelocity scrambler) { * Runs the scrambler at its active speed, and the feeder. * @return */ + + // idk how to do this but like we should run the feeder motor backwards for about 0.5 secs to get arid of the balls stuck in there after we stop shooting + // theoretically this would improve consistency public Command forwardFeed() { return feed(HopperConstants.scramblerActiveVelocity, HopperConstants.feedingVelocity); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 12d533bd..6e2164a5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -25,7 +25,7 @@ public class ShooterConstants { public static final Distance allowedTrenchDistance = Meters.of(1.0); - public static final Time hangTimeOnShot = Seconds.of(7/4.5); + public static final Time hangTimeOnShot = Seconds.of(8/4.5);//number taken from video, 8 frames / 4.5 frames/sec seems to be about the right hang time. this should be tuned better later tho public static final Time dbRotationDelay = Seconds.of(0.5); public class PID { From 8b00a78e886730045e59a86ce7ce2fee95bc1dc7 Mon Sep 17 00:00:00 2001 From: Michael Date: Wed, 18 Feb 2026 17:07:38 -0800 Subject: [PATCH 08/48] saved some lines, added a bunch of comments (more things to think about) --- src/main/java/frc/robot/RobotContainer.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2166ec7d..6d58ed8e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,7 +18,6 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import com.ctre.phoenix6.CANBus; -import com.fasterxml.jackson.core.util.BufferRecycler.Gettable; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -314,24 +313,19 @@ public RobotContainer() { } private Translation2d getTarget(){ - Translation2d target = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? ShooterConstants.Positions.blueHubPose : ShooterConstants.Positions.redHubPose; - return target; } // ONLY USE IN CONFIGURE BINDINGS private Translation2d getVirtualTarget() { - Translation2d virtualTarget = - getTarget().minus( + return getTarget().minus( new Translation2d( MetersPerSecond.of(drivebase_.getFieldChassisSpeeds().vxMetersPerSecond).times(ShooterConstants.hangTimeOnShot), MetersPerSecond.of(drivebase_.getFieldChassisSpeeds().vyMetersPerSecond).times(ShooterConstants.hangTimeOnShot) ) ); - - return virtualTarget; } // Bind robot actions to commands here. @@ -377,6 +371,8 @@ private void configureBindings() { // While the right trigger is held, we will shoot into the hub. // i wish i knew if this shoot command would interrrupt the other aim command, so this is kind of a "just in case", but if it is unnesecary it will be removed. //CHANGE BACK TO RIGHT TRIGGER! + // we might want to limit the acceleration on this while shooting, but idk how to do that and hopefully it wont matter too much. + // just realized we could interrupt this with POV driving, but we would still be shooting, so we might want to create a block for that, but this too probably wont come up that much and i think i am not that numb-skulled to actually do this so idk gamepad_.rightTrigger().whileTrue( Commands.parallel( DriveCommands.joystickDriveAtAngle( @@ -389,7 +385,11 @@ private void configureBindings() { return rotation; }), - Commands.sequence(Commands.waitTime(ShooterConstants.dbRotationDelay), shooter_.shooterSetpointSupplier(() -> getVirtualTarget().minus(drivebase_.getPose().getTranslation()), hopper_))) + Commands.sequence( + Commands.waitTime(ShooterConstants.dbRotationDelay), + shooter_.shooterSetpointSupplier(() -> getVirtualTarget().minus(drivebase_.getPose().getTranslation()), hopper_) + ) + ) ); //While the A button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it. From 42915341451cd6f84b3b58c900377a0afb29e5df Mon Sep 17 00:00:00 2001 From: Michael Date: Wed, 18 Feb 2026 17:27:34 -0800 Subject: [PATCH 09/48] fixed hood --- src/main/java/frc/robot/RobotContainer.java | 3 ++- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 7 ++----- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6d58ed8e..71ae8fbe 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import java.util.Arrays; +import java.util.function.Supplier; import java.util.logging.Logger; import org.ironmaple.simulation.drivesims.COTS; @@ -346,7 +347,7 @@ private void configureBindings() { hopper_.setDefaultCommand(hopper_.idleScrambler()); // When the shooter isnt shooting, get it ready to shoot. - shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose)); + shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose, this::getVirtualTarget)); // while in alliance zone, point drivebase at virtual target, but still allow translational driving //lowkey we are not gonna need this but like maybe so idk imma just keep it diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 76bff45a..13de0a2f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -135,7 +135,7 @@ public Command shootCmd(Hopper hopper) { * or lowering the hood under the trench. * @return A command that does so. */ - public Command awaitShooting(Supplier robotPose) { + public Command awaitShooting(Supplier robotPose, Supplier targetPose) { return runDynamicSetpoints(() -> RadiansPerSecond.zero(), () -> { Pose2d pose = robotPose.get(); Pose2d nearestTrench = pose.nearest(FieldConstants.trenches); @@ -145,10 +145,7 @@ public Command awaitShooting(Supplier robotPose) { return Degrees.zero(); } - Translation2d hubTranslation = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueHubPose - : ShooterConstants.Positions.redHubPose; + Translation2d hubTranslation = targetPose.get(); Distance distanceToHub = Meters.of(pose.getTranslation().getDistance(hubTranslation)); From e3468af64e1abdecb6388cb4761e2918f6e6c211 Mon Sep 17 00:00:00 2001 From: Michael Date: Wed, 18 Feb 2026 17:42:54 -0800 Subject: [PATCH 10/48] updated constants to be more correct all the tested positions should be in the units library and not doubles, but im too lazy to change that rn --- .../robot/subsystems/shooter/ShooterConstants.java | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 6e2164a5..4c55deca 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -62,7 +62,7 @@ public class MotionMagic { } public class SoftwareLimits { - public static final double hoodMaxAngle = 0.0; + public static final double hoodMaxAngle = Positions.hoodHIGH; public static final double hoodMinAngle = 0.0; } @@ -70,6 +70,8 @@ public class Positions { public static final Translation2d blueHubPose = new Translation2d(4.5974,4.034536); public static final Translation2d redHubPose = new Translation2d(11.938,4.034536); + //why are these not all Distances/Angles? + // Hood Setpoints public static final double hoodLOW = 0; public static final double hoodMEDIUM = 1; @@ -88,6 +90,9 @@ public class Positions { public static final double dist8 = 8; public static final double dist9 = 9; + public static final double distLowMid = 3.5; + public static final double distMidHigh = 6.5; + // Tested Velocities public static final double low1 = 10; public static final double low2 = 20; @@ -116,9 +121,9 @@ public static void initMap() { } public enum HubDistance { - LOW(Meters.of(2)), // 0-2 m - MEDIUM(Meters.of(3)), // in - HIGH(Meters.of(4)); // 96+ in + LOW(Meters.of(Positions.distLowMid)), // 0-2 m + MEDIUM(Meters.of(Positions.distMidHigh)), // in + HIGH(Meters.of(Positions.distMidHigh)); // 96+ in private final Distance maxDistance; From e6a4200c1c2c8e29710335a0d32a31573489a81b Mon Sep 17 00:00:00 2001 From: Michael Date: Sat, 21 Feb 2026 09:49:16 -0800 Subject: [PATCH 11/48] took out comments --- src/main/java/frc/robot/RobotContainer.java | 18 ------------------ .../java/frc/robot/subsystems/drive/Drive.java | 7 ------- 2 files changed, 25 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 71ae8fbe..688487d7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -351,24 +351,6 @@ private void configureBindings() { // while in alliance zone, point drivebase at virtual target, but still allow translational driving //lowkey we are not gonna need this but like maybe so idk imma just keep it - // new Trigger(() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? drivebase_.getPose().getMeasureX().lt(Inches.of(156.0)) : drivebase_.getPose().getMeasureX().gt(Inches.of(651.22 - 156.0))).whileTrue( - // Commands.parallel( - // DriveCommands.joystickDriveAtAngle( - // drivebase_, - // () -> -gamepad_.getLeftY() * Constants.aimOnMoveMaxSpeed, - // () -> -gamepad_.getLeftX() * Constants.aimOnMoveMaxSpeed, - // () -> { - // var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); - // var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); - - // return rotation; - // }), - // Commands.run(() -> { - // org.littletonrobotics.junction.Logger.recordOutput("Shooter/Target", getVirtualTarget()); - // }) - // ) - // ); - // While the right trigger is held, we will shoot into the hub. // i wish i knew if this shoot command would interrrupt the other aim command, so this is kind of a "just in case", but if it is unnesecary it will be removed. //CHANGE BACK TO RIGHT TRIGGER! diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 04dec108..f5a95b28 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -473,13 +473,6 @@ public double getMaxAngularSpeedRadPerSec() { public RobotConfig getPathplannerConfig() { return PP_CONFIG; } - - public Trigger whenRobotIsInAllianceZone() { - return new Trigger(() -> { - Pose2d pose = getPose(); - return pose.getMeasureX().lt(Inches.of(156.0)) || pose.getMeasureX().gt(Inches.of(651.22 - 156.0)); - }); - } /** Returns an array of module translations. */ public Translation2d[] getModuleTranslations() { From 8bc4fa7e5d8433a51b5518bfd558821155c221da Mon Sep 17 00:00:00 2001 From: Michael Date: Mon, 23 Feb 2026 09:22:30 -0800 Subject: [PATCH 12/48] moved things around, same functionality --- src/main/java/frc/robot/RobotContainer.java | 38 +++---------------- .../robot/commands/drive/DriveCommands.java | 15 ++++++++ .../frc/robot/subsystems/drive/Drive.java | 18 +++++++-- .../frc/robot/subsystems/shooter/Shooter.java | 2 - 4 files changed, 34 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 688487d7..f2f1a733 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,8 +11,6 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import java.util.Arrays; -import java.util.function.Supplier; -import java.util.logging.Logger; import org.ironmaple.simulation.drivesims.COTS; import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; @@ -22,15 +20,11 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Mode; import frc.robot.Constants.RobotType; @@ -313,21 +307,8 @@ public RobotContainer() { configureDriveBindings(); } - private Translation2d getTarget(){ - return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueHubPose - : ShooterConstants.Positions.redHubPose; - } - // ONLY USE IN CONFIGURE BINDINGS - private Translation2d getVirtualTarget() { - return getTarget().minus( - new Translation2d( - MetersPerSecond.of(drivebase_.getFieldChassisSpeeds().vxMetersPerSecond).times(ShooterConstants.hangTimeOnShot), - MetersPerSecond.of(drivebase_.getFieldChassisSpeeds().vyMetersPerSecond).times(ShooterConstants.hangTimeOnShot) - ) - ); - } + // Bind robot actions to commands here. private void configureBindings() { @@ -347,7 +328,7 @@ private void configureBindings() { hopper_.setDefaultCommand(hopper_.idleScrambler()); // When the shooter isnt shooting, get it ready to shoot. - shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose, this::getVirtualTarget)); + shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose, drivebase_::getVirtualTarget)); // while in alliance zone, point drivebase at virtual target, but still allow translational driving //lowkey we are not gonna need this but like maybe so idk imma just keep it @@ -358,21 +339,12 @@ private void configureBindings() { // just realized we could interrupt this with POV driving, but we would still be shooting, so we might want to create a block for that, but this too probably wont come up that much and i think i am not that numb-skulled to actually do this so idk gamepad_.rightTrigger().whileTrue( Commands.parallel( - DriveCommands.joystickDriveAtAngle( - drivebase_, - () -> -gamepad_.getLeftY() * Constants.shootOnMoveMaxSpeed, - () -> -gamepad_.getLeftX() * Constants.shootOnMoveMaxSpeed, - () -> { - var hubTranslation = getVirtualTarget().minus(drivebase_.getPose().getTranslation()); - var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); - - return rotation; - }), + DriveCommands.pointAtShootingTarget(drivebase_, gamepad_, true), Commands.sequence( Commands.waitTime(ShooterConstants.dbRotationDelay), - shooter_.shooterSetpointSupplier(() -> getVirtualTarget().minus(drivebase_.getPose().getTranslation()), hopper_) + shooter_.shooterSetpointSupplier(() -> drivebase_.getVirtualTarget().minus(drivebase_.getPose().getTranslation()), hopper_) ) - ) + ) ); //While the A button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it. diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index db9befe2..a036d681 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -58,6 +58,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; @@ -238,6 +239,20 @@ public static Command joystickDriveAtAngle( // Reset PID controller when command starts .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } + + public static Command pointAtShootingTarget(Drive drive, CommandXboxController gamepad, boolean shootOnMove){ + var shootOnMoveSpeed = shootOnMove ? Constants.shootOnMoveMaxSpeed : 0.0 ; + return joystickDriveAtAngle( + drive, + () -> -gamepad.getLeftY() * shootOnMoveSpeed, + () -> -gamepad.getLeftX() * shootOnMoveSpeed, + () -> { + var hubTranslation = drive.getVirtualTarget().minus(drive.getPose().getTranslation()); + var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); + + return rotation; + }); + } /** * Measures the velocity feedforward constants for the drive motors. diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index f5a95b28..8ea29e72 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -12,12 +12,9 @@ // GNU General Public License for more details. package frc.robot.subsystems.drive; - -import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; -import java.util.Optional; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.BiFunction; @@ -61,11 +58,11 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; import frc.robot.generated.BetaTunerConstants; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.util.LocalADStarAK; public class Drive extends SubsystemBase { @@ -473,6 +470,19 @@ public double getMaxAngularSpeedRadPerSec() { public RobotConfig getPathplannerConfig() { return PP_CONFIG; } + + public Translation2d getVirtualTarget() { + return ( + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueHubPose + : ShooterConstants.Positions.redHubPose + ).minus( + new Translation2d( + MetersPerSecond.of(getFieldChassisSpeeds().vxMetersPerSecond).times(ShooterConstants.hangTimeOnShot), + MetersPerSecond.of(getFieldChassisSpeeds().vyMetersPerSecond).times(ShooterConstants.hangTimeOnShot) + ) + ); + } /** Returns an array of module translations. */ public Translation2d[] getModuleTranslations() { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 13de0a2f..abcc26e7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -22,8 +22,6 @@ 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.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; From ea6145b9303ef5d6907841ee1885a748f909ba01 Mon Sep 17 00:00:00 2001 From: Michael Date: Mon, 23 Feb 2026 09:29:51 -0800 Subject: [PATCH 13/48] moved more things around, pretty clean now :) should probably test to make sure setting the gp to null doesnt cause any problems, but I dont think it should --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 8 +------- src/main/java/frc/robot/commands/AutoCommands.java | 4 ++-- .../frc/robot/commands/drive/DriveCommands.java | 5 ++--- .../java/frc/robot/subsystems/shooter/Shooter.java | 14 ++++++++++---- 5 files changed, 16 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bdf1963e..d40ececa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -39,7 +39,7 @@ public final class Constants { private static final RobotType robotType = RobotType.COMPETITION; public static final boolean spawnLessFuelInSim = true; - //public static final boolean shootOnMove = false; // if true, we will allow the driver to shoot while moving, but with reduced max speed. if false, we will not allow the driver to shoot while moving. + public static final boolean shootOnMove = false; // if true, we will allow the driver to shoot while moving, but with reduced max speed. if false, we will not allow the driver to shoot while moving. //change to 0 if it really doesnt work, bc the db velocity will go to 0 and the target will just be the hub //but I think it will work so yeah trust me butch public static final double shootOnMoveMaxSpeed = 1.0/3.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f2f1a733..c12b6e57 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -338,13 +338,7 @@ private void configureBindings() { // we might want to limit the acceleration on this while shooting, but idk how to do that and hopefully it wont matter too much. // just realized we could interrupt this with POV driving, but we would still be shooting, so we might want to create a block for that, but this too probably wont come up that much and i think i am not that numb-skulled to actually do this so idk gamepad_.rightTrigger().whileTrue( - Commands.parallel( - DriveCommands.pointAtShootingTarget(drivebase_, gamepad_, true), - Commands.sequence( - Commands.waitTime(ShooterConstants.dbRotationDelay), - shooter_.shooterSetpointSupplier(() -> drivebase_.getVirtualTarget().minus(drivebase_.getPose().getTranslation()), hopper_) - ) - ) + shooter_.shootCmd(drivebase_, hopper_, gamepad_, Constants.shootOnMove) ); //While the A button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it. diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index 930f1f94..4c8be4d1 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -17,11 +17,11 @@ public static Command NeutralZoneTrenchToTrench(Drive drive, IntakeSubsystem int DriveCommands.initialFollowPathCommand(drive,"TopToBottom Trench").deadlineFor(intake.intakeSequence()), - shooter.shootCmd(hopper).withTimeout(3.8), + shooter.shootCmd(drive, hopper, null, false).withTimeout(3.8), DriveCommands.followPathCommand("BottomToTop").deadlineFor(intake.intakeSequence()), - shooter.shootCmd(hopper).withTimeout(2.5) + shooter.shootCmd(drive, hopper, null, false).withTimeout(2.5) ); } diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index a036d681..8bb9adb6 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -241,11 +241,10 @@ public static Command joystickDriveAtAngle( } public static Command pointAtShootingTarget(Drive drive, CommandXboxController gamepad, boolean shootOnMove){ - var shootOnMoveSpeed = shootOnMove ? Constants.shootOnMoveMaxSpeed : 0.0 ; return joystickDriveAtAngle( drive, - () -> -gamepad.getLeftY() * shootOnMoveSpeed, - () -> -gamepad.getLeftX() * shootOnMoveSpeed, + () -> shootOnMove ? -gamepad.getLeftY() * Constants.shootOnMoveMaxSpeed : 0.0, + () -> shootOnMove ? -gamepad.getLeftX() * Constants.shootOnMoveMaxSpeed : 0.0, () -> { var hubTranslation = drive.getVirtualTarget().minus(drive.getPose().getTranslation()); var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index abcc26e7..f985d6b2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -25,10 +25,13 @@ 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.button.CommandXboxController; 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.commands.drive.DriveCommands; +import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.hopper.Hopper; import frc.robot.subsystems.shooter.ShooterConstants.Positions.HubDistance; import frc.robot.util.MapleSimUtil; @@ -120,11 +123,14 @@ private void setSetpoints(AngularVelocity vel, Angle pos) { * Shoot balls from the shooter until the command ends. * @return */ - public Command shootCmd(Hopper hopper) { + public Command shootCmd(Drive drive, Hopper hopper, CommandXboxController gamepad, boolean shootOnMove) { return Commands.parallel( - runDynamicSetpoints(() -> RPM.of(5000), () -> Degrees.of(30)), - hopper.forwardFeed() - ); + DriveCommands.pointAtShootingTarget(drive, gamepad, true), + Commands.sequence( + Commands.waitTime(ShooterConstants.dbRotationDelay), + shooterSetpointSupplier(() -> drive.getVirtualTarget().minus(drive.getPose().getTranslation()), hopper) + ) + ); } /** From d284e2e2ad3038e0a171870a70149185d1cf1e9f Mon Sep 17 00:00:00 2001 From: Michael Date: Tue, 24 Feb 2026 14:00:36 -0800 Subject: [PATCH 14/48] fixed merge conflicts --- src/main/java/frc/robot/subsystems/shooter/ShooterIO.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 53a7314c..57926704 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -26,14 +26,10 @@ public static class ShooterIOInputs { public Voltage shooter3Voltage = Volts.zero(); public Current shooter3Current = Amps.zero(); public AngularVelocity shooter3Velocity = RadiansPerSecond.zero(); - -<<<<<<< michael-shooting-while-moving -======= + public Voltage shooter4Voltage = Volts.zero(); public Current shooter4Current = Amps.zero(); public AngularVelocity shooter4Velocity = RadiansPerSecond.zero(); - ->>>>>>> main public AngularVelocity wheelVelocity = RadiansPerSecond.zero(); } From 8edc55b087c2fdf706507eaa4d5538d4e0cad642 Mon Sep 17 00:00:00 2001 From: Michael Date: Tue, 24 Feb 2026 14:01:24 -0800 Subject: [PATCH 15/48] removed unused imports --- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/subsystems/shooter/Shooter.java | 1 - 2 files changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 071cc0a7..15b5b3b0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -49,7 +49,6 @@ import frc.robot.subsystems.shooter.HoodIOServo; import frc.robot.subsystems.shooter.HoodIOSim; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterIOTalonFX; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 8fd725e1..45ce7d9b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,7 +2,6 @@ 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; import static edu.wpi.first.units.Units.RotationsPerSecond; From d234beeee8ae9a2026add9e02ffe2696344ae9df Mon Sep 17 00:00:00 2001 From: Michael Date: Thu, 26 Feb 2026 10:19:15 -0800 Subject: [PATCH 16/48] fixing bad merge --- src/main/java/frc/robot/subsystems/drive/Drive.java | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 85074d4b..c3d02960 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -61,12 +61,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; -<<<<<<< michael-shooting-while-moving -import frc.robot.generated.BetaTunerConstants; import frc.robot.subsystems.shooter.ShooterConstants; -======= import frc.robot.generated.CompTunerConstants; ->>>>>>> main import frc.robot.util.LocalADStarAK; public class Drive extends SubsystemBase { From f44e3ecf94c74d8c76ddcbda8850546539be50fe Mon Sep 17 00:00:00 2001 From: Michael Date: Thu, 26 Feb 2026 10:25:25 -0800 Subject: [PATCH 17/48] made it so literally there is no added complexity when shooting not on move --- src/main/java/frc/robot/commands/drive/DriveCommands.java | 2 +- src/main/java/frc/robot/subsystems/drive/Drive.java | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 8bb9adb6..4bb1a534 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -246,7 +246,7 @@ public static Command pointAtShootingTarget(Drive drive, CommandXboxController g () -> shootOnMove ? -gamepad.getLeftY() * Constants.shootOnMoveMaxSpeed : 0.0, () -> shootOnMove ? -gamepad.getLeftX() * Constants.shootOnMoveMaxSpeed : 0.0, () -> { - var hubTranslation = drive.getVirtualTarget().minus(drive.getPose().getTranslation()); + var hubTranslation = (shootOnMove ? drive.getVirtualTarget() : drive.getHubTranslation()).minus(drive.getPose().getTranslation()); var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); return rotation; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index c3d02960..0b659e1f 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -471,12 +471,16 @@ public RobotConfig getPathplannerConfig() { return PP_CONFIG; } - public Translation2d getVirtualTarget() { + public Translation2d getHubTranslation(){ return ( DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? ShooterConstants.Positions.blueHubPose : ShooterConstants.Positions.redHubPose - ).minus( + ); + } + + public Translation2d getVirtualTarget() { + return getHubTranslation().minus( new Translation2d( MetersPerSecond.of(getFieldChassisSpeeds().vxMetersPerSecond).times(ShooterConstants.hangTimeOnShot), MetersPerSecond.of(getFieldChassisSpeeds().vyMetersPerSecond).times(ShooterConstants.hangTimeOnShot) From 07c6746562b8fee59cf1b50da9e87e5fb28bdfa6 Mon Sep 17 00:00:00 2001 From: Michael Date: Thu, 26 Feb 2026 15:05:43 -0800 Subject: [PATCH 18/48] added ferrying (NEEDS TESTING) --- src/main/java/frc/robot/Constants.java | 15 +++++++++++ .../robot/commands/drive/DriveCommands.java | 27 ++++++++++++++++++- .../frc/robot/subsystems/shooter/Shooter.java | 2 +- 3 files changed, 42 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fe3aa1a4..6c429312 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,11 +13,15 @@ package frc.robot; +import static edu.wpi.first.units.Units.Inches; + 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.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.RobotBase; @@ -47,6 +51,17 @@ public final class Constants { public static class DriveConstants { public static final double slowModeJoystickMultiplier = 0.4; + + public static final Distance fieldLength = Inches.of(651.22); + public static final Distance fieldWidth = Inches.of(317.69); + + public static final Distance allianceZoneBlue = Inches.of(156.61); + public static final Distance allianceZoneRed = fieldLength.minus(allianceZoneBlue); + + public static final Translation2d blueLeftFerryTarget = new Translation2d(allianceZoneBlue.div(2.0), fieldWidth.times(5.0/6.0)); + public static final Translation2d blueRightFerryTarget = new Translation2d(allianceZoneBlue.div(2.0), fieldWidth.times(1.0/6.0)); + public static final Translation2d redLeftFerryTarget = new Translation2d(fieldLength.minus(allianceZoneBlue.div(2.0)), fieldWidth.times(5.0/6.0)); + public static final Translation2d redRightFerryTarget = new Translation2d(fieldLength.minus(allianceZoneBlue.div(2.0)), fieldWidth.times(1.0/6.0)); } public static class FieldConstants { diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 4bb1a534..5ae1d2c3 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -60,6 +60,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; +import frc.robot.Constants.DriveConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; import frc.robot.subsystems.drive.Drive; @@ -240,7 +241,7 @@ public static Command joystickDriveAtAngle( .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } - public static Command pointAtShootingTarget(Drive drive, CommandXboxController gamepad, boolean shootOnMove){ + public static Command pointAtTarget(Drive drive, CommandXboxController gamepad, Supplier target, boolean shootOnMove){ return joystickDriveAtAngle( drive, () -> shootOnMove ? -gamepad.getLeftY() * Constants.shootOnMoveMaxSpeed : 0.0, @@ -253,6 +254,30 @@ public static Command pointAtShootingTarget(Drive drive, CommandXboxController g }); } + private static Translation2d getTarget(Drive drive) { + Translation2d target; + + boolean blueDS = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue; + + boolean robotInAllianceZone = blueDS ? + drive.getPose().getMeasureX().lt(DriveConstants.allianceZoneBlue): + drive.getPose().getMeasureX().gt(DriveConstants.allianceZoneRed); + + if(robotInAllianceZone){ + target = drive.getVirtualTarget(); + }else if(drive.getPose().getMeasureY().gt(DriveConstants.fieldWidth.div(2.0))){ + target = blueDS ? DriveConstants.blueLeftFerryTarget : DriveConstants.redLeftFerryTarget ; + }else{ + target = blueDS ? DriveConstants.blueRightFerryTarget : DriveConstants.redRightFerryTarget ; + } + + return target; + } + + public static Command pointAtShootingTarget(Drive drive, CommandXboxController gamepad, boolean shootOnMove){ + return pointAtTarget(drive, gamepad, () -> getTarget(drive), shootOnMove); + } + /** * Measures the velocity feedforward constants for the drive motors. * diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 45ce7d9b..98c720c7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -135,7 +135,7 @@ private void setSetpoints(AngularVelocity vel, Angle pos) { */ public Command shootCmd(Drive drive, Hopper hopper, CommandXboxController gamepad, boolean shootOnMove) { return Commands.parallel( - DriveCommands.pointAtShootingTarget(drive, gamepad, true), + DriveCommands.pointAtShootingTarget(drive, gamepad, shootOnMove), Commands.sequence( Commands.waitTime(ShooterConstants.dbRotationDelay), shooterSetpointSupplier(() -> drive.getVirtualTarget().minus(drive.getPose().getTranslation()), hopper) From 4096400e088759fa99ed025d91cd93a6df415ee4 Mon Sep 17 00:00:00 2001 From: Michael Date: Thu, 26 Feb 2026 15:06:30 -0800 Subject: [PATCH 19/48] removed unused imports --- src/main/java/frc/robot/RobotContainer.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 16794997..b302991c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -52,8 +52,6 @@ import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterIOTalonFX; -import frc.robot.subsystems.thriftyclimb.ThriftyClimb; -import frc.robot.subsystems.thriftyclimb.ThriftyClimbIO; import frc.robot.subsystems.vision.AprilTagVision; import frc.robot.subsystems.vision.CameraIO; import frc.robot.subsystems.vision.CameraIOLimelight4; @@ -70,7 +68,6 @@ public class RobotContainer { private IntakeSubsystem intake_; private Shooter shooter_; private Hopper hopper_; - private ThriftyClimb climb_; private CANBus roborioCANBus = new CANBus(); From 76d8d716623c87cc390e564467ce94463f3181c0 Mon Sep 17 00:00:00 2001 From: Michael Date: Fri, 27 Feb 2026 13:42:09 -0800 Subject: [PATCH 20/48] updated for more code reusability --- src/main/java/frc/robot/Constants.java | 9 +++++++++ .../robot/commands/drive/DriveCommands.java | 2 +- .../java/frc/robot/subsystems/drive/Drive.java | 17 ++++------------- .../java/frc/robot/util/VirtualTarget.java | 18 ++++++++++++++++++ 4 files changed, 32 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc/robot/util/VirtualTarget.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6c429312..027810d9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,7 +24,9 @@ import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.RobotBase; +import frc.robot.subsystems.shooter.ShooterConstants; /** * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running @@ -62,6 +64,13 @@ public static class DriveConstants { public static final Translation2d blueRightFerryTarget = new Translation2d(allianceZoneBlue.div(2.0), fieldWidth.times(1.0/6.0)); public static final Translation2d redLeftFerryTarget = new Translation2d(fieldLength.minus(allianceZoneBlue.div(2.0)), fieldWidth.times(5.0/6.0)); public static final Translation2d redRightFerryTarget = new Translation2d(fieldLength.minus(allianceZoneBlue.div(2.0)), fieldWidth.times(1.0/6.0)); + public static Translation2d getHubTranslation(Alliance alliance){ + return ( + alliance == Alliance.Blue + ? ShooterConstants.Positions.blueHubPose + : ShooterConstants.Positions.redHubPose + ); + } } public static class FieldConstants { diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 5ae1d2c3..ccdc4841 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -247,7 +247,7 @@ public static Command pointAtTarget(Drive drive, CommandXboxController gamepad, () -> shootOnMove ? -gamepad.getLeftY() * Constants.shootOnMoveMaxSpeed : 0.0, () -> shootOnMove ? -gamepad.getLeftX() * Constants.shootOnMoveMaxSpeed : 0.0, () -> { - var hubTranslation = (shootOnMove ? drive.getVirtualTarget() : drive.getHubTranslation()).minus(drive.getPose().getTranslation()); + var hubTranslation = (shootOnMove ? drive.getVirtualTarget() : DriveConstants.getHubTranslation(DriverStation.getAlliance().orElse(Alliance.Blue))).minus(drive.getPose().getTranslation()); var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); return rotation; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 0b659e1f..5f5d4092 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -60,10 +60,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; +import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Mode; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.generated.CompTunerConstants; import frc.robot.util.LocalADStarAK; +import frc.robot.util.VirtualTarget; public class Drive extends SubsystemBase { // These Constants should be the same for every drivebase, so just use the comp bot constants. @@ -471,21 +473,10 @@ public RobotConfig getPathplannerConfig() { return PP_CONFIG; } - public Translation2d getHubTranslation(){ - return ( - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueHubPose - : ShooterConstants.Positions.redHubPose - ); - } + public Translation2d getVirtualTarget() { - return getHubTranslation().minus( - new Translation2d( - MetersPerSecond.of(getFieldChassisSpeeds().vxMetersPerSecond).times(ShooterConstants.hangTimeOnShot), - MetersPerSecond.of(getFieldChassisSpeeds().vyMetersPerSecond).times(ShooterConstants.hangTimeOnShot) - ) - ); + return VirtualTarget.getVirtualTargetFromTarget(this, DriveConstants.getHubTranslation(DriverStation.getAlliance().orElse(Alliance.Blue)), ShooterConstants.hangTimeOnShot); } /** Returns an array of module translations. */ diff --git a/src/main/java/frc/robot/util/VirtualTarget.java b/src/main/java/frc/robot/util/VirtualTarget.java new file mode 100644 index 00000000..f53f315e --- /dev/null +++ b/src/main/java/frc/robot/util/VirtualTarget.java @@ -0,0 +1,18 @@ +package frc.robot.util; + +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Time; +import frc.robot.subsystems.drive.Drive; + +public class VirtualTarget { + public static Translation2d getVirtualTargetFromTarget(Drive drive, Translation2d target, Time hangTime){ + return target.minus( + new Translation2d( + MetersPerSecond.of(drive.getFieldChassisSpeeds().vxMetersPerSecond).times(hangTime), + MetersPerSecond.of(drive.getFieldChassisSpeeds().vyMetersPerSecond).times(hangTime) + ) + ); + } +} From 8711ff8439eea795e93b8d6f5400ec7c6fb371b2 Mon Sep 17 00:00:00 2001 From: Michael Date: Fri, 27 Feb 2026 13:52:58 -0800 Subject: [PATCH 21/48] kk it builds again wow these merges are selling --- .../frc/robot/subsystems/shooter/Shooter.java | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 8fdbd3c6..996dae40 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -200,6 +200,7 @@ public Command runVoltageCmd(Voltage vol) { public Command hoodToPosCmd(Angle pos) { return runOnce(() -> setHoodAngle(pos)).withName("Set Hood Position"); + } public Command runSetpoints(AngularVelocity vel, Angle pos) { return startEnd(() -> setSetpoints(vel, pos), this::stopShooter); } @@ -318,16 +319,16 @@ public Command hoodCalibration() { } - /** - * Shoot balls from the shooter until the command ends. - * @return - */ - public Command shootCmd(Hopper hopper) { - return Commands.parallel( - runDynamicSetpoints(() -> RPM.of(5000), () -> Degrees.of(30)), - hopper.forwardFeed() - ); - } + // /** + // * Shoot balls from the shooter until the command ends. + // * @return + // */ + // public Command shootCmd(Hopper hopper) { + // return Commands.parallel( + // runDynamicSetpoints(() -> RevolutionsPerSecond.of(5000.0/60.0), () -> Degrees.of(30)), + // hopper.forwardFeed() + // ); + // } /** * The command that the shooter can run whenever its not shooting to manage From ad98cf7cd0a167db276fd2cce9946478dbf841b3 Mon Sep 17 00:00:00 2001 From: Michael Date: Fri, 27 Feb 2026 13:53:35 -0800 Subject: [PATCH 22/48] remove unused imports --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5b93d109..0c7a6281 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -52,7 +52,6 @@ import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterIOTalonFX; -import frc.robot.subsystems.thriftyclimb.ThriftyClimb; import frc.robot.subsystems.vision.AprilTagVision; import frc.robot.subsystems.vision.CameraIO; import frc.robot.subsystems.vision.CameraIOLimelight4; From af0c70f56d9e99a3bd81f36a8614f17dd0dcebaa Mon Sep 17 00:00:00 2001 From: Michael Date: Fri, 27 Feb 2026 14:51:08 -0800 Subject: [PATCH 23/48] fixed things with merge --- src/main/java/frc/robot/commands/drive/DriveCommands.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 14b82ba4..c9f1cb59 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -64,7 +64,9 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.util.MapleSimUtil; +import frc.robot.util.VirtualTarget; public class DriveCommands { private static final double kStoppedVelocity = 0.15 ; @@ -264,7 +266,7 @@ private static Translation2d getTarget(Drive drive) { drive.getPose().getMeasureX().gt(DriveConstants.allianceZoneRed); if(robotInAllianceZone){ - target = drive.getVirtualTarget(); + target = DriveConstants.getHubTranslation(DriverStation.getAlliance().orElse(Alliance.Blue)); }else if(drive.getPose().getMeasureY().gt(DriveConstants.fieldWidth.div(2.0))){ target = blueDS ? DriveConstants.blueLeftFerryTarget : DriveConstants.redLeftFerryTarget ; }else{ @@ -274,8 +276,9 @@ private static Translation2d getTarget(Drive drive) { return target; } + // make sure this isnt bad with intellisense later public static Command pointAtShootingTarget(Drive drive, CommandXboxController gamepad, boolean shootOnMove){ - return pointAtTarget(drive, gamepad, () -> getTarget(drive), shootOnMove); + return pointAtTarget(drive, gamepad, () -> VirtualTarget.getVirtualTargetFromTarget(drive, getTarget(drive), ShooterConstants.hangTimeOnShot), shootOnMove); } /** From 5e3394837a0e22358016db8a98ef8ff325c353c1 Mon Sep 17 00:00:00 2001 From: Michael Date: Sat, 28 Feb 2026 10:15:43 -0800 Subject: [PATCH 24/48] GUYS IT TOTALLY WORKED im the best --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/commands/drive/DriveCommands.java | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 027810d9..3a9a36ab 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -45,10 +45,10 @@ public final class Constants { private static final RobotType robotType = RobotType.COMPETITION; public static final boolean spawnLessFuelInSim = true; - public static final boolean shootOnMove = false; // if true, we will allow the driver to shoot while moving, but with reduced max speed. if false, we will not allow the driver to shoot while moving. + public static final boolean shootOnMove = true; // if true, we will allow the driver to shoot while moving, but with reduced max speed. if false, we will not allow the driver to shoot while moving. //change to 0 if it really doesnt work, bc the db velocity will go to 0 and the target will just be the hub //but I think it will work so yeah trust me butch - public static final double shootOnMoveMaxSpeed = 1.0/3.0; + public static final double shootOnMoveMaxSpeed = 2.0/5.0; public static final double aimOnMoveMaxSpeed = 2.0/3.0; // obsolete rn, but change if we ever add a aim mode again public static class DriveConstants { diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index c9f1cb59..4586af55 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -249,8 +249,9 @@ public static Command pointAtTarget(Drive drive, CommandXboxController gamepad, () -> shootOnMove ? -gamepad.getLeftY() * Constants.shootOnMoveMaxSpeed : 0.0, () -> shootOnMove ? -gamepad.getLeftX() * Constants.shootOnMoveMaxSpeed : 0.0, () -> { - var hubTranslation = (shootOnMove ? drive.getVirtualTarget() : DriveConstants.getHubTranslation(DriverStation.getAlliance().orElse(Alliance.Blue))).minus(drive.getPose().getTranslation()); - var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); + Logger.recordOutput("Drive/Target", target.get()); + var translation = target.get().minus(drive.getPose().getTranslation()); + var rotation = new Rotation2d(translation.getX(), translation.getY()); return rotation; }); @@ -278,7 +279,7 @@ private static Translation2d getTarget(Drive drive) { // make sure this isnt bad with intellisense later public static Command pointAtShootingTarget(Drive drive, CommandXboxController gamepad, boolean shootOnMove){ - return pointAtTarget(drive, gamepad, () -> VirtualTarget.getVirtualTargetFromTarget(drive, getTarget(drive), ShooterConstants.hangTimeOnShot), shootOnMove); + return pointAtTarget(drive, gamepad, () -> (shootOnMove ? VirtualTarget.getVirtualTargetFromTarget(drive, getTarget(drive), ShooterConstants.hangTimeOnShot) : getTarget(drive)), shootOnMove); } /** From ffd19fb13a313fe927e2b38c097d0c3855070abf Mon Sep 17 00:00:00 2001 From: Michael Date: Sat, 28 Feb 2026 10:40:21 -0800 Subject: [PATCH 25/48] fixed merge crappiness why did we have three different lerps like what were we thinking --- src/main/java/frc/robot/RobotContainer.java | 1 - .../frc/robot/subsystems/shooter/Shooter.java | 81 ++----------------- .../subsystems/shooter/ShooterConstants.java | 22 +++-- 3 files changed, 15 insertions(+), 89 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ae87915d..fc52ce99 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -49,7 +49,6 @@ import frc.robot.subsystems.shooter.HoodIOServo; import frc.robot.subsystems.shooter.HoodIOSim; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.RobotCommands; import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterIOTalonFX; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 74da92d8..9eefc525 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -16,7 +16,6 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -30,7 +29,6 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -43,10 +41,6 @@ import frc.robot.subsystems.shooter.ShooterConstants.Positions.HubDistance; import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; -import frc.robot.subsystems.drive.Drive; -import frc.robot.commands.drive.DriveCommands; -import frc.robot.subsystems.intake.IntakeSubsystem; -import java.util.function.DoubleSupplier; public class Shooter extends SubsystemBase { @@ -185,6 +179,8 @@ public Command awaitShooting(Supplier robotPose, Supplier return Degrees.of(ShooterConstants.Positions.hoodLOW); } + }); + } public Command awaitShooting(Supplier robotPose) { return runDynamicSetpoints(() -> { @@ -204,19 +200,7 @@ public Command awaitShooting(Supplier robotPose) { var vel = 0.0; if ((Math.abs(pose.getX() - zone.magnitude()) < ShooterConstants.Positions.spinUpZone.magnitude())) { - switch(HubDistance.fromDistance(distanceToHub)) { - case LOW: - vel = ShooterConstants.Positions.distMapLow.get(distanceToHub.magnitude()); - - case MEDIUM: - vel = ShooterConstants.Positions.distMapMed.get(distanceToHub.magnitude()); - - case HIGH: - vel = ShooterConstants.Positions.distMapHigh.get(distanceToHub.magnitude()); - - default: - vel = ShooterConstants.Positions.distMapHigh.get(distanceToHub.magnitude()); - } + vel = ShooterConstants.Positions.distMap.get(distanceToHub.magnitude()); } return RotationsPerSecond.of(vel); }, @@ -275,7 +259,7 @@ public Command hoodToPosCmd(Angle pos) { } public Command runSetpoints(AngularVelocity vel, Angle pos) { return startEnd(() -> setSetpoints(vel, pos), this::stopShooter); - + } /** * Calculates Velocity and Hood Angle based on distance and Shoots * @@ -297,20 +281,7 @@ public Command shoot(Supplier pose, Hopper hopper) { return runDynamicSetpoints(() -> { Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); var vel = 0.0; - switch(HubDistance.fromDistance(distanceToHub)) { - case LOW: - vel = ShooterConstants.Positions.distMapLow.get(distanceToHub.magnitude()); - - case MEDIUM: - vel = ShooterConstants.Positions.distMapMed.get(distanceToHub.magnitude()); - - case HIGH: - vel = ShooterConstants.Positions.distMapHigh.get(distanceToHub.magnitude()); - - default: - vel = ShooterConstants.Positions.distMapHigh.get(distanceToHub.magnitude()); - } - + vel = ShooterConstants.Positions.distMap.get(distanceToHub.magnitude()); return RotationsPerSecond.of(vel); }, @@ -481,46 +452,4 @@ public Command hoodCalibration() { // return Degrees.of(45); // TODO: replace this with whatever determines shooter angle // }); // } - - public Command ferryToOutpost(Drive drive, Hopper hopper, IntakeSubsystem intake, DoubleSupplier xSupplier, DoubleSupplier ySupplier){ - return new ConditionalCommand( - Commands.parallel( - DriveCommands.joystickDriveAtAngle( - drive, - ()-> 0, - () -> 0, - () -> { - Translation2d ferryTarget = ShooterConstants.FerryPositions.blueOutpostTarget; - - var targetTranslation= ferryTarget.minus(drive.getPose().getTranslation()); - var targetRotation= new Rotation2d(targetTranslation.getX(), targetTranslation.getY()); - return targetRotation; - } - ), - shootCmd(hopper) - ), - Commands.parallel( - DriveCommands.joystickDriveAtAngle( - drive, - ()-> 0, - () -> 0, - () -> { - Translation2d ferryTarget= ShooterConstants.FerryPositions.redOutpostTarget; - - var targetTranslation= ferryTarget.minus(drive.getPose().getTranslation()); - var targetRotation= new Rotation2d(targetTranslation.getX(), targetTranslation.getY()); - return targetRotation; - } - ), - shootCmd(hopper) - ), - - () -> { - if(DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue){ - return true; - } - return false; - } - ); - } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 0eecfbb9..6dd273c4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -130,25 +130,23 @@ public class Positions { public static final double high2 = 80; public static final double high3 = 90; - public static final InterpolatingDoubleTreeMap distMapLow = new InterpolatingDoubleTreeMap(); - public static final InterpolatingDoubleTreeMap distMapMed = new InterpolatingDoubleTreeMap(); - public static final InterpolatingDoubleTreeMap distMapHigh = new InterpolatingDoubleTreeMap(); + public static final InterpolatingDoubleTreeMap distMap = new InterpolatingDoubleTreeMap(); public static void initMap() { // Hood Low - distMapLow.put(dist1, low1); - distMapLow.put(dist2, low2); - distMapLow.put(dist3, low3); + distMap.put(dist1, low1); + distMap.put(dist2, low2); + distMap.put(dist3, low3); // Hood Med - distMapMed.put(dist4, med1); - distMapMed.put(dist5, med2); - distMapMed.put(dist6, med3); + distMap.put(dist4, med1); + distMap.put(dist5, med2); + distMap.put(dist6, med3); // Hood High - distMapHigh.put(dist7, high1); - distMapHigh.put(dist8, high2); - distMapHigh.put(dist9, high3); + distMap.put(dist7, high1); + distMap.put(dist8, high2); + distMap.put(dist9, high3); } public enum HubDistance { From f4b97cf775c5021a7df70947be74f3f100c044b8 Mon Sep 17 00:00:00 2001 From: Michael Date: Mon, 2 Mar 2026 09:01:10 -0800 Subject: [PATCH 26/48] fixing things from merge it builds but ill test my logic later --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/commands/AutoCommands.java | 4 +- .../robot/commands/drive/DriveCommands.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 4 +- .../subsystems/shooter/RobotCommands.java | 87 ++----------------- .../frc/robot/subsystems/shooter/Shooter.java | 18 ++-- 6 files changed, 21 insertions(+), 96 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2b4a37fc..fdddd9b8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -308,7 +308,7 @@ private void configureBindings() { // we might want to limit the acceleration on this while shooting, but idk how to do that and hopefully it wont matter too much. // just realized we could interrupt this with POV driving, but we would still be shooting, so we might want to create a block for that, but this too probably wont come up that much and i think i am not that numb-skulled to actually do this so idk gamepad_.rightTrigger().whileTrue( - shooter_.shootCmd(drivebase_, hopper_, gamepad_, Constants.shootOnMove) + RobotCommands.shoot(shooter_, hopper_, drivebase_, gamepad_, Constants.shootOnMove) ); // shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose)); diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index 5d5834ed..60882e33 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -16,9 +16,9 @@ public static Command NeutralZoneTrenchToTrench(Drive drive, IntakeSubsystem int return Commands.sequence( DriveCommands.initialFollowPathCommand(drive,"TopToBottom Trench").deadlineFor(intake.intakeSequence()), - RobotCommands.shoot(shooter, hopper, drive), + RobotCommands.shoot(shooter, hopper, drive, null, false), DriveCommands.followPathCommand("BottomToTop").deadlineFor(intake.intakeSequence()), - RobotCommands.shoot(shooter, hopper, drive) + RobotCommands.shoot(shooter, hopper, drive, null, false) ); } diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 4586af55..96537d89 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -257,7 +257,7 @@ public static Command pointAtTarget(Drive drive, CommandXboxController gamepad, }); } - private static Translation2d getTarget(Drive drive) { + public static Translation2d getTarget(Drive drive) { Translation2d target; boolean blueDS = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 0c37fa8e..5a3fc915 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -60,8 +60,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; -import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Mode; +import frc.robot.commands.drive.DriveCommands; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.generated.CompTunerConstants; import frc.robot.util.LocalADStarAK; @@ -470,7 +470,7 @@ public RobotConfig getPathplannerConfig() { public Translation2d getVirtualTarget() { - return VirtualTarget.getVirtualTargetFromTarget(this, DriveConstants.getHubTranslation(DriverStation.getAlliance().orElse(Alliance.Blue)), ShooterConstants.hangTimeOnShot); + return VirtualTarget.getVirtualTargetFromTarget(this, DriveCommands.getTarget(this), ShooterConstants.hangTimeOnShot); } /** Returns an array of module translations. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/RobotCommands.java b/src/main/java/frc/robot/subsystems/shooter/RobotCommands.java index 155475df..2fc61a56 100644 --- a/src/main/java/frc/robot/subsystems/shooter/RobotCommands.java +++ b/src/main/java/frc/robot/subsystems/shooter/RobotCommands.java @@ -1,88 +1,19 @@ package frc.robot.subsystems.shooter; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.drive.DriveCommands; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.hopper.Hopper; public class RobotCommands { - public static Command shoot(Shooter shooter, Hopper hopper, Drive drive) { - return new ConditionalCommand( - // On true - Commands.parallel( - // Parallel one - DriveCommands.joystickDriveAtAngle(drive, - () -> 0, - () -> 0, - () -> { - Translation2d hub = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueHubPose - : ShooterConstants.Positions.redHubPose; - - var hubTranslation = hub.minus(drive.getPose().getTranslation()); - var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY()); - - return rotation; - }).andThen(drive.stopWithXCmd()), - // Parallel two - Commands.waitUntil(() -> Math.abs(drive.getChassisSpeeds().omegaRadiansPerSecond) < .001).andThen(shooter.shoot(() -> drive.getPose(), hopper)) - ), // End on true - - // On false - Commands.parallel(DriveCommands.joystickDriveAtAngle(drive, - () -> drive.getChassisSpeeds().vxMetersPerSecond, - () -> drive.getChassisSpeeds().vyMetersPerSecond, - () -> { - - var rotation = new Rotation2d(); - - var targetTranslation = new Translation2d(); - - Translation2d rightTarget = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueTargetRight - : ShooterConstants.Positions.redTargetRight; - - Translation2d leftTarget = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueTargetLeft - : ShooterConstants.Positions.redTargetLeft; - - if (drive.getPose().getY() < ShooterConstants.Positions.centerLineY) { - targetTranslation = rightTarget.minus(drive.getPose().getTranslation()); - } - else { - targetTranslation = leftTarget.minus(drive.getPose().getTranslation()); - } - rotation = new Rotation2d(targetTranslation.getX(), targetTranslation.getY()); - - return rotation; - }), - Commands.waitUntil(() -> Math.abs(drive.getChassisSpeeds().omegaRadiansPerSecond) < .001).andThen(shooter.shoot(() -> drive.getPose(), hopper)) - ), // End on false - - () -> { - - Pose2d robotPose = drive.getPose(); - - var zone = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueAllianceWall - : ShooterConstants.Positions.redAllianceWall; - - if (Math.abs(robotPose.getX() - zone.magnitude()) < ShooterConstants.Positions.spinUpZone.magnitude()) { - return true; - } - return false; - } - ); + public static Command shoot(Shooter shooter, Hopper hopper, Drive drive, CommandXboxController gamepad, boolean shootOnMove) { + return Commands.parallel( + DriveCommands.pointAtShootingTarget(drive, gamepad, shootOnMove), + Commands.sequence( + Commands.waitTime(ShooterConstants.dbRotationDelay), + shooter.shoot(drive::getVirtualTarget, hopper) + ) + ); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d792e51f..2b338fc0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -40,12 +40,9 @@ import frc.robot.subsystems.hopper.Hopper; import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; -import frc.robot.subsystems.drive.Drive; -import frc.robot.commands.drive.DriveCommands; -import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.shooter.ShooterConstants.Positions.HubDistance; import java.util.Set; -import java.util.function.DoubleSupplier; public class Shooter extends SubsystemBase { @@ -150,7 +147,7 @@ public Command shootCmd(Drive drive, Hopper hopper, CommandXboxController gamepa DriveCommands.pointAtShootingTarget(drive, gamepad, shootOnMove), Commands.sequence( Commands.waitTime(ShooterConstants.dbRotationDelay), - shooterSetpointSupplier(() -> drive.getVirtualTarget().minus(drive.getPose().getTranslation()), hopper) + shoot(drive::getVirtualTarget, hopper) ) ); } @@ -266,20 +263,17 @@ public Command runSetpoints(AngularVelocity vel, Angle pos) { * * */ - public Command shoot(Supplier pose, Hopper hopper) { + public Command shoot(Supplier dist, Hopper hopper) { return Commands.parallel( defer(() -> { // constructing - Translation2d hubTranslation = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueHubPose - : ShooterConstants.Positions.redHubPose; + return runDynamicSetpoints( () -> { - Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); + Distance distanceToHub = Meters.of(dist.get().getNorm()); var params = tuning_.getShooterParams(distanceToHub.in(Meters)); var vel = params.velocity; return RotationsPerSecond.of(vel); @@ -287,7 +281,7 @@ public Command shoot(Supplier pose, Hopper hopper) { () -> { // periodic - Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); + Distance distanceToHub = Meters.of(dist.get().getNorm()); var params = tuning_.getShooterParams(distanceToHub.in(Meters)); return Degrees.of(params.hood) ; }); From 076d73c657a468194bb141766c1c71ae0804bf91 Mon Sep 17 00:00:00 2001 From: Michael Date: Mon, 2 Mar 2026 09:29:15 -0800 Subject: [PATCH 27/48] still need to test shooting, but db is working --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/shooter/RobotCommands.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterTuning.java | 5 +++++ 4 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 994e29a4..c0b2d9c1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -42,6 +42,7 @@ public final class Constants { // Sets the currently running robot. Change to SIMBOT when running the // desktop physics simulation so AdvantageKit runs in SIM mode instead of // falling back to REPLAY. + //REMEMBER TO CHANGE BACK TO COMPETITION! private static final RobotType robotType = RobotType.SIMBOT; public static final boolean spawnLessFuelInSim = true; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fdddd9b8..35f3cbe7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -307,7 +307,7 @@ private void configureBindings() { //CHANGE BACK TO RIGHT TRIGGER! // we might want to limit the acceleration on this while shooting, but idk how to do that and hopefully it wont matter too much. // just realized we could interrupt this with POV driving, but we would still be shooting, so we might want to create a block for that, but this too probably wont come up that much and i think i am not that numb-skulled to actually do this so idk - gamepad_.rightTrigger().whileTrue( + gamepad_.a().whileTrue( RobotCommands.shoot(shooter_, hopper_, drivebase_, gamepad_, Constants.shootOnMove) ); // shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose)); diff --git a/src/main/java/frc/robot/subsystems/shooter/RobotCommands.java b/src/main/java/frc/robot/subsystems/shooter/RobotCommands.java index 2fc61a56..bb4e4d7e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/RobotCommands.java +++ b/src/main/java/frc/robot/subsystems/shooter/RobotCommands.java @@ -12,7 +12,7 @@ public static Command shoot(Shooter shooter, Hopper hopper, Drive drive, Command DriveCommands.pointAtShootingTarget(drive, gamepad, shootOnMove), Commands.sequence( Commands.waitTime(ShooterConstants.dbRotationDelay), - shooter.shoot(drive::getVirtualTarget, hopper) + shooter.shoot(() -> drive.getVirtualTarget().minus(drive.getPose().getTranslation()), hopper) ) ); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java b/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java index 9f4c4e0c..d125ce0f 100755 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java @@ -55,6 +55,10 @@ public OneSettings(double h, double[] d, double[] v) { private static final double highPlusDist[] = { 4.47, 5.2 } ; private static final double highPlusVelocity[] = { 68, 62} ; + private static final double maxHood = 50; + private static final double maxDist[] = { 5.2, 50.0 }; + private static final double maxVelocities[] = {62, 62}; + private List settings_ = new ArrayList() ; private int lastHoodIndex_ ; @@ -119,6 +123,7 @@ private void initData() { addOneHood(lowHood, lowDist, lowVelocities) ; addOneHood(medHood, medDist, medVelocities) ; addOneHood(highPlusHood, highPlusDist, highPlusVelocity) ; + addOneHood(maxHood, maxDist, maxVelocities) ; } private void addOneHood(double hood, double[] dist, double[] vels) { From a430873e709aafc775eed786d26044e914b86f8f Mon Sep 17 00:00:00 2001 From: Michael Date: Mon, 2 Mar 2026 09:30:33 -0800 Subject: [PATCH 28/48] changed back to comp --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c0b2d9c1..e7a72ab0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -43,7 +43,7 @@ public final class Constants { // desktop physics simulation so AdvantageKit runs in SIM mode instead of // falling back to REPLAY. //REMEMBER TO CHANGE BACK TO COMPETITION! - private static final RobotType robotType = RobotType.SIMBOT; + private static final RobotType robotType = RobotType.COMPETITION; public static final boolean spawnLessFuelInSim = true; public static final boolean shootOnMove = true; // if true, we will allow the driver to shoot while moving, but with reduced max speed. if false, we will not allow the driver to shoot while moving. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 35f3cbe7..fdddd9b8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -307,7 +307,7 @@ private void configureBindings() { //CHANGE BACK TO RIGHT TRIGGER! // we might want to limit the acceleration on this while shooting, but idk how to do that and hopefully it wont matter too much. // just realized we could interrupt this with POV driving, but we would still be shooting, so we might want to create a block for that, but this too probably wont come up that much and i think i am not that numb-skulled to actually do this so idk - gamepad_.a().whileTrue( + gamepad_.rightTrigger().whileTrue( RobotCommands.shoot(shooter_, hopper_, drivebase_, gamepad_, Constants.shootOnMove) ); // shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose)); From 4681480b099a8acc64a20dad2ea78722b5094f60 Mon Sep 17 00:00:00 2001 From: Michael Date: Mon, 2 Mar 2026 09:33:49 -0800 Subject: [PATCH 29/48] made max tuning constants dynamic --- .../java/frc/robot/subsystems/shooter/ShooterTuning.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java b/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java index d125ce0f..d8ad8da7 100755 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java @@ -55,9 +55,9 @@ public OneSettings(double h, double[] d, double[] v) { private static final double highPlusDist[] = { 4.47, 5.2 } ; private static final double highPlusVelocity[] = { 68, 62} ; - private static final double maxHood = 50; - private static final double maxDist[] = { 5.2, 50.0 }; - private static final double maxVelocities[] = {62, 62}; + private static final double maxHood = highPlusHood; + private static final double maxDist[] = { highPlusDist[0], 50.0 }; + private static final double maxVelocities[] = {highPlusVelocity[1], highPlusVelocity[1]}; private List settings_ = new ArrayList() ; private int lastHoodIndex_ ; From bdd5563278f717aba7b72c4a96a074dbf65c0082 Mon Sep 17 00:00:00 2001 From: Michael Date: Mon, 2 Mar 2026 09:36:02 -0800 Subject: [PATCH 30/48] added comment ig --- src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java b/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java index d8ad8da7..79c6ab0f 100755 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java @@ -56,7 +56,7 @@ public OneSettings(double h, double[] d, double[] v) { private static final double highPlusVelocity[] = { 68, 62} ; private static final double maxHood = highPlusHood; - private static final double maxDist[] = { highPlusDist[0], 50.0 }; + private static final double maxDist[] = { highPlusDist[0], 50.0 }; // just a big number that will never be crossed private static final double maxVelocities[] = {highPlusVelocity[1], highPlusVelocity[1]}; private List settings_ = new ArrayList() ; From 4296ff4d731593fed629f784fb1086d07fc34c45 Mon Sep 17 00:00:00 2001 From: Michael Date: Mon, 2 Mar 2026 09:53:13 -0800 Subject: [PATCH 31/48] Revert "Fix Long Loop Cycle" This reverts commit 888c41189838f0a93396c58e1d763b183219b4cd. --- src/main/java/frc/robot/RobotContainer.java | 6 ------ .../frc/robot/subsystems/vision/AprilTagVision.java | 10 ++++++++++ 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5098ebb6..3cde3748 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,7 +33,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; import frc.robot.Constants.RobotType; import frc.robot.commands.drive.DriveCommands; @@ -223,11 +222,6 @@ public RobotContainer() { hopper_ = new Hopper(new HopperIO() {}); } - // Force Load Apriltag Layout - for (var tag : FieldConstants.layout.getTags()) { - System.out.println("Tag Loaded: " + tag.ID); - } - DriveCommands.configure( drivebase_, () -> -gamepad_.getLeftY(), diff --git a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java index 2a1fbb4d..11736800 100755 --- a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java +++ b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java @@ -101,6 +101,8 @@ public void periodic() { // Iterate cameras for logging and pose estimations. for (int cam = 0; cam < io_.length; cam++) { + LoggedTracer.reset(); + // Activate disconnected alert. alerts_[cam].set(!inputs_[cam].connected); @@ -109,6 +111,8 @@ public void periodic() { ArrayList tagPoses = new ArrayList<>(); + // Loop through visible tags. + LoggedTracer.record("Vision/Camera" + cam + "/PreLoop"); for (Fiducial fid : inputs_[cam].fiducials) { Optional pose = FieldConstants.layout.getTagPose(fid.id()); if (pose.isPresent()) { @@ -116,15 +120,21 @@ public void periodic() { } } + LoggedTracer.record("Vision/Camera" + cam + "/GatherFiducials"); + PoseEstimation est = inputs_[cam].poseEstimate; poseEstimates.add(est); // Add to summaries. summaryTagPoses.addAll(tagPoses); + LoggedTracer.record("Vision/Camera" + cam + "/AddArrays"); + // Log camera-specific outputs. Logger.recordOutput(getCameraKey(cam, "TagPoses"), tagPoses.toArray(new Pose3d[0])); Logger.recordOutput(getCameraKey(cam, "BotPose"), est.pose()); + + LoggedTracer.record("Vision/Camera" + cam + "/LogOutputs"); } LoggedTracer.record("Vision/GatherPoses"); From f94944e0945ba0b8d9a312608425b5410121c08b Mon Sep 17 00:00:00 2001 From: Michael Date: Mon, 2 Mar 2026 13:14:25 -0800 Subject: [PATCH 32/48] fixed new merge conflicts, added better wait for drivebase --- .../frc/robot/commands/robot/RobotCommands.java | 15 +++++++++------ .../frc/robot/subsystems/shooter/Shooter.java | 11 +++-------- 2 files changed, 12 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/commands/robot/RobotCommands.java b/src/main/java/frc/robot/commands/robot/RobotCommands.java index 489a1989..e7c9335d 100644 --- a/src/main/java/frc/robot/commands/robot/RobotCommands.java +++ b/src/main/java/frc/robot/commands/robot/RobotCommands.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.RobotState; import frc.robot.commands.drive.DriveCommands; import frc.robot.subsystems.drive.Drive; @@ -89,11 +90,13 @@ private static Command ferry(Shooter shooter, Hopper hopper, Drive drive) { * @param drive * @return */ - public static Command shoot(Shooter shooter, Hopper hopper, Drive drive) { - return Commands.either( - shootHub(shooter, hopper, drive), - ferry(shooter, hopper, drive), - RobotState::inAllianceZone - ); + public static Command shoot(Shooter shooter, Hopper hopper, Drive drive, CommandXboxController gamepad, boolean shootOnMove) { + return Commands.parallel( + DriveCommands.pointAtShootingTarget(drive, gamepad, shootOnMove), + Commands.sequence( + Commands.waitTime(ShooterConstants.dbRotationDelay), + shooter.shoot(drive, hopper, gamepad, shootOnMove) + ) + ); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 96f94709..b9c3d6f9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -23,8 +23,6 @@ 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.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; @@ -42,9 +40,6 @@ import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; import frc.robot.subsystems.shooter.ShooterConstants.Positions.HubDistance; -import frc.robot.subsystems.drive.Drive; -import frc.robot.commands.drive.DriveCommands; -import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.ShooterTuning.ShooterParams; import java.util.Set; @@ -147,12 +142,12 @@ private void setSetpoints(AngularVelocity vel, Angle pos) { * @param pos * @return */ - public Command shootCmd(Drive drive, Hopper hopper, CommandXboxController gamepad, boolean shootOnMove) { + public Command shoot(Drive drive, Hopper hopper, CommandXboxController gamepad, boolean shootOnMove) { return Commands.parallel( DriveCommands.pointAtShootingTarget(drive, gamepad, shootOnMove), Commands.sequence( - Commands.waitTime(ShooterConstants.dbRotationDelay), - shoot(drive::getVirtualTarget, hopper) + Commands.waitUntil(() -> drive.rotationIsNear(drive.getVirtualTarget().minus(drive.getPose().getTranslation()).getAngle(), ShooterConstants.aimingTolerance)), + shootAtDistance(() -> Meters.of(drive.getVirtualTarget().getDistance(drive.getPose().getTranslation())), hopper) ) ); } From 90a70dc647ea6b0df9f27813e36049033a095f23 Mon Sep 17 00:00:00 2001 From: Michael Date: Mon, 2 Mar 2026 13:34:09 -0800 Subject: [PATCH 33/48] fixed duplicate things --- .../java/frc/robot/commands/robot/RobotCommands.java | 2 +- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 9 +-------- 2 files changed, 2 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/robot/RobotCommands.java b/src/main/java/frc/robot/commands/robot/RobotCommands.java index e7c9335d..45cd7b11 100644 --- a/src/main/java/frc/robot/commands/robot/RobotCommands.java +++ b/src/main/java/frc/robot/commands/robot/RobotCommands.java @@ -94,7 +94,7 @@ public static Command shoot(Shooter shooter, Hopper hopper, Drive drive, Command return Commands.parallel( DriveCommands.pointAtShootingTarget(drive, gamepad, shootOnMove), Commands.sequence( - Commands.waitTime(ShooterConstants.dbRotationDelay), + Commands.waitUntil(() -> drive.rotationIsNear(drive.getVirtualTarget().minus(drive.getPose().getTranslation()).getAngle(), ShooterConstants.aimingTolerance)), shooter.shoot(drive, hopper, gamepad, shootOnMove) ) ); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b9c3d6f9..ec346902 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -34,7 +34,6 @@ import frc.robot.RobotState; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; -import frc.robot.commands.drive.DriveCommands; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.hopper.Hopper; import frc.robot.util.MapleSimUtil; @@ -143,13 +142,7 @@ private void setSetpoints(AngularVelocity vel, Angle pos) { * @return */ public Command shoot(Drive drive, Hopper hopper, CommandXboxController gamepad, boolean shootOnMove) { - return Commands.parallel( - DriveCommands.pointAtShootingTarget(drive, gamepad, shootOnMove), - Commands.sequence( - Commands.waitUntil(() -> drive.rotationIsNear(drive.getVirtualTarget().minus(drive.getPose().getTranslation()).getAngle(), ShooterConstants.aimingTolerance)), - shootAtDistance(() -> Meters.of(drive.getVirtualTarget().getDistance(drive.getPose().getTranslation())), hopper) - ) - ); + return shootAtDistance(() -> Meters.of(drive.getVirtualTarget().getDistance(drive.getPose().getTranslation())), hopper); } /** From 9d57fbb0c801ca6120178f32a6c3d7cf989ed9b0 Mon Sep 17 00:00:00 2001 From: Michael Date: Tue, 3 Mar 2026 14:21:09 -0800 Subject: [PATCH 34/48] some cleanup --- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/subsystems/shooter/Shooter.java | 7 ------- .../frc/robot/subsystems/shooter/ShooterConstants.java | 2 +- 3 files changed, 1 insertion(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c735a6fa..9ed0ba23 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -310,7 +310,6 @@ private void configureBindings() { // Cycle through shooter tunings when the X button is pressed. gamepad_.x().or(operatorGamepad_.x()).onTrue(shooter_.cycleTuning()) ; - gamepad_.a().whileTrue(intake_.ejectSequence()); } 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 b9b382e1..b40d0c87 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -9,7 +9,6 @@ import static edu.wpi.first.units.Units.Volts; import java.util.Set; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -29,8 +28,6 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -40,7 +37,6 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; import frc.robot.RobotState; -import frc.robot.commands.drive.DriveCommands; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.hopper.Hopper; import frc.robot.subsystems.intake.IntakeSubsystem; @@ -54,9 +50,6 @@ import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; import frc.robot.subsystems.shooter.ShooterConstants.Positions.HubDistance; -import frc.robot.subsystems.shooter.tunings.ShooterTuning.ShooterParams; - -import java.util.Set; public class Shooter extends SubsystemBase { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index b65c4b2e..5ea51112 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -29,7 +29,7 @@ public class ShooterConstants { public static final Distance allowedTrenchDistance = Meters.of(1.0); - public static final Time hangTimeOnShot = Seconds.of(8/4.5);//number taken from video, 8 frames / 4.5 frames/sec seems to be about the right hang time. this should be tuned better later tho + public static final Time hangTimeOnShot = Seconds.of(1.8); public static final Time dbRotationDelay = Seconds.of(0.5); public static final double timeBeforeShoot = 0.2; From 9f5f5063106415aa56748d9ed07a59fcb2c35583 Mon Sep 17 00:00:00 2001 From: Michael Date: Tue, 3 Mar 2026 15:32:59 -0800 Subject: [PATCH 35/48] added support for hangtime tuning --- src/main/java/frc/robot/RobotContainer.java | 4 +-- .../robot/commands/drive/DriveCommands.java | 6 +++-- .../robot/commands/robot/RobotCommands.java | 4 +-- .../frc/robot/subsystems/drive/Drive.java | 5 ++-- .../frc/robot/subsystems/shooter/Shooter.java | 4 +-- .../subsystems/shooter/ShooterConstants.java | 11 +++----- .../shooter/tunings/ArcShooterTuning.java | 14 +++++++---- .../shooter/tunings/FlatShooterTuning.java | 9 ++++--- .../shooter/tunings/ShooterTuning.java | 25 ++++++++++++------- .../java/frc/robot/util/VirtualTarget.java | 15 ++++++++++- 10 files changed, 62 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9ed0ba23..323aaf44 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -291,7 +291,7 @@ private void configureBindings() { // hopper_.setDefaultCommand(hopper_.idleScrambler()); // When the shooter isnt shooting, get it ready to shoot. - shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose, drivebase_::getVirtualTarget)); + shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose, () -> drivebase_.getVirtualTarget(shooter_))); // while in alliance zone, point drivebase at virtual target, but still allow translational driving //lowkey we are not gonna need this but like maybe so idk imma just keep it @@ -309,7 +309,7 @@ private void configureBindings() { gamepad_.a().or(operatorGamepad_.a()).whileTrue(intake_.ejectSequence()); // Cycle through shooter tunings when the X button is pressed. - gamepad_.x().or(operatorGamepad_.x()).onTrue(shooter_.cycleTuning()) ; + gamepad_.back().or(operatorGamepad_.back()).onTrue(shooter_.cycleTuning()) ; } private void configureDriveBindings() { diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 96537d89..ec6e88d6 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -64,6 +64,7 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.util.MapleSimUtil; import frc.robot.util.VirtualTarget; @@ -278,8 +279,9 @@ public static Translation2d getTarget(Drive drive) { } // make sure this isnt bad with intellisense later - public static Command pointAtShootingTarget(Drive drive, CommandXboxController gamepad, boolean shootOnMove){ - return pointAtTarget(drive, gamepad, () -> (shootOnMove ? VirtualTarget.getVirtualTargetFromTarget(drive, getTarget(drive), ShooterConstants.hangTimeOnShot) : getTarget(drive)), shootOnMove); + public static Command pointAtShootingTarget(Drive drive, Shooter shooter, CommandXboxController gamepad, boolean shootOnMove){ + + return pointAtTarget(drive, gamepad, () -> (shootOnMove ? VirtualTarget.getVirtualTargetFromTarget(drive, getTarget(drive), shooter.getTuning(), ShooterConstants.hangtimeLoopPasses) : getTarget(drive)), shootOnMove); } /** diff --git a/src/main/java/frc/robot/commands/robot/RobotCommands.java b/src/main/java/frc/robot/commands/robot/RobotCommands.java index 5bdf9fc0..3469d9f8 100644 --- a/src/main/java/frc/robot/commands/robot/RobotCommands.java +++ b/src/main/java/frc/robot/commands/robot/RobotCommands.java @@ -93,9 +93,9 @@ private static Command ferry(Shooter shooter, Hopper hopper, IntakeSubsystem int */ public static Command shoot(Shooter shooter, Hopper hopper, Drive drive, IntakeSubsystem intake, CommandXboxController gamepad, boolean shootOnMove) { return Commands.parallel( - DriveCommands.pointAtShootingTarget(drive, gamepad, shootOnMove), + DriveCommands.pointAtShootingTarget(drive, shooter, gamepad, shootOnMove), Commands.sequence( - Commands.waitUntil(() -> drive.rotationIsNear(drive.getVirtualTarget().minus(drive.getPose().getTranslation()).getAngle(), ShooterConstants.aimingTolerance)), + Commands.waitUntil(() -> drive.rotationIsNear(drive.getVirtualTarget(shooter).minus(drive.getPose().getTranslation()).getAngle(), ShooterConstants.aimingTolerance)), shooter.shoot(drive, hopper, intake, gamepad, shootOnMove) ) ); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 09f91d11..b3b435d4 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -63,6 +63,7 @@ import frc.robot.Constants; import frc.robot.Constants.Mode; import frc.robot.commands.drive.DriveCommands; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.generated.CompTunerConstants; import frc.robot.util.LocalADStarAK; @@ -475,8 +476,8 @@ public RobotConfig getPathplannerConfig() { - public Translation2d getVirtualTarget() { - return VirtualTarget.getVirtualTargetFromTarget(this, DriveCommands.getTarget(this), ShooterConstants.hangTimeOnShot); + public Translation2d getVirtualTarget(Shooter shooter) { + return VirtualTarget.getVirtualTargetFromTarget(this, DriveCommands.getTarget(this), shooter.getTuning(), ShooterConstants.hangtimeLoopPasses); } /** Returns an array of module translations. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b40d0c87..25c765c3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -173,7 +173,7 @@ private void setSetpoints(AngularVelocity vel, Angle pos) { setHoodAngle(hoodTarget); } - private ShooterTuning getTuning() { + public ShooterTuning getTuning() { return tunings_.get(tuningIndex_); } @@ -191,7 +191,7 @@ public Command cycleTuning() { * @return */ public Command shoot(Drive drive, Hopper hopper, IntakeSubsystem intake, CommandXboxController gamepad, boolean shootOnMove) { - return shootAtDistance(() -> Meters.of(drive.getVirtualTarget().getDistance(drive.getPose().getTranslation())), hopper, intake); + return shootAtDistance(() -> Meters.of(drive.getVirtualTarget(this).getDistance(drive.getPose().getTranslation())), hopper, intake); } /** diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 5ea51112..eea1d100 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -29,19 +29,16 @@ public class ShooterConstants { public static final Distance allowedTrenchDistance = Meters.of(1.0); + @Deprecated public static final Time hangTimeOnShot = Seconds.of(1.8); - public static final Time dbRotationDelay = Seconds.of(0.5); + + public static final int hangtimeLoopPasses = 5; // for a for loop in calculating distance from the hub, as you need the distance to get the hangtime, but you also need the hangtime to get the distance public static final double timeBeforeShoot = 0.2; public static final Angle hoodParkedAngle = Degrees.of(5.0) ; public static final Angle hoodMaxAngle = Degrees.of(75.0) ; public static final Angle hoodMinAngle = Degrees.of(0.0) ; - public class FerryPositions{ - public static final Translation2d blueOutpostTarget= new Translation2d(2.135, 1.639); - public static final Translation2d redOutpostTarget= new Translation2d(14.0,6.0); - } - public class PID { // shooter public static final double shooterkP = 0.45; @@ -124,7 +121,7 @@ public class HoodPWMs { public static final int hoodRightPWMPort = 0; } - public class ferryPositions{ + public class FerryPositions{ public static final Translation2d blueOutpostFerryTarget= new Translation2d(2.136,1.935); //ferry target for the blue outpost public static final Translation2d redOutpostFerryTarget= new Translation2d(14.0,6.0); //ferry target for the red outpost public static final Translation2d blueDepotFerryTarget= new Translation2d(2.0,6.0); //ferry target for the blue depot diff --git a/src/main/java/frc/robot/subsystems/shooter/tunings/ArcShooterTuning.java b/src/main/java/frc/robot/subsystems/shooter/tunings/ArcShooterTuning.java index f6b305d3..b0699d2a 100755 --- a/src/main/java/frc/robot/subsystems/shooter/tunings/ArcShooterTuning.java +++ b/src/main/java/frc/robot/subsystems/shooter/tunings/ArcShooterTuning.java @@ -6,27 +6,31 @@ public class ArcShooterTuning extends ShooterTuning { private static final double touchingHood = 5 ; private static final double touchingDist[] = { 1.616 , 1.86, 2.18} ; private static final double touchingVelocities[] = { 55.0, 58.0, 65.0 } ; + private static final double touchingHangtime[] = { 1.8, 1.8, 1.8 } ; private static final double lowHood = 15 ; private static final double lowDist[] = { 2.47, 2.77, 3.06, 3.35} ; private static final double lowVelocities[] = { 62, 64, 68, 73 } ; + private static final double lowHangtime[] = { 1.8, 1.8, 1.8, 1.8 } ; private static final double medHood = 25 ; private static final double medDist[] = { 3.69, 4.02, 4.30 } ; private static final double medVelocities[] = { 70, 72, 75 }; - + private static final double medHangtime[] = { 1.8, 1.8, 1.8 } ; + private static final double highPlusHood = 35 ; private static final double highPlusDist[] = { 4.60, 4.92, 5.24 } ; private static final double highPlusVelocity[] = { 68, 71, 83 } ; + private static final double highPlusHangtime[] = { 1.8, 1.8, 1.8 } ; public ArcShooterTuning() { super("arc") ; } protected void initData() { - addOneHood(touchingHood, touchingDist, touchingVelocities) ; - addOneHood(lowHood, lowDist, lowVelocities) ; - addOneHood(medHood, medDist, medVelocities) ; - addOneHood(highPlusHood, highPlusDist, highPlusVelocity) ; + addOneHood(touchingHood, touchingDist, touchingVelocities, touchingHangtime) ; + addOneHood(lowHood, lowDist, lowVelocities, lowHangtime) ; + addOneHood(medHood, medDist, medVelocities, medHangtime) ; + addOneHood(highPlusHood, highPlusDist, highPlusVelocity, highPlusHangtime) ; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/tunings/FlatShooterTuning.java b/src/main/java/frc/robot/subsystems/shooter/tunings/FlatShooterTuning.java index dd95fca7..fc4e9fc8 100755 --- a/src/main/java/frc/robot/subsystems/shooter/tunings/FlatShooterTuning.java +++ b/src/main/java/frc/robot/subsystems/shooter/tunings/FlatShooterTuning.java @@ -6,22 +6,25 @@ public class FlatShooterTuning extends ShooterTuning { private static final double touchingHood = 5 ; private static final double touchingDist[] = { 1.616 , 1.86, 2.18} ; private static final double touchingVelocities[] = { 55.0, 58.0, 65.0 } ; + private static final double touchingHangtime[] = { 1.4, 1.4, 1.4 } ; private static final double lowHood = 35 ; private static final double lowDist[] = { 2.47, 2.78, 3.08, 3.40 } ; private static final double lowVelocities[] = { 52, 54, 56, 59} ; + private static final double lowHangtime[] = { 1.4, 1.4, 1.4, 1.4 } ; private static final double medHood = 45 ; private static final double medDist[] = { 4.0, 4.31, 4.93} ; private static final double medVelocities[] = { 58, 61, 66 }; + private static final double medHangtime[] = { 1.4, 1.4, 1.4 } ; public FlatShooterTuning() { super("flat") ; } protected void initData() { - addOneHood(touchingHood, touchingDist, touchingVelocities) ; - addOneHood(lowHood, lowDist, lowVelocities) ; - addOneHood(medHood, medDist, medVelocities) ; + addOneHood(touchingHood, touchingDist, touchingVelocities, touchingHangtime) ; + addOneHood(lowHood, lowDist, lowVelocities, lowHangtime) ; + addOneHood(medHood, medDist, medVelocities, medHangtime) ; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/tunings/ShooterTuning.java b/src/main/java/frc/robot/subsystems/shooter/tunings/ShooterTuning.java index e8f64458..88485b56 100644 --- a/src/main/java/frc/robot/subsystems/shooter/tunings/ShooterTuning.java +++ b/src/main/java/frc/robot/subsystems/shooter/tunings/ShooterTuning.java @@ -13,11 +13,13 @@ public class ShooterParams { public double dist ; public double hood ; public double velocity ; + public double hangtime; - public ShooterParams(double d, double h, double v) { + public ShooterParams(double d, double h, double v, double ht) { dist = d ; hood = h ; velocity = v ; + hangtime = ht; } } ; @@ -25,17 +27,21 @@ private class OneSettings { public double hood_ ; public double min_dist_ ; public double max_dist_ ; - public InterpolatingDoubleTreeMap map_ ; + public InterpolatingDoubleTreeMap velMap_ ; + public InterpolatingDoubleTreeMap hangtimeMap_ ; - public OneSettings(double h, double[] d, double[] v) { + public OneSettings(double h, double[] d, double[] v, double[] ht) { hood_ = h ; - map_ = new InterpolatingDoubleTreeMap() ; + velMap_ = new InterpolatingDoubleTreeMap() ; + hangtimeMap_ = new InterpolatingDoubleTreeMap() ; for(var i = 0 ; i < d.length ; i++) { - map_.put(d[i], v[i]) ; + velMap_.put(d[i], v[i]) ; + hangtimeMap_.put(d[i], ht[i]) ; } min_dist_ = d[0] ; - max_dist_ = d[d.length - 1] ;} + max_dist_ = d[d.length - 1] ; + } } private final double HYSTERESIS_DIST = 0.06; // in meters, about a foot of hysteresis when @@ -57,9 +63,10 @@ public String getName() { public ShooterParams getShooterParams(double dist) { int h = getHoodIndex(dist) ; - var ret = new ShooterParams(dist, settings_.get(h).hood_, settings_.get(h).map_.get(dist)) ; + var ret = new ShooterParams(dist, settings_.get(h).hood_, settings_.get(h).velMap_.get(dist), settings_.get(h).hangtimeMap_.get(dist)) ; Logger.recordOutput("ShooterTuning/hood", ret.hood); Logger.recordOutput("ShooterTuning/velocity", ret.velocity); + Logger.recordOutput("ShooterTuning/hangtime", ret.hangtime); return ret ; } @@ -97,7 +104,7 @@ else if (newIndex == lastHoodIndex_ - 1) { return newIndex ; } - protected void addOneHood(double hood, double[] dist, double[] vels) { + protected void addOneHood(double hood, double[] dist, double[] vels, double[] hangtimes) { if (dist.length != vels.length) { throw new RuntimeException("invalid data, dist and vel data should be the same size") ; } @@ -106,7 +113,7 @@ protected void addOneHood(double hood, double[] dist, double[] vels) { throw new RuntimeException("invalid data, each hood value should contain two entries") ; } - settings_.add(new OneSettings(hood, dist, vels)); + settings_.add(new OneSettings(hood, dist, vels, hangtimes)) ; settings_.sort((s1, s2) -> { return Double.compare(s1.hood_, s2.hood_); diff --git a/src/main/java/frc/robot/util/VirtualTarget.java b/src/main/java/frc/robot/util/VirtualTarget.java index f53f315e..01eab2fd 100644 --- a/src/main/java/frc/robot/util/VirtualTarget.java +++ b/src/main/java/frc/robot/util/VirtualTarget.java @@ -1,13 +1,26 @@ package frc.robot.util; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.shooter.tunings.ShooterTuning; public class VirtualTarget { - public static Translation2d getVirtualTargetFromTarget(Drive drive, Translation2d target, Time hangTime){ + public static Translation2d getVirtualTargetFromTarget(Drive drive, Translation2d target, ShooterTuning tuning, double loopPasses){ + Distance distToHub; + if(loopPasses == 0){ + distToHub = Meters.of(target.getDistance(drive.getPose().getTranslation())); + }else{ + distToHub = Meters.of(getVirtualTargetFromTarget(drive, target, tuning, loopPasses - 1) + .getDistance(drive.getPose().getTranslation()) + ); + } + Time hangTime = Seconds.of(tuning.getShooterParams(distToHub.in(Meters)).hangtime); return target.minus( new Translation2d( MetersPerSecond.of(drive.getFieldChassisSpeeds().vxMetersPerSecond).times(hangTime), From 0b553d29a2df0ad11676cfac5f36b222d127ff75 Mon Sep 17 00:00:00 2001 From: Michael Date: Wed, 4 Mar 2026 09:02:20 -0800 Subject: [PATCH 36/48] added tuning for hangtime and its JSON support that wasnt so bad --- src/main/deploy/tuning/arc.json | 45 ++++-- src/main/{ => deploy/tuning}/flat.json | 150 ++++++++++-------- .../subsystems/shooter/ShooterTuning.java | 31 ++-- .../java/frc/robot/util/VirtualTarget.java | 4 +- 4 files changed, 134 insertions(+), 96 deletions(-) rename src/main/{ => deploy/tuning}/flat.json (64%) mode change 100755 => 100644 diff --git a/src/main/deploy/tuning/arc.json b/src/main/deploy/tuning/arc.json index a75bad1b..2ef3badc 100755 --- a/src/main/deploy/tuning/arc.json +++ b/src/main/deploy/tuning/arc.json @@ -7,17 +7,20 @@ { // Test mode "distance" : 1.616, - "velocity" : 55.0 + "velocity" : 55.0, + "hangtime" : 1.8 }, { // Test mode "distance" : 1.86, - "velocity" : 58.0 + "velocity" : 58.0, + "hangtime" : 1.8 }, { // Test mode "distance" : 2.18, - "velocity" : 65.0 + "velocity" : 65.0, + "hangtime" : 1.8 } ] }, @@ -27,27 +30,32 @@ { // Shoot cmd "distance" : 2.22, - "velocity" : 58.0 + "velocity" : 58.0, + "hangtime" : 1.8 }, { // Shoot mode "distance" : 2.43, - "velocity" : 59.0 + "velocity" : 59.0, + "hangtime" : 1.8 }, { // Test mode "distance" : 2.77, - "velocity" : 64.0 + "velocity" : 64.0, + "hangtime" : 1.8 }, { // Test mode "distance" : 3.06, - "velocity" : 68.0 + "velocity" : 68.0, + "hangtime" : 1.8 }, { // Test mode "distance" : 3.35, - "velocity" : 71.0 + "velocity" : 71.0, + "hangtime" : 1.8 } ] }, @@ -57,22 +65,26 @@ { // Shoot cmd "distance" : 3.40, - "velocity" : 58.5 + "velocity" : 58.5, + "hangtime" : 1.8 }, { // Shoot cmd "distance" : 3.70, - "velocity" : 64.0 + "velocity" : 64.0, + "hangtime" : 1.8 }, { // Test mode "distance" : 4.02, - "velocity" : 72.0 + "velocity" : 72.0, + "hangtime" : 1.8 }, { // Test mode "distance" : 4.30, - "velocity" : 75.0 + "velocity" : 75.0, + "hangtime" : 1.8 } ] }, @@ -82,17 +94,20 @@ { // Test mode "distance" : 4.60, - "velocity" : 68.0 + "velocity" : 68.0, + "hangtime" : 1.8 }, { // Test mode "distance" : 4.92, - "velocity" : 71.0 + "velocity" : 71.0, + "hangtime" : 1.8 }, { // Test mode "distance" : 5.24, - "velocity" : 83.0 + "velocity" : 83.0, + "hangtime" : 1.8 } ] } diff --git a/src/main/flat.json b/src/main/deploy/tuning/flat.json old mode 100755 new mode 100644 similarity index 64% rename from src/main/flat.json rename to src/main/deploy/tuning/flat.json index 47de3759..37288fb8 --- a/src/main/flat.json +++ b/src/main/deploy/tuning/flat.json @@ -1,70 +1,80 @@ -{ - "name" : "flat", - "data" : [ - { - "hood" : 5.0, - "points" : [ - { - // Test mode - "distance" : 1.616, - "velocity" : 55.0 - }, - { - // Test mode - "distance" : 1.86, - "velocity" : 58.0 - }, - { - // Test mode - "distance" : 2.18, - "velocity" : 65.0 - } - ] - }, - { - "hood" : 35.0, - "points" : [ - { - // Test mode - "distance" : 2.47, - "velocity" : 62.0 - }, - { - // Test mode - "distance" : 2.77, - "velocity" : 64.0 - }, - { - // Test mode - "distance" : 3.06, - "velocity" : 68.0 - }, - { - // Test mode - "distance" : 3.35, - "velocity" : 73.0 - } - ] - }, - { - "hood" : 45.0, - "points" : [ - { - // Test mode - "distance" : 4.0, - "velocity" : 58.0 - }, - { - // Test mode - "distance" : 4.31, - "velocity" : 61.0 - }, - { - // Test mode - "distance" : 4.93, - "velocity" : 66.0 - } - ] - } - ] -} +{ + "name" : "flat", + "data" : [ + { + "hood" : 5.0, + "points" : [ + { + // Test mode + "distance" : 1.616, + "velocity" : 55.0, + "hangtime" : 1.8 + }, + { + // Test mode + "distance" : 1.86, + "velocity" : 58.0, + "hangtime" : 1.8 + }, + { + // Test mode + "distance" : 2.18, + "velocity" : 65.0, + "hangtime" : 1.8 + } + ] + }, + { + "hood" : 35.0, + "points" : [ + { + // Test mode + "distance" : 2.47, + "velocity" : 62.0, + "hangtime" : 1.8 + }, + { + // Test mode + "distance" : 2.77, + "velocity" : 64.0, + "hangtime" : 1.8 + }, + { + // Test mode + "distance" : 3.06, + "velocity" : 68.0, + "hangtime" : 1.8 + }, + { + // Test mode + "distance" : 3.35, + "velocity" : 73.0, + "hangtime" : 1.8 + } + ] + }, + { + "hood" : 45.0, + "points" : [ + { + // Test mode + "distance" : 4.0, + "velocity" : 58.0, + "hangtime" : 1.8 + }, + { + // Test mode + "distance" : 4.31, + "velocity" : 61.0, + "hangtime" : 1.8 + }, + { + // Test mode + "distance" : 4.93, + "velocity" : 66.0, + "hangtime" : 1.8 + } + ] + } + ] +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java b/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java index 5d54a557..3812284b 100755 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterTuning.java @@ -19,21 +19,25 @@ public class ShooterParams { public double dist ; public double hood ; public double velocity ; + public double hangtime; - public ShooterParams(double d, double h, double v) { + public ShooterParams(double d, double h, double v, double ht) { dist = d ; hood = h ; velocity = v ; + hangtime = ht ; } } ; public static class OneTuningValue { public final double dist_ ; public final double vel_ ; + public final double hangtime_ ; - public OneTuningValue(double d, double v) { + public OneTuningValue(double d, double v, double ht) { dist_ = d ; vel_ = v ; + hangtime_ = ht ; } } @@ -41,13 +45,17 @@ private class OneHoodTuning { public double hood_ ; public double min_dist_ ; public double max_dist_ ; - public InterpolatingDoubleTreeMap map_ ; + public InterpolatingDoubleTreeMap velmap_ ; + public InterpolatingDoubleTreeMap hangmap_ ; - public OneHoodTuning(double h, double[] d, double[] v) { + + public OneHoodTuning(double h, double[] d, double[] v, double[] ht) { hood_ = h ; - map_ = new InterpolatingDoubleTreeMap() ; + velmap_ = new InterpolatingDoubleTreeMap() ; + hangmap_ = new InterpolatingDoubleTreeMap() ; for(var i = 0 ; i < d.length ; i++) { - map_.put(d[i], v[i]) ; + velmap_.put(d[i], v[i]) ; + hangmap_.put(d[i], ht[i]) ; } min_dist_ = d[0] ; @@ -97,7 +105,9 @@ private void readTuningData(String filename) { JsonNode pt = pointsArray.get(i) ; double distance = pt.get("distance").asDouble() ; double velocity = pt.get("velocity").asDouble() ; - values[i] = new OneTuningValue(distance, velocity) ; + double hangtime = pt.get("hangtime").asDouble() ; + + values[i] = new OneTuningValue(distance, velocity, hangtime) ; } addOneHood(hood, values) ; @@ -111,9 +121,10 @@ private void readTuningData(String filename) { public ShooterParams getShooterParams(double dist) { int h = getHoodIndex(dist) ; - var ret = new ShooterParams(dist, settings_.get(h).hood_, settings_.get(h).map_.get(dist)) ; + var ret = new ShooterParams(dist, settings_.get(h).hood_, settings_.get(h).velmap_.get(dist), settings_.get(h).hangmap_.get(dist)) ; Logger.recordOutput("ShooterTuning/hood", ret.hood); Logger.recordOutput("ShooterTuning/velocity", ret.velocity); + Logger.recordOutput("ShooterTuning/hangtime", ret.hangtime); return ret ; } @@ -159,6 +170,7 @@ protected void addOneHood(double hood, OneTuningValue[] values) { double [] dist = new double[values.length] ; double [] vels = new double[values.length] ; + double [] hangtimes = new double[values.length] ; for(int i = 0 ; i < values.length ; i++) { var v = values[i] ; @@ -167,8 +179,9 @@ protected void addOneHood(double hood, OneTuningValue[] values) { } dist[i] = v.dist_ ; vels[i] = v.vel_ ; + hangtimes[i] = v.hangtime_; } - settings_.add(new OneHoodTuning(hood, dist, vels)); + settings_.add(new OneHoodTuning(hood, dist, vels, hangtimes)); settings_.sort((s1, s2) -> { return Double.compare(s1.hood_, s2.hood_); diff --git a/src/main/java/frc/robot/util/VirtualTarget.java b/src/main/java/frc/robot/util/VirtualTarget.java index 01eab2fd..26e54bae 100644 --- a/src/main/java/frc/robot/util/VirtualTarget.java +++ b/src/main/java/frc/robot/util/VirtualTarget.java @@ -8,10 +8,10 @@ import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.shooter.tunings.ShooterTuning; +import frc.robot.subsystems.shooter.ShooterTuning; public class VirtualTarget { - public static Translation2d getVirtualTargetFromTarget(Drive drive, Translation2d target, ShooterTuning tuning, double loopPasses){ + public static Translation2d getVirtualTargetFromTarget(Drive drive, Translation2d target, ShooterTuning tuning, int loopPasses){ Distance distToHub; if(loopPasses == 0){ distToHub = Meters.of(target.getDistance(drive.getPose().getTranslation())); From 404c100508b2a878e134ebc366ef9c968dc799b5 Mon Sep 17 00:00:00 2001 From: Michael Date: Wed, 4 Mar 2026 12:37:57 -0800 Subject: [PATCH 37/48] add tag loaded back --- src/main/java/frc/robot/RobotContainer.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7fbb89ae..27e526fc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; import frc.robot.Constants.RobotType; import frc.robot.commands.drive.DriveCommands; @@ -226,6 +227,10 @@ public RobotContainer() { if (hopper_ == null) { hopper_ = new Hopper(new HopperIO() {}); } + + for (var tag : FieldConstants.layout.getTags()) { + System.out.println("Tag Loaded: " + tag.ID); + } DriveCommands.configure( drivebase_, @@ -303,7 +308,7 @@ private void configureBindings() { //CHANGE BACK TO RIGHT TRIGGER! // we might want to limit the acceleration on this while shooting, but idk how to do that and hopefully it wont matter too much. // just realized we could interrupt this with POV driving, but we would still be shooting, so we might want to create a block for that, but this too probably wont come up that much and i think i am not that numb-skulled to actually do this so idk - gamepad_.rightTrigger().whileTrue( + gamepad_.rightTrigger().or(operatorGamepad_.rightTrigger()).whileTrue( RobotCommands.shoot(shooter_, hopper_, drivebase_, intake_, gamepad_, Constants.shootOnMove) ); // shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose)); From 9290275bc6cf7a879c7695e5b989ccf700108b6e Mon Sep 17 00:00:00 2001 From: Michael Date: Wed, 4 Mar 2026 20:28:59 -0800 Subject: [PATCH 38/48] added different max speed when ferrying --- src/main/java/frc/robot/Constants.java | 4 ++-- .../robot/commands/drive/DriveCommands.java | 22 +++++++++++++++---- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e7a72ab0..977b113e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -49,8 +49,8 @@ public final class Constants { public static final boolean shootOnMove = true; // if true, we will allow the driver to shoot while moving, but with reduced max speed. if false, we will not allow the driver to shoot while moving. //change to 0 if it really doesnt work, bc the db velocity will go to 0 and the target will just be the hub //but I think it will work so yeah trust me butch - public static final double shootOnMoveMaxSpeed = 2.0/5.0; - public static final double aimOnMoveMaxSpeed = 2.0/3.0; // obsolete rn, but change if we ever add a aim mode again + public static final double shootOnMoveMaxSpeed = 0.4; + public static final double ferryOnMoveMaxSpeed = 0.8; public static class DriveConstants { public static final double slowModeJoystickMultiplier = 0.4; diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index ec6e88d6..78ca1733 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -244,11 +244,11 @@ public static Command joystickDriveAtAngle( .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } - public static Command pointAtTarget(Drive drive, CommandXboxController gamepad, Supplier target, boolean shootOnMove){ + public static Command pointAtTarget(Drive drive, CommandXboxController gamepad, Supplier target, double gamepadMaxSpeed, boolean shootOnMove){ return joystickDriveAtAngle( drive, - () -> shootOnMove ? -gamepad.getLeftY() * Constants.shootOnMoveMaxSpeed : 0.0, - () -> shootOnMove ? -gamepad.getLeftX() * Constants.shootOnMoveMaxSpeed : 0.0, + () -> shootOnMove ? -gamepad.getLeftY() * gamepadMaxSpeed : 0.0, + () -> shootOnMove ? -gamepad.getLeftX() * gamepadMaxSpeed : 0.0, () -> { Logger.recordOutput("Drive/Target", target.get()); var translation = target.get().minus(drive.getPose().getTranslation()); @@ -278,10 +278,24 @@ public static Translation2d getTarget(Drive drive) { return target; } + public static double getGamepadMaxSpeed(Drive drive){ + boolean blueDS = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue; + + boolean robotInAllianceZone = blueDS ? + drive.getPose().getMeasureX().lt(DriveConstants.allianceZoneBlue): + drive.getPose().getMeasureX().gt(DriveConstants.allianceZoneRed); + + if(robotInAllianceZone){ + return Constants.shootOnMoveMaxSpeed; + }else{ + return Constants.ferryOnMoveMaxSpeed; + } + } + // make sure this isnt bad with intellisense later public static Command pointAtShootingTarget(Drive drive, Shooter shooter, CommandXboxController gamepad, boolean shootOnMove){ - return pointAtTarget(drive, gamepad, () -> (shootOnMove ? VirtualTarget.getVirtualTargetFromTarget(drive, getTarget(drive), shooter.getTuning(), ShooterConstants.hangtimeLoopPasses) : getTarget(drive)), shootOnMove); + return pointAtTarget(drive, gamepad, () -> (shootOnMove ? VirtualTarget.getVirtualTargetFromTarget(drive, getTarget(drive), shooter.getTuning(), ShooterConstants.hangtimeLoopPasses) : getTarget(drive)), getGamepadMaxSpeed(drive), shootOnMove); } /** From 224a0a6d287eb86c428ec7b7bfb06e2f8439a72c Mon Sep 17 00:00:00 2001 From: Michael Date: Thu, 5 Mar 2026 16:36:28 -0800 Subject: [PATCH 39/48] get rid of flat tuning --- src/main/deploy/tuning/flat.json | 80 -------------------------------- 1 file changed, 80 deletions(-) delete mode 100644 src/main/deploy/tuning/flat.json diff --git a/src/main/deploy/tuning/flat.json b/src/main/deploy/tuning/flat.json deleted file mode 100644 index 37288fb8..00000000 --- a/src/main/deploy/tuning/flat.json +++ /dev/null @@ -1,80 +0,0 @@ -{ - "name" : "flat", - "data" : [ - { - "hood" : 5.0, - "points" : [ - { - // Test mode - "distance" : 1.616, - "velocity" : 55.0, - "hangtime" : 1.8 - }, - { - // Test mode - "distance" : 1.86, - "velocity" : 58.0, - "hangtime" : 1.8 - }, - { - // Test mode - "distance" : 2.18, - "velocity" : 65.0, - "hangtime" : 1.8 - } - ] - }, - { - "hood" : 35.0, - "points" : [ - { - // Test mode - "distance" : 2.47, - "velocity" : 62.0, - "hangtime" : 1.8 - }, - { - // Test mode - "distance" : 2.77, - "velocity" : 64.0, - "hangtime" : 1.8 - }, - { - // Test mode - "distance" : 3.06, - "velocity" : 68.0, - "hangtime" : 1.8 - }, - { - // Test mode - "distance" : 3.35, - "velocity" : 73.0, - "hangtime" : 1.8 - } - ] - }, - { - "hood" : 45.0, - "points" : [ - { - // Test mode - "distance" : 4.0, - "velocity" : 58.0, - "hangtime" : 1.8 - }, - { - // Test mode - "distance" : 4.31, - "velocity" : 61.0, - "hangtime" : 1.8 - }, - { - // Test mode - "distance" : 4.93, - "velocity" : 66.0, - "hangtime" : 1.8 - } - ] - } - ] -} From 6cd29106043cc811f79143b6d930aab2c757a787 Mon Sep 17 00:00:00 2001 From: Michael Date: Thu, 5 Mar 2026 17:00:46 -0800 Subject: [PATCH 40/48] updated shoot command to stop shooting when angle isn't correct, and removed unused commands --- .../robot/commands/robot/RobotCommands.java | 79 ++----------------- .../subsystems/shooter/ShooterConstants.java | 2 +- 2 files changed, 6 insertions(+), 75 deletions(-) diff --git a/src/main/java/frc/robot/commands/robot/RobotCommands.java b/src/main/java/frc/robot/commands/robot/RobotCommands.java index 760b1339..29314f14 100644 --- a/src/main/java/frc/robot/commands/robot/RobotCommands.java +++ b/src/main/java/frc/robot/commands/robot/RobotCommands.java @@ -1,19 +1,8 @@ package frc.robot.commands.robot; -import static edu.wpi.first.units.Units.Meters; - -import java.util.function.BooleanSupplier; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.RobotState; import frc.robot.commands.drive.DriveCommands; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.hopper.Hopper; @@ -22,79 +11,21 @@ import frc.robot.subsystems.shooter.ShooterConstants; public class RobotCommands { - /** - * Shoots into the hub. - * @param shooter - * @param hopper - * @param drive - * @return - */ - private static Command shootHub(Shooter shooter, Hopper hopper, IntakeSubsystem intake, Drive drive) { - BooleanSupplier aimedAtHub = - () -> drive.rotationIsNear(RobotState.rotationToHub(), ShooterConstants.aimingTolerance); - - return Commands.parallel( - DriveCommands.joystickDriveAtAngle( - drive, - () -> 0, - () -> 0, - () -> RobotState.rotationToHub() - ).finallyDo(drive::stopWithX), - Commands.repeatingSequence( - Commands.waitUntil(aimedAtHub), - shooter.shootAtDistance(RobotState::hubDistance, hopper, intake) - .until(() -> !aimedAtHub.getAsBoolean()) - ) - ); - } - - /** - * Ferrys into a target in your alliance zone. Allows movement. - * @param shooter - * @param hopper - * @param drive - * @return - */ - private static Command ferry(Shooter shooter, Hopper hopper, IntakeSubsystem intake, Drive drive) { - Translation2d rightTarget = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueTargetRight - : ShooterConstants.Positions.redTargetRight; - - Translation2d leftTarget = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? ShooterConstants.Positions.blueTargetLeft - : ShooterConstants.Positions.redTargetLeft; - - Supplier target = - () -> drive.getPose().getY() < ShooterConstants.Positions.centerLineY - ? rightTarget - : leftTarget; - - Supplier targetDistance = - () -> Meters.of(target.get().getDistance(drive.getPose().getTranslation())); - - Supplier targetingAngle = - () -> { - var botToHub = target.get().minus(drive.getPose().getTranslation()); - return new Rotation2d(botToHub.getX(), botToHub.getY()); - }; - - return DriveCommands.joystickDriveAtAngle(targetingAngle) - .alongWith(shooter.shootAtDistance(targetDistance, hopper, intake)); - } - /** * Shoots at either the hub, or ferrying targets based on the current robot position. * @param shooter * @param hopper * @param drive + * @param intake + * @param gamepad + * @param shootOnMove + * * @return */ public static Command shoot(Shooter shooter, Hopper hopper, Drive drive, IntakeSubsystem intake, CommandXboxController gamepad, boolean shootOnMove) { return Commands.parallel( DriveCommands.pointAtShootingTarget(drive, shooter, gamepad, shootOnMove), - Commands.sequence( + Commands.repeatingSequence( Commands.waitUntil(() -> drive.rotationIsNear(drive.getVirtualTarget(shooter).minus(drive.getPose().getTranslation()).getAngle(), ShooterConstants.aimingTolerance)), shooter.shoot(drive, hopper, intake, gamepad, shootOnMove) ) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index c8e9f890..123147da 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -32,7 +32,7 @@ public class ShooterConstants { public static final AngularVelocity shooterTolerance = RotationsPerSecond.of(0.1); public static final AngularVelocity ejectVelocity = RotationsPerSecond.of(-20); - public static final Angle aimingTolerance = Degrees.of(5); + public static final Angle aimingTolerance = Degrees.of(8.0); public static final Distance allowedTrenchDistance = Meters.of(1.0); From 1ad4c1a72fbc1e25c6ee2012d773417985cbd0e4 Mon Sep 17 00:00:00 2001 From: Michael Date: Fri, 6 Mar 2026 15:27:03 -0800 Subject: [PATCH 41/48] fixed idiot merge conflicts --- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/commands/AutoCommands.java | 4 ++-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd611613..815e850b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -63,7 +63,6 @@ import frc.robot.subsystems.vision.CameraIO; import frc.robot.subsystems.vision.CameraIOLimelight4; import frc.robot.subsystems.vision.CameraIOPhotonSim; -import frc.robot.subsystems.vision.PoseEstimateConsumer; import frc.robot.subsystems.vision.VisionConstants; import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index 0b0bbbda..1e695f0a 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -21,11 +21,11 @@ public static Command trenchToTrench(Drive drive, IntakeSubsystem intake, Hopper ), // Added: timeout on shoot - RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(5.0)), + RobotCommands.shoot(shooter, hopper, drive, intake, null, false).withTimeout(Seconds.of(5.0)), DriveCommands.followPathCommand("BottomToTop", mirroredX).deadlineFor(intake.intakeSequence()), // Added: timeout on shoot - RobotCommands.shoot(shooter, hopper, intake, drive, null, false).withTimeout(Seconds.of(5.0)) + RobotCommands.shoot(shooter, hopper, drive, intake, null, false).withTimeout(Seconds.of(5.0)) ); } } \ No newline at end of file From ecc0f836b6c28ada5b33a3a412b4b727e2170d22 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Wed, 11 Mar 2026 13:54:46 -0700 Subject: [PATCH 42/48] Reorganize Shoot on the Move --- src/main/java/frc/robot/RobotContainer.java | 29 +--- src/main/java/frc/robot/RobotState.java | 155 +++++++++++++++--- .../java/frc/robot/commands/AutoCommands.java | 13 +- .../robot/commands/drive/DriveCommands.java | 50 +----- .../robot/commands/robot/RobotCommands.java | 73 +++++++-- .../frc/robot/subsystems/drive/Drive.java | 10 -- .../frc/robot/subsystems/shooter/Shooter.java | 13 +- .../java/frc/robot/util/VirtualTarget.java | 46 ++++-- 8 files changed, 246 insertions(+), 143 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3375d586..e9595c57 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -229,7 +229,11 @@ public RobotContainer() { () -> -gamepad_.getRightX() ); - RobotState.initialize(drivebase_::getPose); + RobotState.initialize( + drivebase_::getPose, + drivebase_::getFieldChassisSpeeds, + shooter_::getTuning + ); // Initialize the visualizers. Mechanism3d.measured.zero(); @@ -287,32 +291,17 @@ private void configureBindings() { operatorGamepad_.b().whileTrue(RobotCommands.ejectUp(shooter_, hopper_)); // While the right trigger is held, we will shoot into the hub or ferry. - // When the hopper isnt shooting, set it to run its idle velocity. - // hopper_.setDefaultCommand(hopper_.idleScrambler()); - - // while in alliance zone, point drivebase at virtual target, but still allow translational driving - //lowkey we are not gonna need this but like maybe so idk imma just keep it - - // While the right trigger is held, we will shoot into the hub. - // i wish i knew if this shoot command would interrrupt the other aim command, so this is kind of a "just in case", but if it is unnesecary it will be removed. - //CHANGE BACK TO RIGHT TRIGGER! - // we might want to limit the acceleration on this while shooting, but idk how to do that and hopefully it wont matter too much. - // just realized we could interrupt this with POV driving, but we would still be shooting, so we might want to create a block for that, but this too probably wont come up that much and i think i am not that numb-skulled to actually do this so idk gamepad_.rightTrigger().or(operatorGamepad_.rightTrigger()).whileTrue( - RobotCommands.shoot(shooter_, hopper_, drivebase_, intake_, gamepad_, Constants.shootOnMove, gamepad_.a().or(operatorGamepad_.a())) + RobotCommands.shoot(shooter_, hopper_, intake_, drivebase_, gamepad_.a().or(operatorGamepad_.a())) ); - // When the shooter isnt shooting, get it ready to shoot. - shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose, () -> drivebase_.getVirtualTarget(shooter_))); - - // When the shooter isnt shooting, get it ready to shoot. - shooter_.setDefaultCommand(shooter_.idleCommand()); //While the X button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it. - operatorGamepad_.x().whileTrue(intake_.hopperEjectSequence().alongWith(hopper_.reverseFeed())); - operatorGamepad_.y().whileTrue(RobotCommands.ejectUp(shooter_, hopper_)); + // When the shooter isnt shooting, stow it. + shooter_.setDefaultCommand(shooter_.idleCommand()); + // Bind dashboard button to refreshing the tuning. var refreshTuningButton = new LoggedNetworkBoolean("/Tuning/RefreshTuning", false); new Trigger(refreshTuningButton::get) diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 2ebd84c4..cb2436be 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -4,37 +4,60 @@ import java.util.function.Supplier; -import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.shooter.ShooterTuning; import frc.robot.util.LoggedTracer; +import frc.robot.util.VirtualTarget; /** * Object that calculates robot-wide values that change periodically, * for example, distance to a field element. */ public class RobotState { - private static Supplier pose = () -> Pose2d.kZero; + private static Supplier pose = null; + private static Supplier speed = null; + private static Supplier tuning = null; - @AutoLogOutput - private static Distance hubDistance = Meters.zero(); + private static boolean updatedRealHub = false; + private static boolean updatedVirtualHub = false; + private static boolean updatedVirtualFerry = false; + + private static Pose2d currentPose = Pose2d.kZero; + private static Translation2d hubTranslation = Translation2d.kZero; + private static Distance allianceWall = Meters.zero(); - private static Rotation2d rotationToHub = Rotation2d.kZero; private static boolean inAllianceZone = false; + private static Distance hubDistance = Meters.zero(); + private static Rotation2d rotationToHub = Rotation2d.kZero; + + private static Distance virtualHubDistance = Meters.zero(); + private static Rotation2d rotationToVirtualHub = Rotation2d.kZero; + + private static Distance virtualFerryDistance = Meters.zero(); + private static Rotation2d rotationToVirtualFerry = Rotation2d.kZero; + /** * This method supplies the object with the information it needs for its calculations. * @param poseSupplier */ - public static void initialize(Supplier poseSupplier) { + public static void initialize( + Supplier poseSupplier, + Supplier speedSupplier, + Supplier tuningSupplier + ) { pose = poseSupplier; + speed = speedSupplier; + tuning = tuningSupplier; } /** @@ -42,42 +65,134 @@ public static void initialize(Supplier poseSupplier) { */ public static void periodic() { LoggedTracer.reset(); - - Pose2d currentPose = pose.get(); - Translation2d hubTranslation = + // If not initialized, do nothing. + if ( + pose == null || + speed == null || + tuning == null + ) return; + + updatedRealHub = false; + updatedVirtualHub = false; + updatedVirtualFerry = false; + + currentPose = pose.get(); + + hubTranslation = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? ShooterConstants.Positions.blueHubPose : ShooterConstants.Positions.redHubPose; - - hubDistance = Meters.of(pose.get().getTranslation().getDistance(hubTranslation)); - Logger.recordOutput("RobotState/hubDistance", hubDistance) ; - Translation2d translationToHub = hubTranslation.minus(currentPose.getTranslation()); - - rotationToHub = new Rotation2d(translationToHub.getX(), translationToHub.getY()); - - Distance allianceWall = + allianceWall = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? ShooterConstants.Positions.blueAllianceWall : ShooterConstants.Positions.redAllianceWall; inAllianceZone = currentPose.getMeasureX().minus(allianceWall).abs(Meters) < ShooterConstants.Positions.spinUpZone.in(Meters); - LoggedTracer.record("RobotState"); + LoggedTracer.record("RobotState/InAllianceZone"); + + // Log Outputs + Logger.recordOutput("RobotState/HubDistance", hubDistance); + Logger.recordOutput("RobotState/VirtualHubDistance", virtualHubDistance); + } + + private static void updateRealHub() { + LoggedTracer.reset(); + + hubDistance = Meters.of(currentPose.getTranslation().getDistance(hubTranslation)); + + Translation2d translationToHub = hubTranslation.minus(currentPose.getTranslation()); + + rotationToHub = new Rotation2d(translationToHub.getX(), translationToHub.getY()); + + updatedRealHub = true; + + LoggedTracer.record("RobotState/UpdateRealHub"); + } + + private static void updateVirtualHub() { + LoggedTracer.reset(); + + Pose2d currentPose = pose.get(); + + Translation2d virtualHub = + VirtualTarget.getVirtualTargetFromTarget(currentPose, speed.get(), hubTranslation, tuning.get(), ShooterConstants.hangtimeLoopPasses); + + virtualHubDistance = Meters.of(currentPose.getTranslation().getDistance(virtualHub)); + + Translation2d translationToTarget = virtualHub.minus(currentPose.getTranslation()); + rotationToVirtualHub = new Rotation2d(translationToTarget.getX(), translationToTarget.getY()); + + Logger.recordOutput("RobotState/VirtualHub", virtualHub); + + updatedVirtualHub = true; + + LoggedTracer.record("RobotState/UpdateVirtualHub"); + } + + private static void updateVirtualFerry() { + LoggedTracer.reset(); + + Translation2d rightTarget = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueTargetRight + : ShooterConstants.Positions.redTargetRight; + + Translation2d leftTarget = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueTargetLeft + : ShooterConstants.Positions.redTargetLeft; + + Translation2d realTarget = currentPose.getY() < ShooterConstants.Positions.centerLineY + ? rightTarget + : leftTarget; + + Translation2d virtualTarget = + VirtualTarget.getVirtualTargetFromTarget(currentPose, speed.get(), realTarget, tuning.get(), ShooterConstants.hangtimeLoopPasses); + + Translation2d botToTarget = virtualTarget.minus(currentPose.getTranslation()); + + virtualFerryDistance = Meters.of(virtualTarget.getDistance(currentPose.getTranslation())); + rotationToVirtualFerry = new Rotation2d(botToTarget.getX(), botToTarget.getY()); + + updatedVirtualFerry = true; + + LoggedTracer.record("RobotState/UpdateVirtualFerry"); } public static Distance hubDistance() { + if (!updatedRealHub) updateRealHub(); return hubDistance; } - @AutoLogOutput public static Rotation2d rotationToHub() { + if (!updatedRealHub) updateRealHub(); return rotationToHub; } - @AutoLogOutput + public static Distance virtualHubDistance() { + if (!updatedVirtualHub) updateVirtualHub(); + return virtualHubDistance; + } + + public static Rotation2d rotationToVirtualHub() { + if (!updatedVirtualHub) updateVirtualHub(); + return rotationToVirtualHub; + } + + public static Distance virtualFerryDistance() { + if (!updatedVirtualFerry) updateVirtualFerry(); + return virtualFerryDistance; + } + + public static Rotation2d rotationToVirtualFerry() { + if (!updatedVirtualFerry) updateVirtualFerry(); + return rotationToVirtualFerry; + } + public static boolean inAllianceZone() { return inAllianceZone; } diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index 2ed451c7..84d9f1f4 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.drive.DriveCommands; import frc.robot.commands.robot.RobotCommands; import frc.robot.commands.robot.StartupCmd; @@ -23,12 +22,12 @@ public static Command a1TrenchToTrench(Drive drive, IntakeSubsystem intake, Hopp ), // Added: timeout on shoot - RobotCommands.shoot(shooter, hopper, drive, intake, null, false, new Trigger(() -> false)).withTimeout(Seconds.of(5.0)), + RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(5.0)), DriveCommands.followPathCommand("A1_BottomToTopTrench", mirroredX) .deadlineFor(RobotCommands.intake(intake, hopper)), // Added: timeout on shoot - RobotCommands.shoot(shooter, hopper, drive, intake, null, false, new Trigger(() -> false)).withTimeout(Seconds.of(5.0)) + RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(5.0)) ); } @@ -44,7 +43,7 @@ public static Command a2NZCollectAuto(Drive drive, Shooter shooter, IntakeSubsys shooter.spinUpForDistanceHoodParked(() -> Meters.of(3.7)), hopper.preShoot() ), - RobotCommands.shoot(shooter, hopper, drive, intake, null, false, new Trigger(() -> false)).withTimeout(Seconds.of(3.8)), + RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(3.8)), //Drive back into the neutral zone, collecting more balls along the way DriveCommands.followPathCommand("a2Shoot1toHub", mirroredX) @@ -55,7 +54,7 @@ public static Command a2NZCollectAuto(Drive drive, Shooter shooter, IntakeSubsys shooter.spinUpForDistanceHoodParked(() -> Meters.of(3.7)), hopper.preShoot() ), - RobotCommands.shoot(shooter, hopper, drive, intake, null, false, new Trigger(() -> false)).withTimeout(Seconds.of(4.0)) + RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(4.0)) ); } @@ -68,14 +67,14 @@ public static Command a3DepotAuto(Drive drive, Shooter shooter, IntakeSubsystem new StartupCmd(intake, hopper, shooter).beforeStarting(Commands.waitTime(Seconds.of(0.6))) ), - RobotCommands.shoot(shooter, hopper, drive, intake, null, false, new Trigger(() -> false)).withTimeout(Seconds.of(5.0)), + RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(5.0)), DriveCommands.followPathCommand("a3DepotToOutpost") .deadlineFor(RobotCommands.intake(intake, hopper)), DriveCommands.followPathCommand("a3OutpostToShoot"), - RobotCommands.shoot(shooter, hopper, drive, intake, null, false, new Trigger(() -> false)).withTimeout(Seconds.of(8.0)) + RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(8.0)) ); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index de78025e..3c0ebdcd 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -243,56 +243,8 @@ public static Command joystickDriveAtAngle( // Reset PID controller when command starts .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } - - public static Command pointAtTarget(Drive drive, CommandXboxController gamepad, Supplier target, double gamepadMaxSpeed, boolean shootOnMove){ - return joystickDriveAtAngle( - drive, - () -> shootOnMove ? -gamepad.getLeftY() * gamepadMaxSpeed : 0.0, - () -> shootOnMove ? -gamepad.getLeftX() * gamepadMaxSpeed : 0.0, - () -> { - Logger.recordOutput("Drive/Target", target.get()); - var translation = target.get().minus(drive.getPose().getTranslation()); - var rotation = new Rotation2d(translation.getX(), translation.getY()); - - return rotation; - }); - } - - public static Translation2d getTarget(Drive drive) { - Translation2d target; - - boolean blueDS = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue; - - boolean robotInAllianceZone = blueDS ? - drive.getPose().getMeasureX().lt(DriveConstants.allianceZoneBlue): - drive.getPose().getMeasureX().gt(DriveConstants.allianceZoneRed); - - if(robotInAllianceZone){ - target = DriveConstants.getHubTranslation(DriverStation.getAlliance().orElse(Alliance.Blue)); - }else if(drive.getPose().getMeasureY().gt(DriveConstants.fieldWidth.div(2.0))){ - target = blueDS ? DriveConstants.blueLeftFerryTarget : DriveConstants.redLeftFerryTarget ; - }else{ - target = blueDS ? DriveConstants.blueRightFerryTarget : DriveConstants.redRightFerryTarget ; - } - - return target; - } - - public static double getGamepadMaxSpeed(Drive drive){ - boolean blueDS = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue; - - boolean robotInAllianceZone = blueDS ? - drive.getPose().getMeasureX().lt(DriveConstants.allianceZoneBlue): - drive.getPose().getMeasureX().gt(DriveConstants.allianceZoneRed); - - if(robotInAllianceZone){ - return Constants.shootOnMoveMaxSpeed; - }else{ - return Constants.ferryOnMoveMaxSpeed; - } - } - // make sure this isnt bad with intellisense later + @Deprecated public static Command pointAtShootingTarget(Drive drive, Shooter shooter, CommandXboxController gamepad, boolean shootOnMove){ return pointAtTarget(drive, gamepad, () -> (shootOnMove ? VirtualTarget.getVirtualTargetFromTarget(drive, getTarget(drive), shooter.getTuning(), ShooterConstants.hangtimeLoopPasses) : getTarget(drive)), getGamepadMaxSpeed(drive), shootOnMove); diff --git a/src/main/java/frc/robot/commands/robot/RobotCommands.java b/src/main/java/frc/robot/commands/robot/RobotCommands.java index 84acbf89..7989df24 100644 --- a/src/main/java/frc/robot/commands/robot/RobotCommands.java +++ b/src/main/java/frc/robot/commands/robot/RobotCommands.java @@ -1,9 +1,13 @@ package frc.robot.commands.robot; +import java.util.function.BooleanSupplier; + +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.RobotState; import frc.robot.commands.drive.DriveCommands; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.hopper.Hopper; @@ -13,24 +17,67 @@ public class RobotCommands { /** - * Shoots at either the hub, or ferrying targets based on the current robot position. + * Shoots into the hub. * @param shooter * @param hopper * @param drive - * @param intake - * @param gamepad - * @param shootOnMove - * * @return */ - public static Command shoot(Shooter shooter, Hopper hopper, Drive drive, IntakeSubsystem intake, CommandXboxController gamepad, boolean shootOnMove, Trigger shakeTrigger) { + private static Command shootHub(Shooter shooter, Hopper hopper, IntakeSubsystem intake, Drive drive, Trigger shakeTrigger) { + BooleanSupplier aimedAtHub = + () -> drive.rotationIsNear(RobotState.rotationToVirtualHub(), ShooterConstants.aimingTolerance); + return Commands.parallel( - DriveCommands.pointAtShootingTarget(drive, shooter, gamepad, shootOnMove), - Commands.repeatingSequence( - Commands.waitUntil(() -> drive.rotationIsNear(drive.getVirtualTarget(shooter).minus(drive.getPose().getTranslation()).getAngle(), ShooterConstants.aimingTolerance)), - shooter.shoot(drive, hopper, intake, shakeTrigger) - ) - ); + DriveCommands.joystickDriveAtAngle(RobotState::rotationToVirtualHub), + Commands.repeatingSequence( + Commands.waitUntil(aimedAtHub).deadlineFor(shooter.spinUpForDistance(RobotState::hubDistance)), + shooter.shootAtDistance(RobotState::virtualHubDistance, hopper, intake, shakeTrigger) + .until(() -> !aimedAtHub.getAsBoolean()) + ) + ); + } + + /** + * Ferrys into a target in your alliance zone. Allows movement. + * @param shooter + * @param hopper + * @param drive + * @return + */ + private static Command ferry(Shooter shooter, Hopper hopper, IntakeSubsystem intake, Drive drive, Trigger shakeTrigger) { + return DriveCommands.joystickDriveAtAngle(RobotState::rotationToVirtualFerry) + .alongWith( + shooter.shootAtDistance(RobotState::virtualFerryDistance, hopper, intake, shakeTrigger), + Commands.runOnce(() -> Logger.recordOutput("Ferry/IsFerrying", true)) + ) + .finallyDo(i -> Logger.recordOutput("Ferry/IsFerrying", false)); + } + + /** + * Shoots at either the hub, or ferrying targets based on the current robot position. + * @param shooter + * @param hopper + * @param drive + * @param shakeTrigger + * @return + */ + public static Command shoot(Shooter shooter, Hopper hopper, IntakeSubsystem intake, Drive drive, Trigger shakeTrigger) { + return Commands.either( + shootHub(shooter, hopper, intake, drive, shakeTrigger), + ferry(shooter, hopper, intake, drive, shakeTrigger), + RobotState::inAllianceZone + ); + } + + /** + * Shoots at either the hub, or ferrying targets based on the current robot position. + * @param shooter + * @param hopper + * @param drive + * @return + */ + public static Command shoot(Shooter shooter, Hopper hopper, IntakeSubsystem intake, Drive drive) { + return shoot(shooter, hopper, intake, drive, new Trigger(() -> false)); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 948e32b0..5f7dcf25 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -62,12 +62,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; -import frc.robot.commands.drive.DriveCommands; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.generated.CompTunerConstants; import frc.robot.util.LocalADStarAK; -import frc.robot.util.VirtualTarget; import frc.robot.util.LoggedTracer; public class Drive extends SubsystemBase { @@ -478,12 +474,6 @@ public double getMaxAngularSpeedRadPerSec() { public RobotConfig getPathplannerConfig() { return PP_CONFIG; } - - - - public Translation2d getVirtualTarget(Shooter shooter) { - return VirtualTarget.getVirtualTargetFromTarget(this, DriveCommands.getTarget(this), shooter.getTuning(), ShooterConstants.hangtimeLoopPasses); - } /** Returns an array of module translations. */ public Translation2d[] getModuleTranslations() { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 5ca5eddf..d1cc05d1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -42,14 +42,13 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; import frc.robot.RobotState; -import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.hopper.Hopper; import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.shooter.ShooterConstants.Positions.HubDistance; import frc.robot.subsystems.shooter.ShooterTuning.ShooterParams; import frc.robot.util.LoggedTracer; import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; -import frc.robot.subsystems.shooter.ShooterConstants.Positions.HubDistance; public class Shooter extends SubsystemBase { @@ -192,16 +191,6 @@ public Command reloadTunings() { }).ignoringDisable(true); } - /** - * Runs specified setpoints until the command ends, then stops. - * @param vel - * @param pos - * @return - */ - public Command shoot(Drive drive, Hopper hopper, IntakeSubsystem intake, Trigger shakeTrigger) { - return shootAtDistance(() -> Meters.of(drive.getVirtualTarget(this).getDistance(drive.getPose().getTranslation())), hopper, intake, shakeTrigger); - } - /** * 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, diff --git a/src/main/java/frc/robot/util/VirtualTarget.java b/src/main/java/frc/robot/util/VirtualTarget.java index 26e54bae..7a549c1e 100644 --- a/src/main/java/frc/robot/util/VirtualTarget.java +++ b/src/main/java/frc/robot/util/VirtualTarget.java @@ -4,28 +4,50 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Seconds; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; -import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.shooter.ShooterTuning; public class VirtualTarget { - public static Translation2d getVirtualTargetFromTarget(Drive drive, Translation2d target, ShooterTuning tuning, int loopPasses){ + /** + * Gets the translation of the virtual target based on a target, and the current drivebase velocity. + * + * In order to shoot, we need to find the tuning constants for a shot at the distance from the virtual target, + * which includes the hangtime of a shot of that distance. + * + * In order to calculate the virtual target, you need this hangtime which can only be found after the virtual target + * is calculated, in order to fix this, this function is recursive, and based on the number of loop passes, it will + * push the virtual target closer and closer to where it should be. + * + * This has been tested to work at slow velocities. + * + * @param drive + * @param target + * @param tuning + * @param loopPasses + * @return + */ + public static Translation2d getVirtualTargetFromTarget(Pose2d pose, ChassisSpeeds speeds, Translation2d target, ShooterTuning tuning, int loopPasses){ Distance distToHub; - if(loopPasses == 0){ - distToHub = Meters.of(target.getDistance(drive.getPose().getTranslation())); - }else{ - distToHub = Meters.of(getVirtualTargetFromTarget(drive, target, tuning, loopPasses - 1) - .getDistance(drive.getPose().getTranslation()) + + if(loopPasses == 0) { + distToHub = Meters.of(target.getDistance(pose.getTranslation())); + } else{ + distToHub = Meters.of( + getVirtualTargetFromTarget(pose, speeds, target, tuning, loopPasses - 1).getDistance(pose.getTranslation()) ); } + Time hangTime = Seconds.of(tuning.getShooterParams(distToHub.in(Meters)).hangtime); + return target.minus( - new Translation2d( - MetersPerSecond.of(drive.getFieldChassisSpeeds().vxMetersPerSecond).times(hangTime), - MetersPerSecond.of(drive.getFieldChassisSpeeds().vyMetersPerSecond).times(hangTime) - ) - ); + new Translation2d( + MetersPerSecond.of(speeds.vxMetersPerSecond).times(hangTime), + MetersPerSecond.of(speeds.vyMetersPerSecond).times(hangTime) + ) + ); } } From 3b8a109a8dd2ba9bc3483f966344690a8a71fc0c Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Wed, 11 Mar 2026 13:56:00 -0700 Subject: [PATCH 43/48] Remove cmd --- .../java/frc/robot/commands/drive/DriveCommands.java | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 3c0ebdcd..4155d330 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -58,16 +58,11 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; -import frc.robot.Constants.DriveConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.Mode; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.util.MapleSimUtil; -import frc.robot.util.VirtualTarget; public class DriveCommands { private static final double kStoppedVelocity = 0.15 ; @@ -244,12 +239,6 @@ public static Command joystickDriveAtAngle( .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } - @Deprecated - public static Command pointAtShootingTarget(Drive drive, Shooter shooter, CommandXboxController gamepad, boolean shootOnMove){ - - return pointAtTarget(drive, gamepad, () -> (shootOnMove ? VirtualTarget.getVirtualTargetFromTarget(drive, getTarget(drive), shooter.getTuning(), ShooterConstants.hangtimeLoopPasses) : getTarget(drive)), getGamepadMaxSpeed(drive), shootOnMove); - } - /** * Measures the velocity feedforward constants for the drive motors. * From 6852bb37c64d7178aa2307389e62b58984d5a1a5 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Wed, 11 Mar 2026 13:58:13 -0700 Subject: [PATCH 44/48] Revert --- src/main/java/frc/robot/commands/AutoCommands.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/Drive.java | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index 84d9f1f4..8a19fbc3 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -43,7 +43,7 @@ public static Command a2NZCollectAuto(Drive drive, Shooter shooter, IntakeSubsys shooter.spinUpForDistanceHoodParked(() -> Meters.of(3.7)), hopper.preShoot() ), - RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(3.8)), + RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(5.0)), //Drive back into the neutral zone, collecting more balls along the way DriveCommands.followPathCommand("a2Shoot1toHub", mirroredX) @@ -54,7 +54,7 @@ public static Command a2NZCollectAuto(Drive drive, Shooter shooter, IntakeSubsys shooter.spinUpForDistanceHoodParked(() -> Meters.of(3.7)), hopper.preShoot() ), - RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(4.0)) + RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(10.0)) ); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 5f7dcf25..8577488e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -222,8 +222,10 @@ public void periodic() { for (var module : modules) { module.stop(); } - - // Log empty setpoint states when disabled + } + + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } From d1071cd3046e10fd328344966d47815ad80a2183 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Wed, 11 Mar 2026 13:59:31 -0700 Subject: [PATCH 45/48] Clean --- src/main/java/frc/robot/commands/AutoCommands.java | 2 ++ src/main/java/frc/robot/subsystems/drive/Drive.java | 2 +- src/main/java/frc/robot/subsystems/hopper/Hopper.java | 3 --- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index 8a19fbc3..23f76cad 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -43,6 +43,7 @@ public static Command a2NZCollectAuto(Drive drive, Shooter shooter, IntakeSubsys shooter.spinUpForDistanceHoodParked(() -> Meters.of(3.7)), hopper.preShoot() ), + RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(5.0)), //Drive back into the neutral zone, collecting more balls along the way @@ -54,6 +55,7 @@ public static Command a2NZCollectAuto(Drive drive, Shooter shooter, IntakeSubsys shooter.spinUpForDistanceHoodParked(() -> Meters.of(3.7)), hopper.preShoot() ), + RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(10.0)) ); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 8577488e..472c2182 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -223,7 +223,7 @@ public void periodic() { module.stop(); } } - + // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index 81c1b508..599a9091 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -167,9 +167,6 @@ public Command dynamicFeed(Supplier feeder, Supplier Date: Wed, 11 Mar 2026 14:11:52 -0700 Subject: [PATCH 46/48] CLeanup --- src/main/java/frc/robot/Constants.java | 24 ----- src/main/java/frc/robot/RobotContainer.java | 10 +- .../java/frc/robot/commands/AutoCommands.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 1 + .../frc/robot/subsystems/shooter/Shooter.java | 65 +----------- .../subsystems/shooter/ShooterConstants.java | 100 +++++++----------- .../subsystems/vision/AprilTagVision.java | 9 -- 7 files changed, 45 insertions(+), 166 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 977b113e..bd66876d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,20 +13,14 @@ package frc.robot; -import static edu.wpi.first.units.Units.Inches; - 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.math.geometry.Translation2d; -import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.RobotBase; -import frc.robot.subsystems.shooter.ShooterConstants; /** * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running @@ -54,24 +48,6 @@ public final class Constants { public static class DriveConstants { public static final double slowModeJoystickMultiplier = 0.4; - - public static final Distance fieldLength = Inches.of(651.22); - public static final Distance fieldWidth = Inches.of(317.69); - - public static final Distance allianceZoneBlue = Inches.of(156.61); - public static final Distance allianceZoneRed = fieldLength.minus(allianceZoneBlue); - - public static final Translation2d blueLeftFerryTarget = new Translation2d(allianceZoneBlue.div(2.0), fieldWidth.times(5.0/6.0)); - public static final Translation2d blueRightFerryTarget = new Translation2d(allianceZoneBlue.div(2.0), fieldWidth.times(1.0/6.0)); - public static final Translation2d redLeftFerryTarget = new Translation2d(fieldLength.minus(allianceZoneBlue.div(2.0)), fieldWidth.times(5.0/6.0)); - public static final Translation2d redRightFerryTarget = new Translation2d(fieldLength.minus(allianceZoneBlue.div(2.0)), fieldWidth.times(1.0/6.0)); - public static Translation2d getHubTranslation(Alliance alliance){ - return ( - alliance == Alliance.Blue - ? ShooterConstants.Positions.blueHubPose - : ShooterConstants.Positions.redHubPose - ); - } } public static class FieldConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e9595c57..f5b50346 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -218,6 +218,7 @@ public RobotContainer() { hopper_ = new Hopper(new HopperIO() {}); } + // Force Preload Static Apriltag Layout for (var tag : FieldConstants.layout.getTags()) { System.out.println("Tag Loaded: " + tag.ID); } @@ -288,14 +289,11 @@ private void configureBindings() { // While the left trigger is held, we will run the intake. If the intake is stowed, it will also deploy it. gamepad_.leftTrigger().or(operatorGamepad_.leftTrigger()).whileTrue(RobotCommands.intake(intake_, hopper_)); - operatorGamepad_.b().whileTrue(RobotCommands.ejectUp(shooter_, hopper_)); - // While the right trigger is held, we will shoot into the hub or ferry. - gamepad_.rightTrigger().or(operatorGamepad_.rightTrigger()).whileTrue( - RobotCommands.shoot(shooter_, hopper_, intake_, drivebase_, gamepad_.a().or(operatorGamepad_.a())) - ); + gamepad_.rightTrigger().or(operatorGamepad_.rightTrigger()) + .whileTrue(RobotCommands.shoot(shooter_, hopper_, intake_, drivebase_, gamepad_.a().or(operatorGamepad_.a()))); - //While the X button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it. + // While the X button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it. operatorGamepad_.x().whileTrue(intake_.hopperEjectSequence().alongWith(hopper_.reverseFeed())); operatorGamepad_.y().whileTrue(RobotCommands.ejectUp(shooter_, hopper_)); diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index 23f76cad..4d92bd64 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -55,7 +55,7 @@ public static Command a2NZCollectAuto(Drive drive, Shooter shooter, IntakeSubsys shooter.spinUpForDistanceHoodParked(() -> Meters.of(3.7)), hopper.preShoot() ), - + RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(10.0)) ); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 472c2182..9e30effd 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -12,6 +12,7 @@ // GNU General Public License for more details. package frc.robot.subsystems.drive; + import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d1cc05d1..5e770b4e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -20,8 +20,6 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -39,12 +37,9 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; 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.RobotState; import frc.robot.subsystems.hopper.Hopper; import frc.robot.subsystems.intake.IntakeSubsystem; -import frc.robot.subsystems.shooter.ShooterConstants.Positions.HubDistance; import frc.robot.subsystems.shooter.ShooterTuning.ShooterParams; import frc.robot.util.LoggedTracer; import frc.robot.util.MapleSimUtil; @@ -191,62 +186,6 @@ public Command reloadTunings() { }).ignoringDisable(true); } - /** - * 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, Supplier targetPose) { - 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(); - } - - Translation2d hubTranslation = targetPose.get(); - - Distance distanceToHub = Meters.of(pose.getTranslation().getDistance(hubTranslation)); - - switch(HubDistance.fromDistance(distanceToHub)) { - case LOW: - return Degrees.of(ShooterConstants.Positions.hoodLOW); - case MEDIUM: - return Degrees.of(ShooterConstants.Positions.hoodMEDIUM); - case HIGH: - return Degrees.of(ShooterConstants.Positions.hoodHIGH); - default: - return Degrees.of(ShooterConstants.Positions.hoodLOW); - - } - }); - } - public Command awaitShooting(Supplier robotPose) { - return runDynamicSetpoints( - // () -> RotationsPerSecond.of( - // RobotState.inAllianceZone() - // ? getTuning().getShooterParams(RobotState.hubDistance().in(Meters)).velocity - // : 0.0 - // ), - () -> RotationsPerSecond.of(0.0) , - () -> { - 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(); - } - - var params = getTuning().getShooterParams(RobotState.hubDistance().in(Meters)); - return Degrees.of(params.hood); - } - ); - } - public Command runToSetpointsCmd(AngularVelocity vel, Angle pos) { return startEnd(() -> setSetpoints(vel, pos), this::stopShooter).withName("Set Shooter Setpoints"); } @@ -271,9 +210,7 @@ public Command runVoltageCmd(Voltage vol) { public Command hoodToPosCmd(Angle pos) { return runOnce(() -> setHoodAngle(pos)).withName("Set Hood Position"); } - public Command runSetpoints(AngularVelocity vel, Angle pos) { - return startEnd(() -> setSetpoints(vel, pos), this::stopShooter); - } + /** * Calculates Velocity and Hood Angle based on distance and Shoots * diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 0fe1907c..81d1f9c0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -37,9 +37,6 @@ public class ShooterConstants { public static final Distance allowedTrenchDistance = Meters.of(1.0); - @Deprecated - public static final Time hangTimeOnShot = Seconds.of(1.8); - public static final int hangtimeLoopPasses = 5; // for a for loop in calculating distance from the hub, as you need the distance to get the hangtime, but you also need the hangtime to get the distance public static final double timeBeforeShoot = 0.2; @@ -58,81 +55,60 @@ public class PID { public static final double shooterkS = 0.0; } - public class MotionMagic { - - // shooter - public static final double shooterkMaxVelocity = 1000.0; - public static final double shooterkMaxAcceleration = 3000.0; - public static final double shooterkJerk = 0.0; - - // hood - public static final double hoodkMaxVelocity = 0.0; - public static final double hoodkMaxAcceleration = 300.0; - public static final double hoodkJerk = 0.0; - } - - public class SoftwareLimits { - public static final double hoodMaxAngle = Positions.hoodHIGH; - public static final double hoodMinAngle = 0.0; - } + public class MotionMagic { - public class Positions { - // Field Positions - public static final Translation2d blueHubPose = new Translation2d(4.5974,4.034536); - public static final Translation2d redHubPose = new Translation2d(11.938,4.034536); + // shooter + public static final double shooterkMaxVelocity = 1000.0; + public static final double shooterkMaxAcceleration = 3000.0; + public static final double shooterkJerk = 0.0; + } - public static final Distance allianceZone = Meters.of(4.5974); - public static final Distance spinUpZone = Meters.of(6.0); + public class Positions { + // Field Positions + public static final Translation2d blueHubPose = new Translation2d(4.5974,4.034536); + public static final Translation2d redHubPose = new Translation2d(11.938,4.034536); - public static final Distance blueAllianceWall = Meters.of(0); - public static final Distance redAllianceWall = Meters.of(FieldConstants.layout.getFieldLength()); + public static final Distance allianceZone = Meters.of(4.5974); + public static final Distance spinUpZone = Meters.of(6.0); - public static final Translation2d blueTargetLeft = new Translation2d(2,6); - public static final Translation2d blueTargetRight = new Translation2d(2,2); + public static final Distance blueAllianceWall = Meters.of(0); + public static final Distance redAllianceWall = Meters.of(FieldConstants.layout.getFieldLength()); - public static final Translation2d redTargetLeft = new Translation2d(14,6); - public static final Translation2d redTargetRight = new Translation2d(14,2); + public static final Translation2d blueTargetLeft = new Translation2d(2,6); + public static final Translation2d blueTargetRight = new Translation2d(2,2); - public static final double centerLineY = 4.034536; + public static final Translation2d redTargetLeft = new Translation2d(14,6); + public static final Translation2d redTargetRight = new Translation2d(14,2); - // Hood Setpoints - public static final double hoodLOW = 0; - public static final double hoodMEDIUM = 1; - public static final double hoodHIGH = 2; - - public enum HubDistance { - LOW(Meters.of(2)), // 0-2 m - MEDIUM(Meters.of(3)), // in - HIGH(Meters.of(4)); // 96+ in + public static final double centerLineY = FieldConstants.layout.getFieldWidth() / 2.0; + + public enum HubDistance { + LOW(Meters.of(2)), // 0-2 m + MEDIUM(Meters.of(3)), // in + HIGH(Meters.of(4)); // 96+ in - private final Distance maxDistance; + private final Distance maxDistance; - private HubDistance(Distance maxDistance) { - this.maxDistance = maxDistance; - } + private HubDistance(Distance maxDistance) { + this.maxDistance = maxDistance; + } - public Distance maxDistance() { return maxDistance; } + public Distance maxDistance() { return maxDistance; } - public static HubDistance fromDistance(Distance pos) { - for(HubDistance vol : values()) { - if (pos.baseUnitMagnitude() <= vol.maxDistance().baseUnitMagnitude()) { - return vol; - } - } - return HubDistance.HIGH; + public static HubDistance fromDistance(Distance pos) { + for(HubDistance vol : values()) { + if (pos.baseUnitMagnitude() <= vol.maxDistance().baseUnitMagnitude()) { + return vol; + } } + return HubDistance.HIGH; } + } } public class HoodPWMs { - public static final int hoodLeftPWMPort = 2; - public static final int hoodRightPWMPort = 0; + public static final int hoodLeftPWMPort = 2; + public static final int hoodRightPWMPort = 0; } - public class FerryPositions{ - public static final Translation2d blueOutpostFerryTarget= new Translation2d(2.136,1.935); //ferry target for the blue outpost - public static final Translation2d redOutpostFerryTarget= new Translation2d(14.0,6.0); //ferry target for the red outpost - public static final Translation2d blueDepotFerryTarget= new Translation2d(2.0,6.0); //ferry target for the blue depot - public static final Translation2d redDepotFerryTarget= new Translation2d(14.0,2.0); //ferry target for the red depot - } } diff --git a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java index 11736800..7c3e9f90 100755 --- a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java +++ b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java @@ -101,7 +101,6 @@ public void periodic() { // Iterate cameras for logging and pose estimations. for (int cam = 0; cam < io_.length; cam++) { - LoggedTracer.reset(); // Activate disconnected alert. alerts_[cam].set(!inputs_[cam].connected); @@ -111,8 +110,6 @@ public void periodic() { ArrayList tagPoses = new ArrayList<>(); - // Loop through visible tags. - LoggedTracer.record("Vision/Camera" + cam + "/PreLoop"); for (Fiducial fid : inputs_[cam].fiducials) { Optional pose = FieldConstants.layout.getTagPose(fid.id()); if (pose.isPresent()) { @@ -120,21 +117,15 @@ public void periodic() { } } - LoggedTracer.record("Vision/Camera" + cam + "/GatherFiducials"); - PoseEstimation est = inputs_[cam].poseEstimate; poseEstimates.add(est); // Add to summaries. summaryTagPoses.addAll(tagPoses); - LoggedTracer.record("Vision/Camera" + cam + "/AddArrays"); - // Log camera-specific outputs. Logger.recordOutput(getCameraKey(cam, "TagPoses"), tagPoses.toArray(new Pose3d[0])); Logger.recordOutput(getCameraKey(cam, "BotPose"), est.pose()); - - LoggedTracer.record("Vision/Camera" + cam + "/LogOutputs"); } LoggedTracer.record("Vision/GatherPoses"); From 8bc15f2a081a1a8d4424a58693ed447464173b8b Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Wed, 11 Mar 2026 15:13:12 -0700 Subject: [PATCH 47/48] Add slow mode for shooting --- src/main/java/frc/robot/Constants.java | 10 +++---- .../robot/commands/drive/DriveCommands.java | 28 +++++++++++++++---- .../robot/commands/robot/RobotCommands.java | 7 +++-- 3 files changed, 33 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bd66876d..666268e9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -40,11 +40,11 @@ public final class Constants { private static final RobotType robotType = RobotType.COMPETITION; public static final boolean spawnLessFuelInSim = true; - public static final boolean shootOnMove = true; // if true, we will allow the driver to shoot while moving, but with reduced max speed. if false, we will not allow the driver to shoot while moving. - //change to 0 if it really doesnt work, bc the db velocity will go to 0 and the target will just be the hub - //but I think it will work so yeah trust me butch - public static final double shootOnMoveMaxSpeed = 0.4; - public static final double ferryOnMoveMaxSpeed = 0.8; + + public static final boolean shootOnMove = true; + + public static final double speedMultiplierShooting = 0.4; + public static final double speedMultiplierFerrying = 0.8; public static class DriveConstants { public static final double slowModeJoystickMultiplier = 0.4; diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index 4155d330..e0e5a152 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -134,6 +134,20 @@ public static Command joystickDrive() { return joystickDrive(drive_, xSupplier_, ySupplier_, omegaSupplier_); } + /** + * Field relative drive command using joystick for linear control and PID for + * angular control. + * Possible use cases include snapping to an angle, aiming at a vision target, + * or controlling + * absolute rotation with a joystick. + * This is preconfigured with {@link #configure(Drive, DoubleSupplier, DoubleSupplier, DoubleSupplier)} + */ + public static Command joystickDriveAtAngle(Supplier rotationSupplier, DoubleSupplier throttle) { + if (!configured) throw new IllegalStateException("DriveCommands joystickDriveAtAngle called without first configuring!"); + + return joystickDriveAtAngle(drive_, xSupplier_, ySupplier_, rotationSupplier, throttle); + } + /** * Field relative drive command using joystick for linear control and PID for * angular control. @@ -145,7 +159,7 @@ public static Command joystickDrive() { public static Command joystickDriveAtAngle(Supplier rotationSupplier) { if (!configured) throw new IllegalStateException("DriveCommands joystickDriveAtAngle called without first configuring!"); - return joystickDriveAtAngle(drive_, xSupplier_, ySupplier_, rotationSupplier); + return joystickDriveAtAngle(rotationSupplier, () -> 1.0); } /** @@ -197,7 +211,9 @@ public static Command joystickDriveAtAngle( Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier, - Supplier rotationSupplier) { + Supplier rotationSupplier, + DoubleSupplier throttle + ) { // Create PID controller ProfiledPIDController angleController = new ProfiledPIDController( @@ -220,9 +236,11 @@ public static Command joystickDriveAtAngle( // Convert to field relative speeds & send command ChassisSpeeds speeds = new ChassisSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega); + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec() * throttle.getAsDouble(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec() * throttle.getAsDouble(), + omega + ); + boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( diff --git a/src/main/java/frc/robot/commands/robot/RobotCommands.java b/src/main/java/frc/robot/commands/robot/RobotCommands.java index 7989df24..cbf5589a 100644 --- a/src/main/java/frc/robot/commands/robot/RobotCommands.java +++ b/src/main/java/frc/robot/commands/robot/RobotCommands.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants; import frc.robot.RobotState; import frc.robot.commands.drive.DriveCommands; import frc.robot.subsystems.drive.Drive; @@ -28,7 +29,9 @@ private static Command shootHub(Shooter shooter, Hopper hopper, IntakeSubsystem () -> drive.rotationIsNear(RobotState.rotationToVirtualHub(), ShooterConstants.aimingTolerance); return Commands.parallel( - DriveCommands.joystickDriveAtAngle(RobotState::rotationToVirtualHub), + Constants.shootOnMove + ? DriveCommands.joystickDriveAtAngle(RobotState::rotationToVirtualHub, () -> Constants.speedMultiplierShooting) + : DriveCommands.joystickDriveAtAngle(drive, () -> 0.0, () -> 0.0, RobotState::rotationToHub, () -> 1.0), Commands.repeatingSequence( Commands.waitUntil(aimedAtHub).deadlineFor(shooter.spinUpForDistance(RobotState::hubDistance)), shooter.shootAtDistance(RobotState::virtualHubDistance, hopper, intake, shakeTrigger) @@ -45,7 +48,7 @@ private static Command shootHub(Shooter shooter, Hopper hopper, IntakeSubsystem * @return */ private static Command ferry(Shooter shooter, Hopper hopper, IntakeSubsystem intake, Drive drive, Trigger shakeTrigger) { - return DriveCommands.joystickDriveAtAngle(RobotState::rotationToVirtualFerry) + return DriveCommands.joystickDriveAtAngle(RobotState::rotationToVirtualFerry, () -> Constants.speedMultiplierFerrying) .alongWith( shooter.shootAtDistance(RobotState::virtualFerryDistance, hopper, intake, shakeTrigger), Commands.runOnce(() -> Logger.recordOutput("Ferry/IsFerrying", true)) From 70de08c10042bcf5b656fe741363f57de88f25e8 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Wed, 11 Mar 2026 15:54:02 -0700 Subject: [PATCH 48/48] Shoot on the move --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/commands/robot/RobotCommands.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 666268e9..e6bfdbbc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -43,7 +43,7 @@ public final class Constants { public static final boolean shootOnMove = true; - public static final double speedMultiplierShooting = 0.4; + public static final double speedMultiplierShooting = 0.2; public static final double speedMultiplierFerrying = 0.8; public static class DriveConstants { diff --git a/src/main/java/frc/robot/commands/robot/RobotCommands.java b/src/main/java/frc/robot/commands/robot/RobotCommands.java index cbf5589a..d9f793c3 100644 --- a/src/main/java/frc/robot/commands/robot/RobotCommands.java +++ b/src/main/java/frc/robot/commands/robot/RobotCommands.java @@ -36,7 +36,7 @@ private static Command shootHub(Shooter shooter, Hopper hopper, IntakeSubsystem Commands.waitUntil(aimedAtHub).deadlineFor(shooter.spinUpForDistance(RobotState::hubDistance)), shooter.shootAtDistance(RobotState::virtualHubDistance, hopper, intake, shakeTrigger) .until(() -> !aimedAtHub.getAsBoolean()) - ) + ).alongWith(Commands.run(() -> Logger.recordOutput("Shooter/AimedAtHub", aimedAtHub.getAsBoolean()))) ); }