Add double dump command

Added a subsystem command to activate the dump twice automatically. This is now the default command for the copilot controller.

Modified the dump constants to reflect opening and closing of the DoubleSolenoid.

Added a Time utility class that can assist in performing actions and also maintain other time functions.

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

@@ -39,8 +39,8 @@ public static final class PneumaticsConstants {
     public static final class DumpConstants {
 
       // Uppy Downy solonoid
-      public static final int OUT = 0;
-      public static final int IN = 1;
+      public static final int OUT = 1;
+      public static final int IN = 0;
     }
 
     public static final class ElevatorConstants {

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

@@ -13,7 +13,7 @@ public class DumpControl extends Command {
 
   private static Dump DumpSubsystem;
 
-  private static Joystick Controller = new Joystick(OperatorConstants.COPILOT_CONTROLLER);
+  private static Joystick DumpJoystick = new Joystick(OperatorConstants.COPILOT_CONTROLLER);
 
   /** Creates a new PneumaticsControl. */
   public DumpControl(Dump subsystem) {
@@ -30,10 +30,10 @@ public void initialize() {}
   @Override
   public void execute() {
     // TODO: Change this to the correct button
-    if (Controller.getRawButton(1)) {
-      DumpSubsystem.open();
+    if (DumpJoystick.getRawButton(1)) {
+      DumpSubsystem.openThenClose();
       // TODO: Change this to the correct button
-    } else if (Controller.getRawButton(2)) {
+    } else if (DumpJoystick.getRawButton(2)) {
       DumpSubsystem.close();
     }
   }

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

@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.PneumaticsConstants;
 import frc.robot.Constants.PneumaticsConstants.DumpConstants;
+import frc.robot.util.Time;
 
 public class Dump extends SubsystemBase {
 
@@ -39,6 +40,14 @@ public void close() {
     SmartDashboard.putBoolean(getName(), false);
   }
 
+  /** Opens the piston, waits after a delay, then closes the piston */
+  public void openThenClose() {
+    Time.performThenWait(this::open, 2.0);
+    Time.performThenWait(this::close, 0.5);
+    Time.performThenWait(this::open, 1);
+    close();
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

src/main/java/frc/robot/util/Time.java

@@ -0,0 +1,20 @@
+package frc.robot.util;
+
+import edu.wpi.first.wpilibj.Timer;
+
+public class Time {
+
+  /**
+   * Performs an action then waits for a specified amount of time
+   *
+   * @param action The action to perform
+   * @param delay The delay in seconds
+   */
+  public static void performThenWait(Runnable action, double delay) {
+    double timer = Timer.getFPGATimestamp();
+    action.run();
+    while (Timer.getFPGATimestamp() - timer < delay) {
+      // Wait for the delay
+    }
+  }
+}