Add ShooterDefaultCommand to warm up shooter to configurable RPM

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

src/main/java/competition/subsystems/pose/PoseSubsystem.java

@@ -341,6 +341,19 @@ public void setPreferOdometryToVision(boolean preferOdometryToVision) {
         }
     }
 
+    public boolean isInAllianceZone() {
+        var alliance = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue);
+        double robotX = getCurrentPose2d().getX();
+        double allianceZoneDepth = 5.0;
+        double fieldWidth = fieldXMidpointInMeters.in(Meters) * 2;
+
+        if (alliance == DriverStation.Alliance.Blue) {
+            return robotX < allianceZoneDepth;
+        } else {
+            return robotX > fieldWidth - allianceZoneDepth;
+        }
+    }
+
     // Start of Closest Landmark Calcs
 
     public Pose2d closestAllianceTrench() {

src/main/java/competition/subsystems/shooter/commands/ShooterDefaultCommand.java

@@ -0,0 +1,50 @@
+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;
+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 PoseSubsystem pose;
+    private final AngularVelocityProperty warmUpVelocity;
+
+    @Inject
+    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));
+    }
+
+    @Override
+    public void initialize() {
+        super.initialize();
+    }
+
+    @Override
+    public void execute() {
+        if (pose.isInAllianceZone()) {
+            this.shooter.setTargetValue(warmUpVelocity.get());
+        } else {
+            this.shooter.setTargetValue(RPM.of(0));
+        }
+    }
+
+    @Override
+    public boolean runsWhenDisabled() {
+        return true;
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}