From 34590dba3f1fe39110f9525db22905b988b0d4b6 Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Fri, 10 Apr 2026 11:55:47 -0700 Subject: [PATCH 1/3] Add ShooterDefaultCommand to warm up shooter to configurable RPM Replaces ShooterStopCommand as the shooter setpoint lock default, so the shooter warms up to a tunable velocity (default 2500 RPM) when no other command is active. Co-Authored-By: Claude Opus 4.6 --- .../SubsystemDefaultCommandMap.java | 6 +-- .../commands/ShooterDefaultCommand.java | 39 +++++++++++++++++++ 2 files changed, 42 insertions(+), 3 deletions(-) create mode 100644 src/main/java/competition/subsystems/shooter/commands/ShooterDefaultCommand.java diff --git a/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java b/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java index df3ba221..15042303 100644 --- a/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java +++ b/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java @@ -15,7 +15,7 @@ import competition.subsystems.intake_deploy.commands.IntakeDeployMaintainerCommand; import competition.subsystems.shooter.ShooterSubsystem; import competition.subsystems.intake_deploy.IntakeDeploySubsystem; -import competition.subsystems.shooter.commands.ShooterStopCommand; +import competition.subsystems.shooter.commands.ShooterDefaultCommand; import competition.subsystems.shooter.commands.ShooterWheelMaintainerCommand; import competition.subsystems.shooter_feeder.ShooterFeederSubsystem; import competition.subsystems.shooter_feeder.commands.ShooterFeederStop; @@ -42,9 +42,9 @@ public void setupCollectorSubsystem(CollectorSubsystem collect, CollectorStopCom @Inject public void setupShooterSubsystem(ShooterSubsystem shooter, ShooterWheelMaintainerCommand command, - ShooterStopCommand stopCommand) { + ShooterDefaultCommand defaultCommand) { shooter.setDefaultCommand(command); - shooter.getSetpointLock().setDefaultCommand(stopCommand); + shooter.getSetpointLock().setDefaultCommand(defaultCommand); } @Inject diff --git a/src/main/java/competition/subsystems/shooter/commands/ShooterDefaultCommand.java b/src/main/java/competition/subsystems/shooter/commands/ShooterDefaultCommand.java new file mode 100644 index 00000000..724d34a2 --- /dev/null +++ b/src/main/java/competition/subsystems/shooter/commands/ShooterDefaultCommand.java @@ -0,0 +1,39 @@ +package competition.subsystems.shooter.commands; + +import competition.subsystems.shooter.ShooterSubsystem; +import xbot.common.command.BaseSetpointCommand; +import xbot.common.properties.AngularVelocityProperty; +import xbot.common.properties.PropertyFactory; + +import javax.inject.Inject; + +import static edu.wpi.first.units.Units.RPM; + +public class ShooterDefaultCommand extends BaseSetpointCommand { + private final ShooterSubsystem shooter; + private final AngularVelocityProperty warmUpVelocity; + + @Inject + public ShooterDefaultCommand(ShooterSubsystem shooterSubsystem, PropertyFactory pf) { + super(shooterSubsystem); + this.shooter = shooterSubsystem; + pf.setPrefix(this); + this.warmUpVelocity = pf.createPersistentProperty("WarmUpVelocity", RPM.of(2500)); + } + + @Override + public void initialize() { + super.initialize(); + this.shooter.setTargetValue(warmUpVelocity.get()); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } + + @Override + public boolean isFinished() { + return false; + } +} From d422b1d80f382bb6c0adc07301b4e01c0aebfade Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Fri, 10 Apr 2026 12:38:38 -0700 Subject: [PATCH 2/3] Only warm up shooter when robot is in alliance zone Adds PoseSubsystem.isInAllianceZone() to check if the robot is on its own half of the field. ShooterDefaultCommand now continuously checks position and only spins up to warm-up RPM in the alliance zone, setting target to 0 RPM otherwise to save power. Co-Authored-By: Claude Opus 4.6 --- .../subsystems/pose/PoseSubsystem.java | 12 ++++++++++++ .../shooter/commands/ShooterDefaultCommand.java | 15 +++++++++++++-- 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 4f9fcd98..a9289ddc 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -341,6 +341,18 @@ public void setPreferOdometryToVision(boolean preferOdometryToVision) { } } + public boolean isInAllianceZone() { + var alliance = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue); + double robotX = getCurrentPose2d().getX(); + double midpoint = fieldXMidpointInMeters.in(Meters); + + if (alliance == DriverStation.Alliance.Blue) { + return robotX < midpoint; + } else { + return robotX > midpoint; + } + } + // Start of Closest Landmark Calcs public Pose2d closestAllianceTrench() { diff --git a/src/main/java/competition/subsystems/shooter/commands/ShooterDefaultCommand.java b/src/main/java/competition/subsystems/shooter/commands/ShooterDefaultCommand.java index 724d34a2..ad072f27 100644 --- a/src/main/java/competition/subsystems/shooter/commands/ShooterDefaultCommand.java +++ b/src/main/java/competition/subsystems/shooter/commands/ShooterDefaultCommand.java @@ -1,5 +1,6 @@ package competition.subsystems.shooter.commands; +import competition.subsystems.pose.PoseSubsystem; import competition.subsystems.shooter.ShooterSubsystem; import xbot.common.command.BaseSetpointCommand; import xbot.common.properties.AngularVelocityProperty; @@ -11,12 +12,14 @@ public class ShooterDefaultCommand extends BaseSetpointCommand { private final ShooterSubsystem shooter; + private final PoseSubsystem pose; private final AngularVelocityProperty warmUpVelocity; @Inject - public ShooterDefaultCommand(ShooterSubsystem shooterSubsystem, PropertyFactory pf) { + public ShooterDefaultCommand(ShooterSubsystem shooterSubsystem, PoseSubsystem pose, PropertyFactory pf) { super(shooterSubsystem); this.shooter = shooterSubsystem; + this.pose = pose; pf.setPrefix(this); this.warmUpVelocity = pf.createPersistentProperty("WarmUpVelocity", RPM.of(2500)); } @@ -24,7 +27,15 @@ public ShooterDefaultCommand(ShooterSubsystem shooterSubsystem, PropertyFactory @Override public void initialize() { super.initialize(); - this.shooter.setTargetValue(warmUpVelocity.get()); + } + + @Override + public void execute() { + if (pose.isInAllianceZone()) { + this.shooter.setTargetValue(warmUpVelocity.get()); + } else { + this.shooter.setTargetValue(RPM.of(0)); + } } @Override From 1c89c7a2e3cc7e8da2bf043b663ec75030d41e5a Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Fri, 10 Apr 2026 13:30:05 -0700 Subject: [PATCH 3/3] tweak logic so alliance zone --- .../java/competition/subsystems/pose/PoseSubsystem.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index a9289ddc..9204dbc2 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -344,12 +344,13 @@ public void setPreferOdometryToVision(boolean preferOdometryToVision) { public boolean isInAllianceZone() { var alliance = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue); double robotX = getCurrentPose2d().getX(); - double midpoint = fieldXMidpointInMeters.in(Meters); + double allianceZoneDepth = 5.0; + double fieldWidth = fieldXMidpointInMeters.in(Meters) * 2; if (alliance == DriverStation.Alliance.Blue) { - return robotX < midpoint; + return robotX < allianceZoneDepth; } else { - return robotX > midpoint; + return robotX > fieldWidth - allianceZoneDepth; } }