From 480eb864d4c9ac35a47efb5918e10559dc410ad3 Mon Sep 17 00:00:00 2001 From: Michael Date: Mon, 16 Feb 2026 20:47:42 -0800 Subject: [PATCH 01/25] 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 a763ab7..2f65d69 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 5aaae06..86f0b72 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 7737b45..5753414 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 7fe866a..76bff45 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 11137a1..ed6ab1e 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/25] 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 2f65d69..6c8363b 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 86f0b72..3169e8f 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/25] 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 3169e8f..04993b3 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 5753414..04dec10 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/25] 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 6c8363b..02cdc40 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 04993b3..67d1b30 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/25] 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 d6d761d..3e3d84a 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/25] 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 3e3d84a..1bd88b0 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 ed6ab1e..12d533b 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/25] 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 02cdc40..bdf1963 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 1bd88b0..2166ec7 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 fd832b4..cc443ce 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 12d533b..6e2164a 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/25] 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 2166ec7..6d58ed8 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/25] 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 6d58ed8..71ae8fb 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 76bff45..13de0a2 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/25] 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 6e2164a..4c55dec 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/25] 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 71ae8fb..688487d 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 04dec10..f5a95b2 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/25] 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 688487d..f2f1a73 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 db9befe..a036d68 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 f5a95b2..8ea29e7 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 13de0a2..abcc26e 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/25] 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 bdf1963..d40ecec 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 f2f1a73..c12b6e5 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 930f1f9..4c8be4d 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 a036d68..8bb9adb 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 abcc26e..f985d6b 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/25] 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 53a7314..5792670 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/25] 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 071cc0a..15b5b3b 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 8fd725e..45ce7d9 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/25] 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 85074d4..c3d0296 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/25] 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 8bb9adb..4bb1a53 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 c3d0296..0b659e1 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/25] 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 fe3aa1a..6c42931 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 4bb1a53..5ae1d2c 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 45ce7d9..98c720c 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/25] 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 1679499..b302991 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/25] 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 6c42931..027810d 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 5ae1d2c..ccdc484 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 0b659e1..5f5d409 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 0000000..f53f315 --- /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/25] 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 8fdbd3c..996dae4 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/25] 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 5b93d10..0c7a628 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/25] 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 14b82ba..c9f1cb5 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/25] 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 027810d..3a9a36a 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 c9f1cb5..4586af5 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/25] 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 ae87915..fc52ce9 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 74da92d..9eefc52 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 0eecfbb..6dd273c 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 {