From d2bc1e7d8ae48f5b48a34d089d3967bfb73a8fc5 Mon Sep 17 00:00:00 2001
From: "Aceius E." <aceiuselms@seattleacademy.org>
Date: Tue, 19 Nov 2024 16:42:43 -0800
Subject: [PATCH] Gateway subsystem

---
 .../frc/robot/subsystems/gateway/Gateway.java | 42 +++++++++++++++++++
 .../robot/subsystems/gateway/GatewayIO.java   |  1 +
 .../subsystems/gateway/GatewayIOHardware.java |  4 +-
 3 files changed, 46 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/frc/robot/subsystems/gateway/Gateway.java

diff --git a/src/main/java/frc/robot/subsystems/gateway/Gateway.java b/src/main/java/frc/robot/subsystems/gateway/Gateway.java
new file mode 100644
index 0000000..34eb698
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/gateway/Gateway.java
@@ -0,0 +1,42 @@
+package frc.robot.subsystems.gateway;
+
+import edu.wpi.first.math.controller.BangBangController;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Gateway extends SubsystemBase {
+  private GatewayIO IO;
+  private GatewayIOInputAutoLogged input;
+  private BangBangController controller;
+  
+  public Gateway(GatewayIO gatewayIO) {
+    IO = gatewayIO;
+  }
+  
+  @Override
+  public void periodic() {
+    if (controller.calculate(input.psi, getTargetPsi()) == 1) {
+      IO.beginFilling();
+    } else {
+      IO.stopFilling();
+    }
+
+    IO.updateInputs(input);
+  }
+
+  public float getTargetPsi() {
+    return input.targetPsi;
+  }
+
+  public void setTargetPsi(int targetPsi) {
+    input.targetPsi = targetPsi;
+  }
+
+  public void stopFilling() {
+    input.targetPsi = input.psi;
+    IO.stopFilling();
+  }
+
+  public void fireCannon(byte cannonId) {
+    IO.fireCannon(cannonId);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/gateway/GatewayIO.java b/src/main/java/frc/robot/subsystems/gateway/GatewayIO.java
index 7cb6070..af502ef 100644
--- a/src/main/java/frc/robot/subsystems/gateway/GatewayIO.java
+++ b/src/main/java/frc/robot/subsystems/gateway/GatewayIO.java
@@ -7,6 +7,7 @@ public interface GatewayIO {
   public static class GatewayIOInput {
     boolean filling;
     float psi;
+    float targetPsi;
   }
 
   public default void updateInputs(GatewayIOInput inputs) {}
diff --git a/src/main/java/frc/robot/subsystems/gateway/GatewayIOHardware.java b/src/main/java/frc/robot/subsystems/gateway/GatewayIOHardware.java
index ad0015b..1350159 100644
--- a/src/main/java/frc/robot/subsystems/gateway/GatewayIOHardware.java
+++ b/src/main/java/frc/robot/subsystems/gateway/GatewayIOHardware.java
@@ -1,3 +1,5 @@
 package frc.robot.subsystems.gateway;
 
-public class GatewayIOHardware implements GatewayIO {}
+public class GatewayIOHardware implements GatewayIO {
+  //
+}