start getting rid of weird original system and replacing it with comm…

…and based
redshiftrobotics · Dec 12, 2024 · f9831ef · f9831ef
1 parent 609fa15
commit f9831ef
Show file tree

Hide file tree

Showing 28 changed files with 317 additions and 507 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -20,7 +20,7 @@ private Constants() {}
 
   public static final boolean SHOW_SYS_ID_AUTOS = false;
 
-  private static RobotType robotType = RobotType.TEST_BOT;
+  private static RobotType robotType = RobotType.SIM_BOT;
 
   public static RobotType getRobot() {
     if (RobotBase.isReal() && robotType == RobotType.SIM_BOT) {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -21,11 +21,6 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.Mode;
 import frc.robot.Constants.RobotType;
-import frc.robot.subsystems.cannon.CannonConstants;
-import frc.robot.subsystems.cannon.CannonIO;
-import frc.robot.subsystems.cannon.CannonIOHardware;
-import frc.robot.subsystems.cannon.CannonIOSim;
-import frc.robot.subsystems.cannon.FiringTube;
 import frc.robot.subsystems.dashboard.DriverDashboard;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveConstants;
@@ -36,14 +31,20 @@
 import frc.robot.subsystems.drive.controllers.SpeedController;
 import frc.robot.subsystems.drive.controllers.SpeedController.SpeedLevel;
 import frc.robot.subsystems.drive.controllers.TeleopDriveController;
-import frc.robot.subsystems.gateway.GatewayIO;
-import frc.robot.subsystems.gateway.GatewayIOHardware;
-import frc.robot.subsystems.gateway.GatewayIOSim;
-import frc.robot.subsystems.gateway.GatewayTank;
-import frc.robot.subsystems.reservoir.ReservoirIO;
-import frc.robot.subsystems.reservoir.ReservoirIOHardware;
-import frc.robot.subsystems.reservoir.ReservoirIOSim;
-import frc.robot.subsystems.reservoir.ReservoirTank;
+import frc.robot.subsystems.pneumatics.cannon.CannonConstants;
+import frc.robot.subsystems.pneumatics.cannon.CannonIO;
+import frc.robot.subsystems.pneumatics.cannon.CannonIOHardware;
+import frc.robot.subsystems.pneumatics.cannon.CannonIOSim;
+import frc.robot.subsystems.pneumatics.cannon.FiringTube;
+import frc.robot.subsystems.pneumatics.gateway.GatewayIO;
+import frc.robot.subsystems.pneumatics.gateway.GatewayIOHardware;
+import frc.robot.subsystems.pneumatics.gateway.GatewayIOSim;
+import frc.robot.subsystems.pneumatics.gateway.GatewayTank;
+import frc.robot.subsystems.pneumatics.reservoir.MaintainPressure;
+import frc.robot.subsystems.pneumatics.reservoir.ReservoirIO;
+import frc.robot.subsystems.pneumatics.reservoir.ReservoirIOHardware;
+import frc.robot.subsystems.pneumatics.reservoir.ReservoirIOSim;
+import frc.robot.subsystems.pneumatics.reservoir.ReservoirTank;
 import frc.robot.subsystems.vision.AprilTagVision;
 import frc.robot.utility.NormUtil;
 import frc.robot.utility.OverrideSwitch;
@@ -96,8 +97,7 @@ public RobotContainer() {
         gatewayTank = new GatewayTank(new GatewayIOHardware());
         firingTube =
             new FiringTube(
-                new CannonIOHardware(CannonConstants.MIDDLE_FIRING_TUBE_SOLENOID_CHANNEL),
-                "Main");
+                new CannonIOHardware(CannonConstants.MIDDLE_FIRING_TUBE_SOLENOID_CHANNEL), "Main");
         break;
 
       case SIM_BOT:
@@ -159,20 +159,23 @@ public RobotContainer() {
         });
 
     // Set up reservoir tank
-    reservoirTank.setPressureThresholds(20, 30);
-    reservoirTank.addPauseFillingCondition(
-        () ->
-            NormUtil.norm(drive.getRobotSpeeds()) > drive.getMaxLinearSpeedMetersPerSec() * 0.75
-                || Math.abs(drive.getRobotSpeeds().omegaRadiansPerSecond)
-                    > drive.getMaxAngularSpeedRadPerSec() * 0.75,
-        "Drive Speed High",
-        0.3);
+
+    reservoirTank.setDefaultCommand(
+        new MaintainPressure(reservoirTank, 20, 30).withName("MaintainPressure"));
+
+    new Trigger(
+            () ->
+                NormUtil.norm(drive.getRobotSpeeds()) > drive.getMaxLinearSpeedMetersPerSec() * 0.75
+                    || Math.abs(drive.getRobotSpeeds().omegaRadiansPerSecond)
+                        > drive.getMaxAngularSpeedRadPerSec() * 0.75)
+        .debounce(0.3)
+        .whileTrue(Commands.idle(reservoirTank).withName("Pause: Drive High Speed"));
+
     reservoirTank.setSimDrain(gatewayTank::isFilling);
 
-    // Set up gateway tank
-    gatewayTank.setDesiredPSI(20);
-    gatewayTank.addPauseFillingCondition(
-        () -> firingTube.isWaitingToFire() || firingTube.isOpen(), "Firing Tube Open", 0.5);
+    new Trigger(() -> firingTube.isWaitingToFire() || firingTube.isOpen())
+        .debounce(0.5)
+        .whileTrue(Commands.idle(gatewayTank).withName("Pause: Firing Tube Open"));
     gatewayTank.setSimDrain(firingTube::isOpen);
 
     // Set up firing tube
@@ -215,11 +218,11 @@ private void initDashboard() {
     dashboard.setFieldRelativeSupplier(() -> false);
     dashboard.setAngleDrivenSupplier(() -> false);
 
-    dashboard.setReservoirTank(
-        reservoirTank::isFilling, reservoirTank::getPressure, reservoirTank::getStatusString);
+    dashboard.addSubsystem(reservoirTank);
+    dashboard.setReservoirTank(reservoirTank::isFilling, reservoirTank::getPressure);
 
-    dashboard.setGatewayTank(
-        gatewayTank::isFilling, gatewayTank::getPressure, gatewayTank::getStatusString);
+    dashboard.addSubsystem(gatewayTank);
+    dashboard.setGatewayTank(gatewayTank::isFilling, gatewayTank::getPressure);
 
     dashboard.setCannon(
         gatewayTank::isPressureWithinTolerance,
@@ -460,24 +463,13 @@ private void configureOperatorControllerBindings() {
     if (operatorController instanceof CommandXboxController) {
       final CommandXboxController operatorXbox = (CommandXboxController) operatorController;
 
-      // TODO, this should be the other way around, pressing buttons should put command that pause
-      // the filling and in general more commands should be used to greatly simplify code.
+      operatorXbox.y().whileTrue(Commands.idle(reservoirTank).withName("Pause: Operator Y Button"));
 
-      reservoirTank.addPauseFillingCondition(operatorXbox.y(), "Operator Y Button", 0);
-
-      gatewayTank.addPauseFillingCondition(operatorXbox.leftTrigger(), "Operator Prepare Fire", 0);
+      operatorXbox
+          .leftTrigger()
+          .whileTrue(Commands.idle(gatewayTank).withName("Pause: Operator Prepare Fire"));
 
       operatorXbox.rightTrigger().onTrue(firingTube.runOnce(firingTube::fire).withName("Fire"));
-
-      gatewayTank.setDefaultCommand(
-          gatewayTank
-              .run(
-                  () -> {
-                    gatewayTank.setTargetLaunchDistance(
-                        gatewayTank.getTargetLaunchDistance()
-                            - MathUtil.applyDeadband(operatorXbox.getLeftY(), 0.1) * 0.4);
-                  })
-              .withName("GatewayTankSeDistance"));
     }
   }
 

diff --git a/src/main/java/frc/robot/subsystems/dashboard/DriverDashboard.java b/src/main/java/frc/robot/subsystems/dashboard/DriverDashboard.java
@@ -35,11 +35,9 @@ public static DriverDashboard getInstance() {
 
   private BooleanSupplier reservoirTankFilling;
   private Supplier<Double> reservoirTankPressure;
-  private Supplier<String> reservoirTankStatus;
 
   private BooleanSupplier gatewayTankFilling;
   private Supplier<Double> gatewayTankPressure;
-  private Supplier<String> gatewayTankStatus;
 
   private BooleanSupplier readyToFireSupplier;
   private DoubleSupplier targetLaunchDistance;
@@ -51,7 +49,9 @@ public void addSubsystem(SubsystemBase subsystem) {
     if (subsystem instanceof Drive) {
       SmartDashboard.putData("Drive Subsystem", subsystem);
     } else {
-      throw new IllegalArgumentException("Unknown subsystem can not be added to driver dashboard");
+      // throw new IllegalArgumentException("Unknown subsystem can not be added to driver
+      // dashboard");
+      SmartDashboard.putData(subsystem);
     }
   }
 
@@ -80,21 +80,15 @@ public void setAngleDrivenSupplier(BooleanSupplier angleDrivenSupplier) {
   }
 
   public void setReservoirTank(
-      BooleanSupplier reservoirTankFilling,
-      Supplier<Double> reservoirTankPressure,
-      Supplier<String> reservoirTankStatus) {
+      BooleanSupplier reservoirTankFilling, Supplier<Double> reservoirTankPressure) {
     this.reservoirTankFilling = reservoirTankFilling;
     this.reservoirTankPressure = reservoirTankPressure;
-    this.reservoirTankStatus = reservoirTankStatus;
   }
 
   public void setGatewayTank(
-      BooleanSupplier gatewayTankFilling,
-      Supplier<Double> gatewayTankPressure,
-      Supplier<String> gatewayTankStatus) {
+      BooleanSupplier gatewayTankFilling, Supplier<Double> gatewayTankPressure) {
     this.gatewayTankFilling = gatewayTankFilling;
     this.gatewayTankPressure = gatewayTankPressure;
-    this.gatewayTankStatus = gatewayTankStatus;
   }
 
   public void setCannon(
@@ -149,10 +143,6 @@ public void periodic() {
       SmartDashboard.putNumber("Reservoir Pressure", reservoirTankPressure.get());
     }
 
-    if (reservoirTankStatus != null) {
-      SmartDashboard.putString("Reservoir Status", reservoirTankStatus.get());
-    }
-
     if (gatewayTankFilling != null) {
       SmartDashboard.putBoolean("Gateway Filling", gatewayTankFilling.getAsBoolean());
     }
@@ -161,10 +151,6 @@ public void periodic() {
       SmartDashboard.putNumber("Gateway Pressure", gatewayTankPressure.get());
     }
 
-    if (gatewayTankStatus != null) {
-      SmartDashboard.putString("Gateway Status", gatewayTankStatus.get());
-    }
-
     if (readyToFireSupplier != null) {
       SmartDashboard.putBoolean("Fire Accurate", readyToFireSupplier.getAsBoolean());
     }