Adjustable Auto Delay (Sorry Mohammed)

BadRobots1014 · Mar 6, 2024 · aae1f2b · aae1f2b
1 parent 8240d95
commit aae1f2b
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -4,9 +4,11 @@
 
 package frc.robot;
 
+import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -57,6 +59,7 @@ public class RobotContainer {
   // Auto
   private final ShuffleboardTab m_tab;
   private SendableChooser<Command> m_chosenAuto = new SendableChooser<>();
+  private GenericEntry m_delay;
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -76,19 +79,20 @@ public RobotContainer() {
     // Auto chooser setup
     m_tab = Shuffleboard.getTab("Auto");
 
+    m_delay = m_tab.add("Delay", 0).getEntry();
+
     m_chosenAuto.setDefaultOption("Shoot and drive from middle",
-      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)))); //used to be an empty Pose2D (should not change but maybe the empty pose has something to do with it)
+      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); //used to be an empty Pose2D (should not change but maybe the empty pose has something to do with it)
     m_chosenAuto.addOption("Shoot and drive from left",
-      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(45))));
+      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(55)), m_delay.getDouble(0)));
     m_chosenAuto.addOption("Shoot and drive from right",
-      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(-45))));
+      new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(-55)), m_delay.getDouble(0)));
     m_chosenAuto.addOption("Drive, turn, and shoot from left",
-      new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), 55));
+      new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), 55, m_delay.getDouble(0)));
     m_chosenAuto.addOption("Drive, turn, and shoot from right",
-      new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), -55));
-
+      new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), -55, m_delay.getDouble(0)));
     m_chosenAuto.addOption("Drive back only",
-      new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)))); //drive back only auto
+      new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); //drive back only auto
 
     m_tab.add(m_chosenAuto);
 
@@ -146,7 +150,9 @@ boolean getFastMode() {
     return fastMode;
   }
 
-  double getRightX() {return m_driverController.getRightX();}
+  double getRightX() {
+    System.out.println("DELAY: " + m_delay.getDouble(0));
+    return m_driverController.getRightX();}
   double getLeftX() {return -m_driverController.getLeftX();}
   double getLeftY() {return -m_driverController.getLeftY();}
   double getPOV() {return m_driverController.getPOV();}

diff --git a/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java b/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.SetPoseCommand;
 import frc.robot.commands.ShootCommand;
 import frc.robot.commands.SwerveDriveCommand;
@@ -18,8 +19,9 @@
 import frc.robot.subsystems.SwerveSubsystem;
 
 public class DriveAutoCommand extends SequentialCommandGroup {
-  public DriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset) {
+  public DriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double delay) {
     super(
+      new WaitCommand(delay),
       new SetPoseCommand(swerve, startingOffset).withTimeout(0),
       new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyDouble(-1))
       .withTimeout(3),

diff --git a/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java b/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.SetPoseCommand;
 import frc.robot.commands.ShootCommand;
 import frc.robot.commands.SwerveDriveCommand;
@@ -18,8 +19,9 @@
 import frc.robot.subsystems.SwerveSubsystem;
 
 public class ShootAndDriveAutoCommand extends SequentialCommandGroup {
-  public ShootAndDriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset) {
+  public ShootAndDriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double delay) {
     super(
+      new WaitCommand(delay),
       new ShootCommand(shoot).withTimeout(4),
       new SetPoseCommand(swerve, startingOffset).withTimeout(0),
       new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyDouble(-1))

diff --git a/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java b/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.SetPoseCommand;
 import frc.robot.commands.ShootCommand;
 import frc.robot.commands.SwerveDriveCommand;
@@ -18,8 +19,9 @@
 import frc.robot.subsystems.SwerveSubsystem;
 
 public class TurnAndShootAutoCommand extends SequentialCommandGroup {
-  public TurnAndShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double turnDegrees) {
+  public TurnAndShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double turnDegrees, double delay) {
     super(
+      new WaitCommand(delay),
       new SetPoseCommand(swerve, startingOffset).withTimeout(0),
       new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyDouble(-1))
       .withTimeout(1.9),