From 5f21f022a67485380256ad75a0b7e0f8173bdd51 Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Fri, 29 Mar 2024 19:55:46 -0400 Subject: [PATCH] got shooter logic for auton (to allow for revving while actuating in / have rev be on all of auton) --- src/main/java/frc/robot/Constants.java | 4 +-- .../frc/robot/commands/auto/AutonShoot.java | 12 ++++----- .../robot/commands/intake/FeedActuate.java | 2 ++ .../frc/robot/subsystems/IntakeSubsystem.java | 1 + .../robot/subsystems/ShooterSubsystem.java | 25 +++++++++++++------ 5 files changed, 28 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d54d25e8..226fd83d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -62,9 +62,9 @@ public static class Speeds { public static final double SWERVE_DRIVE_MAX_ANGULAR_SPEED = Math.PI * 2.5; // TODO: Get this value public static final double SHOOTER_FAST_SPIN_SPEED = 1; - public static final double SHOOTER_SLOW_SPIN_SPEED = 1; + public static final double SHOOTER_SLOW_SPIN_SPEED = 0.8; public static final double SHOOTER_AMP_SPEED = 0.2; - public static final double SHOOTER_INTAKE_SPEED = -.15; + public static final double SHOOTER_INTAKE_SPEED = -0.15; public static final double SHOOTER_IDLE_SPEED = 0.3; public static final double SHOOTER_ANGLE_MAX_SPEED = 0.3; diff --git a/src/main/java/frc/robot/commands/auto/AutonShoot.java b/src/main/java/frc/robot/commands/auto/AutonShoot.java index fadeb2b6..969c802b 100644 --- a/src/main/java/frc/robot/commands/auto/AutonShoot.java +++ b/src/main/java/frc/robot/commands/auto/AutonShoot.java @@ -8,6 +8,7 @@ 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.commands.intake.FeedActuate; import frc.robot.commands.shooter.SpinShooter; import frc.robot.subsystems.ElevatorSubsystem; @@ -36,14 +37,11 @@ public AutonShoot( // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( + // This command will squeeze the note and rev up the shooter if needed, all while auto-aiming. 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)).andThen(() -> System.out.println("REVVED")), - new FeedActuate(intake, FeedMode.INTAKE).withTimeout(1).andThen(() -> System.out.println("INTAKED")) - ).onlyIf(() -> !intake.hasNoteAuton()), - - new AutoAim(swerve, shooter, elevator, leds).withTimeout(1).andThen(() -> System.out.println("AIMED")) + new SpinShooter(shooter, ShooterState.SHOOT, true).andThen(new WaitUntilCommand(shooter::isRevved)), + new FeedActuate(intake, FeedMode.INTAKE).withTimeout(1).onlyIf(() -> !intake.hasNoteAuton()), + new AutoAim(swerve, shooter, elevator, leds).withTimeout(1) ), new FeedActuate(intake, FeedMode.OUTTAKE).withTimeout(1), diff --git a/src/main/java/frc/robot/commands/intake/FeedActuate.java b/src/main/java/frc/robot/commands/intake/FeedActuate.java index 53a7a91f..bbb0554e 100644 --- a/src/main/java/frc/robot/commands/intake/FeedActuate.java +++ b/src/main/java/frc/robot/commands/intake/FeedActuate.java @@ -47,6 +47,8 @@ public FeedActuate(IntakeSubsystem intake, FeedMode feedMode) { public void initialize() { _intake.feed(_feedMode); _intake.actuate(_actuatorState); + + if (_feedMode == FeedMode.OUTTAKE) _intake.setHasNoteAuton(false); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 5d4d76af..b92d6560 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -154,5 +154,6 @@ public void periodic() { SmartDashboard.putNumber("ACTUATOR ENCODER", getActuator()); SmartDashboard.putNumber("ACTUATOR PERCENT OUTPUT", _actuatorMotor.get()); SmartDashboard.putNumber("FEED CURRENT OUTPUT", _feedMotor.getOutputCurrent()); + SmartDashboard.putBoolean("HAS NOTE AUTON", _hasNoteAuton); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index bce9daa4..afb6abaf 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -47,6 +48,8 @@ public class ShooterSubsystem extends SubsystemBase { private final Debouncer _beamDebouncer = new Debouncer(0.3, DebounceType.kRising); + private final Timer _revTimer = new Timer(); + private double _shooterTrim = 3; private boolean _holdNote = false; @@ -98,7 +101,8 @@ public void periodic() { SmartDashboard.putNumber("SHOOTER ANGLE", getAngle()); SmartDashboard.putNumber("SHOOTER PERCENT OUTPUT", _leftMotor.get()); SmartDashboard.putNumber("SHOOTER INCREMENTAL ENCODER", _revShooterEncoder.getDistance()); - SmartDashboard.putBoolean("SHOOTER REVVED", false); + SmartDashboard.putBoolean("SHOOTER REVVED", isRevved()); + SmartDashboard.putNumber("SHOOTER MOTOR RPM", getVelocity()); _shooterTrim = SmartDashboard.getNumber("SHOOTER TRIM", _shooterTrim); } @@ -119,6 +123,11 @@ public double speakerAngle() { return angle + _shooterTrim; } + /** Returns whether the shooter (motor) is revved up (if enough time has elapsed). */ + public boolean isRevved() { + return _revTimer.hasElapsed(2); + } + /** * Reset the shooter's hold note, allowing it to shoot freely with a note. */ @@ -159,7 +168,7 @@ public double getAngularVelocity(){ return _angleMotor.getVelocity().getValueAsDouble() / Constants.Physical.SHOOTER_ANGLE_GEAR_RATIO * 360; } - /** Get velocity of the shooter flywheel in encoder val. */ + /** Get velocity of the shooter flywheel in encoder RPM. */ public double getVelocity() { return _leftEncoder.getVelocity(); } @@ -182,13 +191,15 @@ public void stopAngle() { /** Sets the state of the shooter. */ public void setShooterState(ShooterState state) { + if (state != ShooterState.SHOOT) { + _revTimer.stop(); + _revTimer.reset(); + } + switch (state) { case SHOOT: - if (UtilFuncs.ShotVector().getNorm() > FieldConstants.SHOOTER_SLOW_THRESHOLD) { - spinShooter(Speeds.SHOOTER_FAST_SPIN_SPEED); - } else { - spinShooter(Speeds.SHOOTER_SLOW_SPIN_SPEED); - } + _revTimer.start(); + spinShooter(Speeds.SHOOTER_FAST_SPIN_SPEED); break; case AMP: