Gateway subsystem

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);
+  }
+}

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) {}

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 {
+  //
+}