diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6e040ea..cf7fb84 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -85,7 +85,7 @@ public RobotContainer() { NamedCommands.registerCommand("actuateOut", new SetShooter(_shooterSubsystem, () -> Presets.ACTUATE_SHOOTER_ANGLE).andThen( new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE) )); - NamedCommands.registerCommand("actuateIn", new FeedActuate(_intakeSubsystem, ActuatorState.STOWED).alongWith(new SpinShooter(_shooterSubsystem, ShooterState.SHOOT).until(() -> true))); + NamedCommands.registerCommand("actuateIn", new FeedActuate(_intakeSubsystem, ActuatorState.STOWED).alongWith(new SpinShooter(_shooterSubsystem, ShooterState.SHOOT, true))); NamedCommands.registerCommand("shoot", new AutonShoot(_shooterSubsystem, _elevatorSubsystem, _ledSubsystem, _swerveSubsystem, _intakeSubsystem)); // Drive/Operate default commands diff --git a/src/main/java/frc/robot/commands/shooter/AutonShoot.java b/src/main/java/frc/robot/commands/shooter/AutonShoot.java index 5852230..c7dc206 100644 --- a/src/main/java/frc/robot/commands/shooter/AutonShoot.java +++ b/src/main/java/frc/robot/commands/shooter/AutonShoot.java @@ -4,17 +4,17 @@ package frc.robot.commands.shooter; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import frc.robot.Constants.Encoders; +import frc.robot.Constants.Presets; import frc.robot.commands.intake.FeedActuate; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDriveSubsystem; -import frc.robot.subsystems.IntakeSubsystem.ActuatorState; import frc.robot.subsystems.IntakeSubsystem.FeedMode; import frc.robot.subsystems.ShooterSubsystem.ShooterState; import frc.robot.utils.UtilFuncs; @@ -23,6 +23,8 @@ // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class AutonShoot extends SequentialCommandGroup { + private double _headingPreset; + /** Creates a new AutonShoot. */ public AutonShoot( ShooterSubsystem shooter, @@ -34,10 +36,17 @@ public AutonShoot( // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( - new AutoAim(shooter, elevator, leds, swerve).alongWith(new FeedActuate(intake, FeedMode.INTAKE).withTimeout(5)).withTimeout(6), + Commands.runOnce(() -> {_headingPreset = (UtilFuncs.GetAlliance() == Alliance.Red) ? 0 : 180;}), + + // pre-shooting (rev up if needed, while auto aiming, while feeding in for squish) + new ParallelCommandGroup( + new SpinShooter(shooter, ShooterState.SHOOT, true).unless(() -> shooter.isState(ShooterState.SHOOT)).withTimeout(3), + new AutoAim(shooter, elevator, leds, swerve, Presets.CLOSE_SHOOTER_ANGLE, Presets.CLOSE_ELEVATOR_HEIGHT, _headingPreset).withTimeout(5), + new FeedActuate(intake, FeedMode.INTAKE).withTimeout(4) + ), // new WaitUntilCommand(() -> UtilFuncs.InRange(shooter.getVelocity(), Encoders.SHOOTER_SHOOT_VEL, 1)), new FeedActuate(intake, FeedMode.OUTTAKE).withTimeout(2), - new SpinShooter(shooter, ShooterState.NONE).until(() -> true) + new SpinShooter(shooter, ShooterState.NONE, true) ); } } diff --git a/src/main/java/frc/robot/commands/shooter/SpinShooter.java b/src/main/java/frc/robot/commands/shooter/SpinShooter.java index 545514c..67db639 100644 --- a/src/main/java/frc/robot/commands/shooter/SpinShooter.java +++ b/src/main/java/frc/robot/commands/shooter/SpinShooter.java @@ -14,12 +14,18 @@ public class SpinShooter extends Command { private final ShooterSubsystem _shooter; private final ShooterState _state; + private final boolean _instant; - public SpinShooter(ShooterSubsystem shooter, ShooterState state) { + public SpinShooter(ShooterSubsystem shooter, ShooterState state, boolean instant) { _shooter = shooter; _state = state; + _instant = instant; + + // NO SHOOTER SUBSYSTEM REQUIREMENT TO NOT MESS WITH SHOOTER ANGLING COMMANDS + } - // NO SHOOTER REQUIREMENT TO NOT MESS WITH SHOOTER ANGLING COMMANDS + public SpinShooter(ShooterSubsystem shooter, ShooterState state) { + this(shooter, state, false); } // Called when the command is initially scheduled. @@ -40,6 +46,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return _instant; } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index b0d4c5d..aba912f 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -37,6 +37,8 @@ public class ShooterSubsystem extends SubsystemBase { private final ArmFeedforward _angleFeed = new ArmFeedforward(0, 0, 0); private final PIDController _angleController = new PIDController(PID.SHOOTER_ANGLE_KP, 0, 0); + private ShooterState _shooterState = ShooterState.NONE; + /** Represents the state of the shooter's flywheels (speaker shoot, amp, nothing). */ public enum ShooterState { SHOOT, @@ -72,6 +74,15 @@ public void periodic() { SmartDashboard.putNumber("SHOOTER ANGLE", getAngle()); } + /** + * Returns true if the shooter's set state matches the supplied one. + * + * @param state Check if the shooter spin been set to this state. + */ + public boolean isState(ShooterState state) { + return _shooterState == state; + } + /** Returns true if the shooter is at the last desired angle setpoint. */ public boolean atDesiredAngle() { return _angleController.atSetpoint(); @@ -113,6 +124,8 @@ public void stopAngle() { /** Sets the state of the shooter. */ public void setShooterState(ShooterState state) { + _shooterState = state; + switch (state) { case SHOOT: spinShooter(Speeds.SHOOTER_SPIN_SPEED);