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/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 4f9fcd98..9204dbc2 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/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() { 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..ad072f27 --- /dev/null +++ b/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; + } +}