From 83cb62ca192b077b9c5f67bd9ee35a3265e94c4d Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Tue, 26 Mar 2024 23:57:47 -0400 Subject: [PATCH] added logic to allow for constant shoot rev in auton --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../frc/robot/commands/auto/AutonShoot.java | 22 ++++++++++++------- .../robot/commands/intake/FeedActuate.java | 6 ++--- .../robot/commands/shooter/SpinShooter.java | 15 ++++++++++++- .../robot/subsystems/ShooterSubsystem.java | 6 +---- 5 files changed, 34 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 951c725..0949732 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -148,7 +148,6 @@ private void configureBindings() { _driveController.L1().onTrue(Commands.runOnce(_swerveSubsystem::toggleSpeed, _swerveSubsystem)); _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem)); _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem, _ledSubsystem)); - _driveController.square().onTrue(Commands.runOnce(() -> AutonShoot.canShoot = false)); // TESTING ONLY!!! _driveController.triangle().onTrue(Commands.runOnce(() -> _swerveSubsystem.resetGyro(180), _swerveSubsystem)); @@ -193,8 +192,9 @@ public void teleopInit() { /** @return The Command to schedule for auton. */ public Command getAutonCommand() { + AutonShoot.reset(); _swerveSubsystem.fieldOriented = false; // make sure swerve is robot-relative for pathplanner to work - _shooterSubsystem.setShooterState(ShooterState.IDLE); + _shooterSubsystem.setShooterState(ShooterState.SHOOT); return _autonChooser.getSelected(); // return null; diff --git a/src/main/java/frc/robot/commands/auto/AutonShoot.java b/src/main/java/frc/robot/commands/auto/AutonShoot.java index 7276946..9b88da1 100644 --- a/src/main/java/frc/robot/commands/auto/AutonShoot.java +++ b/src/main/java/frc/robot/commands/auto/AutonShoot.java @@ -31,11 +31,11 @@ // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class AutonShoot extends SequentialCommandGroup { - /** - * Indicates whether the note in the intake can be shot right away, this means that the note is already squished, and - * that the shooter is already revved enough. - */ - public static boolean canShoot = false; + /** If the note is intaked all the way to shoot. */ + public static boolean isIntaked = false; + + /** If the shooter is revved all the way to shoot. */ + public static boolean isRevved = false; /** Creates a new AutonShoot. */ public AutonShoot( @@ -51,9 +51,9 @@ public AutonShoot( new ParallelCommandGroup( // This command will squeeze the note while revving up the shooter. It will only run if the note can't be shot right away. new ParallelCommandGroup( - new SpinShooter(shooter, ShooterState.SHOOT, true).andThen(new WaitCommand(2)), - new FeedActuate(intake, FeedMode.INTAKE).withTimeout(1) - ).onlyIf(() -> !canShoot).andThen(() -> canShoot = true), + new SpinShooter(shooter, ShooterState.SHOOT, true).onlyIf(() -> !isRevved).andThen(new WaitCommand(2)), + new FeedActuate(intake, FeedMode.INTAKE).onlyIf(() -> !isIntaked).withTimeout(1) + ), new AutoAim(swerve, shooter, elevator, leds) ), @@ -63,6 +63,12 @@ public AutonShoot( ); } + /** Resets this command for re-testing auton. */ + public static void reset() { + isIntaked = false; + isRevved = false; + } + private double headingPreset() { return (UtilFuncs.GetAlliance() == Alliance.Red) ? 0 : 180; } diff --git a/src/main/java/frc/robot/commands/intake/FeedActuate.java b/src/main/java/frc/robot/commands/intake/FeedActuate.java index e79e563..975813c 100644 --- a/src/main/java/frc/robot/commands/intake/FeedActuate.java +++ b/src/main/java/frc/robot/commands/intake/FeedActuate.java @@ -48,9 +48,9 @@ public void initialize() { _intake.feed(_feedMode); _intake.actuate(_actuatorState); - // if not squished (and revved), can't shoot, else can - if (_runOnce) AutonShoot.canShoot = false; - else AutonShoot.canShoot = true; + // if not squished, can't shoot, else can (FOR AUTON) + if (_runOnce && _actuatorState == ActuatorState.STOWED) AutonShoot.isIntaked = false; + if (!_runOnce && _actuatorState == ActuatorState.STOWED) AutonShoot.isIntaked = true; } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/shooter/SpinShooter.java b/src/main/java/frc/robot/commands/shooter/SpinShooter.java index 155a174..964b0a4 100644 --- a/src/main/java/frc/robot/commands/shooter/SpinShooter.java +++ b/src/main/java/frc/robot/commands/shooter/SpinShooter.java @@ -1,7 +1,9 @@ /* Copyright (C) 2024 Team 334. All Rights Reserved.*/ package frc.robot.commands.shooter; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.auto.AutonShoot; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.ShooterSubsystem.ShooterState; @@ -15,11 +17,15 @@ public class SpinShooter extends Command { private final ShooterState _state; private final boolean _runOnce; + private final Timer _revTimer; + public SpinShooter(ShooterSubsystem shooter, ShooterState state, boolean runOnce) { _shooter = shooter; _state = state; _runOnce = runOnce; + _revTimer = new Timer(); + // NO SHOOTER SUBSYSTEM REQUIREMENT TO NOT MESS WITH SHOOTER ANGLING COMMANDS } @@ -32,11 +38,18 @@ public SpinShooter(ShooterSubsystem shooter, ShooterState state) { @Override public void initialize() { _shooter.setShooterState(_state); + + _revTimer.start(); + + // If not set to shoot state then not revved (AUTON ONLY). + if (_runOnce && _state != ShooterState.SHOOT) _revTimer.stop(); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + AutonShoot.isRevved = _revTimer.hasElapsed(2); + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 76d11e9..bce9daa 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -51,7 +51,7 @@ public class ShooterSubsystem extends SubsystemBase { private boolean _holdNote = false; - /** Represents the state of the shooter's flywheels (speaker shoot, amp, nothing). */ + /** Represents the state of the shooter's flywheels (speaker shoot, amp, idle, nothing). */ public enum ShooterState { SHOOT, AMP, @@ -182,9 +182,6 @@ public void stopAngle() { /** Sets the state of the shooter. */ public void setShooterState(ShooterState state) { - SmartDashboard.putString("SHOOTER STATE", state.toString()); - spinShooter(0.3); - switch (state) { case SHOOT: if (UtilFuncs.ShotVector().getNorm() > FieldConstants.SHOOTER_SLOW_THRESHOLD) { @@ -203,7 +200,6 @@ public void setShooterState(ShooterState state) { break; case IDLE: - // spinShooter(0); spinShooter(Speeds.SHOOTER_IDLE_SPEED); break;