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

@@ -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;

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),

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.

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);
   }
 }

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: