Merge branch 'main' of https://github.com/frc3322/code-2024

src/main/java/frc/robot/Constants.java

@@ -187,6 +187,8 @@ public static final class ShooterConstants{
     public static final double shooterBottomV = 0.00018;
 
     public static final double shootingRMPAutoLine = 3000;
+
+    public static final double shooterRPMThreshold = 50;
   }
 
   public static final class IntakeConstants {

src/main/java/frc/robot/commands/AutoCommmands.java

@@ -0,0 +1,43 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.Shooter;
+import frc.robot.subsystems.Transfer;
+
+public class AutoCommmands {
+
+    private final DriveSubsystem robotDrive;
+    private final Elevator elevator;
+    private final Intake intake;
+    private final Shooter shooter;
+    private final Transfer transfer;
+    private final ComboCommands combo;
+
+    public AutoCommmands(DriveSubsystem robotDrive, Intake intake, Elevator elevator, Transfer transfer, Shooter shooter, ComboCommands combo) {
+        this.robotDrive = robotDrive;
+        this.elevator = elevator;
+        this.intake = intake;
+        this.transfer = transfer;
+        this.shooter = shooter;
+        this.combo = combo;
+    }
+
+    public Command shootOnStart() {
+        return new SequentialCommandGroup(
+            shooter.shooterAutoLineRevUpCommand(),
+            new StepCommand(transfer.shootCommand(), shooter::bothAtSetpointRPM, transfer)
+        );
+    }
+
+    // public Command autoIntake() {
+    //     return new SequentialCommandGroup(
+    //         combo.startShooterIntakeCommand()
+    //         .until(transfer::shooterFull)
+    //         .andThen(combo.stowCommandGroup())
+    //     );
+    // }
+}

src/main/java/frc/robot/commands/StepCommand.java

@@ -11,7 +11,7 @@
 
 public class StepCommand extends Command {
 
-  private Runnable toRun;
+  private Command toRun;
   private BooleanSupplier condition;
 
   private boolean hasRun = false;
@@ -22,7 +22,7 @@ public class StepCommand extends Command {
    * @param condition The condition to wait for.
    * @param requirements The subsystems required by this command
    */
-  public StepCommand(Runnable toRun, BooleanSupplier condition, Subsystem... requirements) {
+  public StepCommand(Command toRun, BooleanSupplier condition, Subsystem... requirements) {
     this.toRun = toRun;
 
     addRequirements(requirements);
@@ -38,7 +38,7 @@ public void initialize() {
   @Override
   public void execute() {
     if (condition.getAsBoolean()){
-      toRun.run();
+      toRun.schedule();
       hasRun = true;
     }
   }

src/main/java/frc/robot/subsystems/Shooter.java

@@ -124,16 +124,28 @@ public double getBottomCombinedControllers() {
   }
 
   @Log
-  public double getTopWheelRPM(){
+  public double getTopWheelRPM() {
     return shooterTopEncoder.getVelocity();
   }
 
   @Log
-  public double getBottomWheelRPM(){
+  public double getBottomWheelRPM() {
     return shooterBottomEncoder.getVelocity();
   }
 
+  @Log
+  public boolean topAtSetpointRPM() {
+    return Math.abs(getTopWheelRPM() - shooterTopSetpoint) < ShooterConstants.shooterRPMThreshold;
+  }
+
+  @Log
+  public boolean bottomAtSetpointRPM() {
+    return Math.abs(getBottomWheelRPM() - shooterBottomSetpoint) < ShooterConstants.shooterRPMThreshold;
+  }
 
+  @Log public boolean bothAtSetpointRPM() {
+    return topAtSetpointRPM() && bottomAtSetpointRPM();
+  }
 
   /*◇─◇──◇─◇
   ✨Setters✨