1 note auto

frc3322 · Feb 19, 2024 · 22433c8 · 22433c8
1 parent 3a6f2fe
commit 22433c8
Show file tree

Hide file tree

Showing 5 changed files with 26 additions and 13 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -188,7 +188,9 @@ public static final class ShooterConstants{
 
     public static final double shootingRMPAutoLine = 3000;
 
-    public static final double shooterRPMThreshold = 50;
+    public static final double shooterRPMThreshold = 100;
+
+    public static final double shooterAutoRPMThreshold = 2000;
   }
 
   public static final class IntakeConstants {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import frc.robot.Constants.OIConstants;
+import frc.robot.commands.AutoCommmands;
 import frc.robot.commands.ComboCommands;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.Elevator;
@@ -50,6 +51,8 @@ public class RobotContainer {
   //combination commands class
   ComboCommands comboCommands = new ComboCommands(elevator, intake, transfer, shooter);
 
+  AutoCommmands autoCommmands = new AutoCommmands(robotDrive, intake, elevator, transfer, shooter, comboCommands);
+
   // Auton selector for dashboard
   SendableChooser<SequentialCommandGroup> autoSelector = new SendableChooser<>();
 
@@ -249,7 +252,7 @@ public Command getAutonomousCommand() {
      * https://github.com/mjansen4857/pathplanner
      */
 
-     return new PathPlannerAuto("SegmentedTopFour");
+     return autoCommmands.shootOnStart();
   }
 
   /*◇─◇──◇─◇

diff --git a/src/main/java/frc/robot/commands/AutoCommmands.java b/src/main/java/frc/robot/commands/AutoCommmands.java
@@ -1,6 +1,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.Elevator;
@@ -28,16 +29,19 @@ public AutoCommmands(DriveSubsystem robotDrive, Intake intake, Elevator elevator
 
     public Command shootOnStart() {
         return new SequentialCommandGroup(
-            shooter.shooterAutoLineRevUpCommand(),
-            new StepCommand(transfer.shootCommand(), shooter::bothAtSetpointRPM, transfer)
+            new ParallelCommandGroup(
+                shooter.shooterAutoLineRevUpCommand(),
+                new StepCommand(transfer.shootCommand(), shooter::bothAtSetpointRPM, transfer)
+            ).until(transfer::shooterNotFull),
+            shooter.stopShooterCommand()
         );
     }
 
-    // public Command autoIntake() {
-    //     return new SequentialCommandGroup(
-    //         combo.startShooterIntakeCommand()
-    //         .until(transfer::shooterFull)
-    //         .andThen(combo.stowCommandGroup())
-    //     );
-    // }
+    public Command autoIntakeToShooter() {
+        return new SequentialCommandGroup(
+            combo.startShooterIntakeCommand()
+            .until(transfer::shooterFull)
+            .andThen(combo.stowCommandGroup())
+        );
+    }
 }
diff --git a/src/main/java/frc/robot/commands/StepCommand.java b/src/main/java/frc/robot/commands/StepCommand.java
@@ -24,6 +24,7 @@ public class StepCommand extends Command {
    */
   public StepCommand(Command toRun, BooleanSupplier condition, Subsystem... requirements) {
     this.toRun = toRun;
+    this.condition = condition;
 
     addRequirements(requirements);
   }
@@ -41,6 +42,7 @@ public void execute() {
       toRun.schedule();
       hasRun = true;
     }
+
   }
 
   // Called once the command ends or is interrupted.

diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -135,12 +135,14 @@ public double getBottomWheelRPM() {
 
   @Log
   public boolean topAtSetpointRPM() {
-    return Math.abs(getTopWheelRPM() - shooterTopSetpoint) < ShooterConstants.shooterRPMThreshold;
+    //return Math.abs(getTopWheelRPM() - shooterTopSetpoint) < ShooterConstants.shooterRPMThreshold;
+    return getTopWheelRPM() > 2000;
   }
 
   @Log
   public boolean bottomAtSetpointRPM() {
-    return Math.abs(getBottomWheelRPM() - shooterBottomSetpoint) < ShooterConstants.shooterRPMThreshold;
+    //return Math.abs(getBottomWheelRPM() - shooterBottomSetpoint) < ShooterConstants.shooterRPMThreshold;
+    return getTopWheelRPM() > 2000;
   }
 
   @Log public boolean bothAtSetpointRPM() {