From 3d8afb9e330183bcab42ea59595ccc76048ff920 Mon Sep 17 00:00:00 2001
From: Garrett Summerfield <garrettsummerfi3ld@gmail.com>
Date: Sun, 18 Feb 2024 22:09:23 -0600
Subject: [PATCH] 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        |  4 ++--
 .../java/frc/robot/commands/DumpControl.java  |  8 ++++----
 src/main/java/frc/robot/subsystems/Dump.java  |  9 +++++++++
 src/main/java/frc/robot/util/Time.java        | 20 +++++++++++++++++++
 4 files changed, 35 insertions(+), 6 deletions(-)
 create mode 100644 src/main/java/frc/robot/util/Time.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 4edcb8f..eadaa16 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/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 {
diff --git a/src/main/java/frc/robot/commands/DumpControl.java b/src/main/java/frc/robot/commands/DumpControl.java
index 8d2efd0..e801fb8 100644
--- a/src/main/java/frc/robot/commands/DumpControl.java
+++ b/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();
     }
   }
diff --git a/src/main/java/frc/robot/subsystems/Dump.java b/src/main/java/frc/robot/subsystems/Dump.java
index a1b0411..579c51a 100644
--- a/src/main/java/frc/robot/subsystems/Dump.java
+++ b/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
diff --git a/src/main/java/frc/robot/util/Time.java b/src/main/java/frc/robot/util/Time.java
new file mode 100644
index 0000000..ee441b8
--- /dev/null
+++ b/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
+    }
+  }
+}