From f9831ef1e3a2e17c3f854e91bf441056a9872621 Mon Sep 17 00:00:00 2001 From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com> Date: Wed, 11 Dec 2024 23:44:40 -0800 Subject: [PATCH] start getting rid of weird original system and replacing it with command based --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 84 ++++--- .../subsystems/dashboard/DriverDashboard.java | 24 +- .../robot/subsystems/gateway/GatewayTank.java | 215 ------------------ .../pneumatics/ControlDistance.java | 58 +++++ .../pneumatics/MaintainPressure.java | 44 ++++ .../cannon/CannonConstants.java | 2 +- .../{ => pneumatics}/cannon/CannonIO.java | 2 +- .../cannon/CannonIOHardware.java | 2 +- .../{ => pneumatics}/cannon/CannonIOSim.java | 2 +- .../{ => pneumatics}/cannon/FiringSystem.java | 2 +- .../{ => pneumatics}/cannon/FiringTube.java | 16 +- .../gateway/GatewayConstants.java | 2 +- .../{ => pneumatics}/gateway/GatewayIO.java | 6 +- .../gateway/GatewayIOHardware.java | 6 +- .../gateway/GatewayIOSim.java | 8 +- .../pneumatics/gateway/GatewayTank.java | 77 +++++++ .../gateway/InterpolatingShotTable.java | 2 +- .../hardwareWrappers/PressureSwitch.java | 2 +- .../hardwareWrappers/RelaySolenoid.java | 2 +- .../hardwareWrappers/Transducer.java | 2 +- .../reservoir/ReservoirConstants.java | 2 +- .../reservoir/ReservoirIO.java | 2 +- .../reservoir/ReservoirIOHardware.java | 4 +- .../reservoir/ReservoirIOSim.java | 2 +- .../pneumatics/reservoir/ReservoirTank.java | 63 +++++ .../subsystems/reservoir/ReservoirTank.java | 186 --------------- .../robot/utility/ThresholdController.java | 5 + 28 files changed, 317 insertions(+), 507 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/gateway/GatewayTank.java create mode 100644 src/main/java/frc/robot/subsystems/pneumatics/ControlDistance.java create mode 100644 src/main/java/frc/robot/subsystems/pneumatics/MaintainPressure.java rename src/main/java/frc/robot/subsystems/{ => pneumatics}/cannon/CannonConstants.java (77%) rename src/main/java/frc/robot/subsystems/{ => pneumatics}/cannon/CannonIO.java (85%) rename src/main/java/frc/robot/subsystems/{ => pneumatics}/cannon/CannonIOHardware.java (91%) rename src/main/java/frc/robot/subsystems/{ => pneumatics}/cannon/CannonIOSim.java (87%) rename src/main/java/frc/robot/subsystems/{ => pneumatics}/cannon/FiringSystem.java (93%) rename src/main/java/frc/robot/subsystems/{ => pneumatics}/cannon/FiringTube.java (74%) rename src/main/java/frc/robot/subsystems/{ => pneumatics}/gateway/GatewayConstants.java (87%) rename src/main/java/frc/robot/subsystems/{ => pneumatics}/gateway/GatewayIO.java (80%) rename src/main/java/frc/robot/subsystems/{ => pneumatics}/gateway/GatewayIOHardware.java (80%) rename src/main/java/frc/robot/subsystems/{ => pneumatics}/gateway/GatewayIOSim.java (86%) create mode 100644 src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayTank.java rename src/main/java/frc/robot/subsystems/{ => pneumatics}/gateway/InterpolatingShotTable.java (96%) rename src/main/java/frc/robot/{ => subsystems/pneumatics}/hardwareWrappers/PressureSwitch.java (95%) rename src/main/java/frc/robot/{ => subsystems/pneumatics}/hardwareWrappers/RelaySolenoid.java (90%) rename src/main/java/frc/robot/{ => subsystems/pneumatics}/hardwareWrappers/Transducer.java (95%) rename src/main/java/frc/robot/subsystems/{ => pneumatics}/reservoir/ReservoirConstants.java (78%) rename src/main/java/frc/robot/subsystems/{ => pneumatics}/reservoir/ReservoirIO.java (93%) rename src/main/java/frc/robot/subsystems/{ => pneumatics}/reservoir/ReservoirIOHardware.java (85%) rename src/main/java/frc/robot/subsystems/{ => pneumatics}/reservoir/ReservoirIOSim.java (96%) create mode 100644 src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirTank.java delete mode 100644 src/main/java/frc/robot/subsystems/reservoir/ReservoirTank.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3058f0e..e2aaa75 100644 --- 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 index ff3d68d..4720918 100644 --- 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 index 0f25cdf..1d3c9be 100644 --- 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 reservoirTankPressure; - private Supplier reservoirTankStatus; private BooleanSupplier gatewayTankFilling; private Supplier gatewayTankPressure; - private Supplier 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 reservoirTankPressure, - Supplier reservoirTankStatus) { + BooleanSupplier reservoirTankFilling, Supplier reservoirTankPressure) { this.reservoirTankFilling = reservoirTankFilling; this.reservoirTankPressure = reservoirTankPressure; - this.reservoirTankStatus = reservoirTankStatus; } public void setGatewayTank( - BooleanSupplier gatewayTankFilling, - Supplier gatewayTankPressure, - Supplier gatewayTankStatus) { + BooleanSupplier gatewayTankFilling, Supplier 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()); } diff --git a/src/main/java/frc/robot/subsystems/gateway/GatewayTank.java b/src/main/java/frc/robot/subsystems/gateway/GatewayTank.java deleted file mode 100644 index c2a8e68..0000000 --- a/src/main/java/frc/robot/subsystems/gateway/GatewayTank.java +++ /dev/null @@ -1,215 +0,0 @@ -package frc.robot.subsystems.gateway; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.Constants.RobotType; -import frc.robot.utility.ThresholdController; -import java.util.ArrayList; -import java.util.Optional; -import java.util.function.BooleanSupplier; -import org.littletonrobotics.junction.Logger; - -/** - * Subsystem of t-shirt cannon robot representing the gateway tank and the value which fills it, - * along with the sensors on the gateway tank. - */ -public class GatewayTank extends SubsystemBase { - - private final GatewayIO io; - private final GatewayIOInputsAutoLogged inputs = new GatewayIOInputsAutoLogged(); - - private final ThresholdController controller; - - private final InterpolatingShotTable shotTable; - - private final ArrayList pauseConditions; - - private double targetShotDistance; - - /** Create a new GatewayTank subsystem */ - public GatewayTank(GatewayIO io) { - this.io = io; - - pauseConditions = new ArrayList<>(); - - controller = new ThresholdController(); - - shotTable = new InterpolatingShotTable(); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("GatewayTank", inputs); - - if (Constants.getRobot() == RobotType.TEST_BOT) { - return; - } - - if (controller.calculate(inputs.tankPSI) > 0 - && !shouldPauseFilling() - && DriverStation.isEnabled()) { - io.beganFilling(); - } else { - io.stopFilling(); - } - - Logger.recordOutput("GatewayTank/status", getStatusString()); - } - - // --- Getters --- - - /** - * Get whether compressor is currently active and filling Gateway tank - * - * @return compressor state - */ - public boolean isFilling() { - return inputs.isFilling; - } - - /** - * Get current pressure of gateway tank - * - * @return pressure in psi (pound per square inch) - */ - public double getPressure() { - return inputs.tankPSI; - } - - /** - * Get whether pressure is within the tolerance range - * - * @return true if pressure is within tolerance range, false otherwise - */ - public boolean isPressureWithinTolerance() { - return MathUtil.isNear( - getPressure(), - shotTable.getDesiredPSI(targetShotDistance), - GatewayConstants.TOLERANCE_PRESSURE); - } - - /** - * Get the estimated distance the t-shirt will travel based on the current pressure in the gateway - * - * @return distance in meters - */ - public double getEstimatedLaunchDistance() { - return shotTable.getEstimatedLaunchDistance(inputs.tankPSI); - } - - /** - * Get the target distance the t-shirt will travel - * - * @return distance in meters - */ - public double getTargetLaunchDistance() { - return targetShotDistance; - } - - // --- Setters --- - - /** - * Set the target distance the t-shirt will travel, changes target pressure of the gateway tank - * - * @param distance in meters - */ - public void setTargetLaunchDistance(double distance) { - distance = - MathUtil.clamp( - distance, GatewayConstants.MIN_SHOT_DISTANCE, GatewayConstants.MAX_SHOT_DISTANCE); - setDesiredPSI(shotTable.getDesiredPSI(distance)); - } - - /** - * Set the target pressure of the gateway tank - * - * @param psi target pressure in psi (pound per square inch) - */ - public void setDesiredPSI(double psi) { - double min = GatewayConstants.MIN_ALLOWED_PRESSURE; - double max = GatewayConstants.MAX_ALLOWED_PRESSURE; - double tolerance = GatewayConstants.TOLERANCE_PRESSURE; - - psi = MathUtil.clamp(psi, min, max); - - controller.setThresholds(Math.max(psi - tolerance, min), psi); - - targetShotDistance = shotTable.getEstimatedLaunchDistance(psi); - } - - /** Sets the setpoint pressure to none. The compressor will not activate. */ - public void stopFilling() { - controller.setThresholds(0, 0); - } - - // --- Sim Drain --- - - /** - * Sets a supplier that tells the sim whether it is draining. - * - * @param isDrainingSupplier A supplier that returns true if the tank is draining air. - */ - public void setSimDrain(BooleanSupplier isDrainingSupplier) { - this.io.setSimDrain(isDrainingSupplier); - } - - // --- Pause Conditions --- - - public void addPauseFillingCondition(BooleanSupplier condition, String reason) { - pauseConditions.add(new PauseCondition(condition, reason)); - } - - public void addPauseFillingCondition( - BooleanSupplier condition, String reason, double debounceTimeSeconds) { - final Debouncer debouncer = new Debouncer(debounceTimeSeconds, DebounceType.kBoth); - pauseConditions.add( - new PauseCondition(() -> debouncer.calculate(condition.getAsBoolean()), reason)); - } - - public boolean shouldPauseFilling() { - return pauseConditions.stream().anyMatch(PauseCondition::isActive); - } - - public Optional getPauseReason() { - return pauseConditions.stream() - .filter(PauseCondition::isActive) - .map(PauseCondition::reason) - .findFirst(); - } - - public String getStatusString() { - if (isFilling()) { - return String.format("Filling to %.2f PSI (End threshold)", controller.getUpperThreshold()); - } else if (!controller.isOn() && getPressure() > controller.getLowerThreshold()) { - return String.format( - "Stopped till %.2f PSI (Start threshold)", controller.getLowerThreshold()); - } - if (shouldPauseFilling()) { - return "Paused: " + getPauseReason().orElse("Unknown"); - } - return "Idle"; - } - - public static record PauseCondition(BooleanSupplier condition, String reason) { - public boolean isActive() { - return condition.getAsBoolean(); - } - } - - // --- Testing --- - - public void forceOpen() { - System.out.println("Opening Gateway Tank"); - io.beganFilling(); - } - - public void forceClose() { - System.out.println("Closing Gateway Tank"); - io.stopFilling(); - } -} diff --git a/src/main/java/frc/robot/subsystems/pneumatics/ControlDistance.java b/src/main/java/frc/robot/subsystems/pneumatics/ControlDistance.java new file mode 100644 index 0000000..5432d9e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pneumatics/ControlDistance.java @@ -0,0 +1,58 @@ +package frc.robot.subsystems.pneumatics; + +import java.util.function.Supplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.pneumatics.gateway.GatewayTank; +import frc.robot.utility.ThresholdController; + +public class ControlDistance extends Command { + + private final GatewayTank tank; + + private final ThresholdController controller; + + private final Supplier distanceChange; + + private double targetDistance; + + public ControlDistance(GatewayTank tank, Supplier distanceChange) { + + this.tank = tank; + + controller = new ThresholdController(0.0, 0.0); + + this.distanceChange = distanceChange; + + addRequirements(tank); + } + + @Override + public void initialize() { + controller.reset(); + } + + @Override + public void execute() { + targetDistance += MathUtil.applyDeadband(distanceChange.get(), 0.1); + + // controller.setThresholds(targetDistance, ); + + if (controller.calculate(tank.getPressure()) > 0) { + tank.startFilling(); + } else { + tank.stopFilling(); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + tank.stopFilling(); + } +} diff --git a/src/main/java/frc/robot/subsystems/pneumatics/MaintainPressure.java b/src/main/java/frc/robot/subsystems/pneumatics/MaintainPressure.java new file mode 100644 index 0000000..6a7ea8f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pneumatics/MaintainPressure.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems.pneumatics; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.pneumatics.reservoir.ReservoirTank; +import frc.robot.utility.ThresholdController; + +public class MaintainPressure extends Command { + + private final ReservoirTank tank; + + private final ThresholdController controller; + + public MaintainPressure(ReservoirTank tank, double lowerThreshold, double upperThreshold) { + + this.tank = tank; + controller = new ThresholdController(lowerThreshold, upperThreshold); + + addRequirements(tank); + } + + @Override + public void initialize() { + controller.reset(); + } + + @Override + public void execute() { + if (controller.calculate(tank.getPressure()) > 0) { + tank.startFilling(); + } else { + tank.stopFilling(); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + tank.stopFilling(); + } +} diff --git a/src/main/java/frc/robot/subsystems/cannon/CannonConstants.java b/src/main/java/frc/robot/subsystems/pneumatics/cannon/CannonConstants.java similarity index 77% rename from src/main/java/frc/robot/subsystems/cannon/CannonConstants.java rename to src/main/java/frc/robot/subsystems/pneumatics/cannon/CannonConstants.java index c658d87..d92d8a2 100644 --- a/src/main/java/frc/robot/subsystems/cannon/CannonConstants.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/cannon/CannonConstants.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.cannon; +package frc.robot.subsystems.pneumatics.cannon; public class CannonConstants { public static final double FIRE_TUBE_OPEN_TIME_SECONDS = 0.2; diff --git a/src/main/java/frc/robot/subsystems/cannon/CannonIO.java b/src/main/java/frc/robot/subsystems/pneumatics/cannon/CannonIO.java similarity index 85% rename from src/main/java/frc/robot/subsystems/cannon/CannonIO.java rename to src/main/java/frc/robot/subsystems/pneumatics/cannon/CannonIO.java index 1badcde..0c5a771 100644 --- a/src/main/java/frc/robot/subsystems/cannon/CannonIO.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/cannon/CannonIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.cannon; +package frc.robot.subsystems.pneumatics.cannon; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/cannon/CannonIOHardware.java b/src/main/java/frc/robot/subsystems/pneumatics/cannon/CannonIOHardware.java similarity index 91% rename from src/main/java/frc/robot/subsystems/cannon/CannonIOHardware.java rename to src/main/java/frc/robot/subsystems/pneumatics/cannon/CannonIOHardware.java index d021197..fb8d662 100644 --- a/src/main/java/frc/robot/subsystems/cannon/CannonIOHardware.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/cannon/CannonIOHardware.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.cannon; +package frc.robot.subsystems.pneumatics.cannon; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.Solenoid; diff --git a/src/main/java/frc/robot/subsystems/cannon/CannonIOSim.java b/src/main/java/frc/robot/subsystems/pneumatics/cannon/CannonIOSim.java similarity index 87% rename from src/main/java/frc/robot/subsystems/cannon/CannonIOSim.java rename to src/main/java/frc/robot/subsystems/pneumatics/cannon/CannonIOSim.java index 5a95faf..e21807f 100644 --- a/src/main/java/frc/robot/subsystems/cannon/CannonIOSim.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/cannon/CannonIOSim.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.cannon; +package frc.robot.subsystems.pneumatics.cannon; public class CannonIOSim implements CannonIO { diff --git a/src/main/java/frc/robot/subsystems/cannon/FiringSystem.java b/src/main/java/frc/robot/subsystems/pneumatics/cannon/FiringSystem.java similarity index 93% rename from src/main/java/frc/robot/subsystems/cannon/FiringSystem.java rename to src/main/java/frc/robot/subsystems/pneumatics/cannon/FiringSystem.java index 890e028..08875b5 100644 --- a/src/main/java/frc/robot/subsystems/cannon/FiringSystem.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/cannon/FiringSystem.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.cannon; +package frc.robot.subsystems.pneumatics.cannon; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.Arrays; diff --git a/src/main/java/frc/robot/subsystems/cannon/FiringTube.java b/src/main/java/frc/robot/subsystems/pneumatics/cannon/FiringTube.java similarity index 74% rename from src/main/java/frc/robot/subsystems/cannon/FiringTube.java rename to src/main/java/frc/robot/subsystems/pneumatics/cannon/FiringTube.java index 7dc9045..325174e 100644 --- a/src/main/java/frc/robot/subsystems/cannon/FiringTube.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/cannon/FiringTube.java @@ -1,6 +1,5 @@ -package frc.robot.subsystems.cannon; +package frc.robot.subsystems.pneumatics.cannon; -import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.BooleanSupplier; @@ -73,17 +72,4 @@ public void fire() { public void setFireRequirements(BooleanSupplier fireRequirement) { this.fireRequirement = fireRequirement; } - - /** - * Adds a requirement for the firing tube to open. Supplier must return true for the firing tube - * to open. - * - * @param fireRequirement the requirement for the firing tube to open - * @param debounceTimeSeconds the time in seconds the fire requirement must be true before the - * firing tube opens - */ - public void setFireRequirements(BooleanSupplier fireRequirement, double debounceTimeSeconds) { - final Debouncer debouncer = new Debouncer(debounceTimeSeconds); - this.fireRequirement = () -> debouncer.calculate(fireRequirement.getAsBoolean()); - } } diff --git a/src/main/java/frc/robot/subsystems/gateway/GatewayConstants.java b/src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayConstants.java similarity index 87% rename from src/main/java/frc/robot/subsystems/gateway/GatewayConstants.java rename to src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayConstants.java index 5f1154d..663db4b 100644 --- a/src/main/java/frc/robot/subsystems/gateway/GatewayConstants.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayConstants.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.gateway; +package frc.robot.subsystems.pneumatics.gateway; public class GatewayConstants { private GatewayConstants() {} diff --git a/src/main/java/frc/robot/subsystems/gateway/GatewayIO.java b/src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayIO.java similarity index 80% rename from src/main/java/frc/robot/subsystems/gateway/GatewayIO.java rename to src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayIO.java index accab87..cdb73e7 100644 --- a/src/main/java/frc/robot/subsystems/gateway/GatewayIO.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.gateway; +package frc.robot.subsystems.pneumatics.gateway; import java.util.function.BooleanSupplier; import org.littletonrobotics.junction.AutoLog; @@ -14,10 +14,10 @@ public static class GatewayIOInputs { public default void updateInputs(GatewayIOInputs inputs) {} /** Open valve to begin filling tank */ - public default void beganFilling() {} + public default void openFillingValve() {} /** Close valve to stop filling tank */ - public default void stopFilling() {} + public default void closeFillingValve() {} /** Sets a supplier that tells the sim whether it is draining. */ public default void setSimDrain(BooleanSupplier pressureConsumer) {} diff --git a/src/main/java/frc/robot/subsystems/gateway/GatewayIOHardware.java b/src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayIOHardware.java similarity index 80% rename from src/main/java/frc/robot/subsystems/gateway/GatewayIOHardware.java rename to src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayIOHardware.java index 6ff6da0..dc9a2eb 100644 --- a/src/main/java/frc/robot/subsystems/gateway/GatewayIOHardware.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayIOHardware.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.gateway; +package frc.robot.subsystems.pneumatics.gateway; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.Solenoid; @@ -18,12 +18,12 @@ public void updateInputs(GatewayIOInputs inputs) { } @Override - public void beganFilling() { + public void openFillingValve() { solenoid.set(true); } @Override - public void stopFilling() { + public void closeFillingValve() { solenoid.set(false); } } diff --git a/src/main/java/frc/robot/subsystems/gateway/GatewayIOSim.java b/src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayIOSim.java similarity index 86% rename from src/main/java/frc/robot/subsystems/gateway/GatewayIOSim.java rename to src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayIOSim.java index 0364de2..c3e05be 100644 --- a/src/main/java/frc/robot/subsystems/gateway/GatewayIOSim.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayIOSim.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.gateway; +package frc.robot.subsystems.pneumatics.gateway; import frc.robot.Constants; import java.util.function.BooleanSupplier; @@ -10,7 +10,7 @@ public class GatewayIOSim implements GatewayIO { private static final double DRAIN_PSI_CHANGE_PER_SECOND = 20; private static final double PASSIVE_PSI_LEAK_PER_SECOND = 0.02; - private BooleanSupplier isDrainingSupplier; + private BooleanSupplier isDrainingSupplier = () -> false; private double tankPSI; private boolean isFilling; @@ -35,12 +35,12 @@ public void updateInputs(GatewayIOInputs inputs) { } @Override - public void beganFilling() { + public void openFillingValve() { isFilling = true; } @Override - public void stopFilling() { + public void closeFillingValve() { isFilling = false; } diff --git a/src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayTank.java b/src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayTank.java new file mode 100644 index 0000000..479631e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pneumatics/gateway/GatewayTank.java @@ -0,0 +1,77 @@ +package frc.robot.subsystems.pneumatics.gateway; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.BooleanSupplier; +import org.littletonrobotics.junction.Logger; + +/** + * Subsystem of t-shirt cannon robot representing the gateway tank and the value which fills it, + * along with the sensors on the gateway tank. + */ +public class GatewayTank extends SubsystemBase { + + private final GatewayIO io; + private final GatewayIOInputsAutoLogged inputs = new GatewayIOInputsAutoLogged(); + + private final InterpolatingShotTable shotTable; + + /** Create a new GatewayTank subsystem */ + public GatewayTank(GatewayIO io) { + this.io = io; + shotTable = new InterpolatingShotTable(); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("GatewayTank", inputs); + } + + // --- Getters --- + + /** + * Get whether compressor is currently active and filling Gateway tank + * + * @return compressor state + */ + public boolean isFilling() { + return inputs.isFilling; + } + + /** + * Get current pressure of gateway tank + * + * @return pressure in psi (pound per square inch) + */ + public double getPressure() { + return inputs.tankPSI; + } + + /** + * Get the estimated distance the t-shirt will travel based on the current pressure in the gateway + * + * @return distance in meters + */ + public double getEstimatedLaunchDistance() { + return shotTable.getEstimatedLaunchDistance(inputs.tankPSI); + } + + /** Opens the value to fill the gateway tank. */ + public void startFilling() { + io.openFillingValve(); + } + + /** Closes the valve. */ + public void stopFilling() { + io.closeFillingValve(); + } + + /** + * Sets a supplier that tells the sim whether it is draining. + * + * @param isDrainingSupplier A supplier that returns true if the tank is draining air. + */ + public void setSimDrain(BooleanSupplier isDrainingSupplier) { + this.io.setSimDrain(isDrainingSupplier); + } +} diff --git a/src/main/java/frc/robot/subsystems/gateway/InterpolatingShotTable.java b/src/main/java/frc/robot/subsystems/pneumatics/gateway/InterpolatingShotTable.java similarity index 96% rename from src/main/java/frc/robot/subsystems/gateway/InterpolatingShotTable.java rename to src/main/java/frc/robot/subsystems/pneumatics/gateway/InterpolatingShotTable.java index 31f1262..811aa48 100644 --- a/src/main/java/frc/robot/subsystems/gateway/InterpolatingShotTable.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/gateway/InterpolatingShotTable.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.gateway; +package frc.robot.subsystems.pneumatics.gateway; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; diff --git a/src/main/java/frc/robot/hardwareWrappers/PressureSwitch.java b/src/main/java/frc/robot/subsystems/pneumatics/hardwareWrappers/PressureSwitch.java similarity index 95% rename from src/main/java/frc/robot/hardwareWrappers/PressureSwitch.java rename to src/main/java/frc/robot/subsystems/pneumatics/hardwareWrappers/PressureSwitch.java index 5378aa9..5518dc7 100644 --- a/src/main/java/frc/robot/hardwareWrappers/PressureSwitch.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/hardwareWrappers/PressureSwitch.java @@ -1,4 +1,4 @@ -package frc.robot.hardwareWrappers; +package frc.robot.subsystems.pneumatics.hardwareWrappers; import edu.wpi.first.wpilibj.DigitalGlitchFilter; import edu.wpi.first.wpilibj.DigitalInput; diff --git a/src/main/java/frc/robot/hardwareWrappers/RelaySolenoid.java b/src/main/java/frc/robot/subsystems/pneumatics/hardwareWrappers/RelaySolenoid.java similarity index 90% rename from src/main/java/frc/robot/hardwareWrappers/RelaySolenoid.java rename to src/main/java/frc/robot/subsystems/pneumatics/hardwareWrappers/RelaySolenoid.java index 7243b1f..d83e13f 100644 --- a/src/main/java/frc/robot/hardwareWrappers/RelaySolenoid.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/hardwareWrappers/RelaySolenoid.java @@ -1,4 +1,4 @@ -package frc.robot.hardwareWrappers; +package frc.robot.subsystems.pneumatics.hardwareWrappers; import edu.wpi.first.wpilibj.Relay; diff --git a/src/main/java/frc/robot/hardwareWrappers/Transducer.java b/src/main/java/frc/robot/subsystems/pneumatics/hardwareWrappers/Transducer.java similarity index 95% rename from src/main/java/frc/robot/hardwareWrappers/Transducer.java rename to src/main/java/frc/robot/subsystems/pneumatics/hardwareWrappers/Transducer.java index f4b4479..2b5750a 100644 --- a/src/main/java/frc/robot/hardwareWrappers/Transducer.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/hardwareWrappers/Transducer.java @@ -1,4 +1,4 @@ -package frc.robot.hardwareWrappers; +package frc.robot.subsystems.pneumatics.hardwareWrappers; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.AnalogInput; diff --git a/src/main/java/frc/robot/subsystems/reservoir/ReservoirConstants.java b/src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirConstants.java similarity index 78% rename from src/main/java/frc/robot/subsystems/reservoir/ReservoirConstants.java rename to src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirConstants.java index a5af9cf..05c01da 100644 --- a/src/main/java/frc/robot/subsystems/reservoir/ReservoirConstants.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirConstants.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.reservoir; +package frc.robot.subsystems.pneumatics.reservoir; public class ReservoirConstants { private ReservoirConstants() {} diff --git a/src/main/java/frc/robot/subsystems/reservoir/ReservoirIO.java b/src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirIO.java similarity index 93% rename from src/main/java/frc/robot/subsystems/reservoir/ReservoirIO.java rename to src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirIO.java index d0da628..884e822 100644 --- a/src/main/java/frc/robot/subsystems/reservoir/ReservoirIO.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.reservoir; +package frc.robot.subsystems.pneumatics.reservoir; import java.util.function.BooleanSupplier; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/reservoir/ReservoirIOHardware.java b/src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirIOHardware.java similarity index 85% rename from src/main/java/frc/robot/subsystems/reservoir/ReservoirIOHardware.java rename to src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirIOHardware.java index efc616b..b0ba58f 100644 --- a/src/main/java/frc/robot/subsystems/reservoir/ReservoirIOHardware.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirIOHardware.java @@ -1,9 +1,9 @@ -package frc.robot.subsystems.reservoir; +package frc.robot.subsystems.pneumatics.reservoir; import edu.wpi.first.wpilibj.Relay; import edu.wpi.first.wpilibj.Relay.Direction; import edu.wpi.first.wpilibj.Relay.Value; -import frc.robot.hardwareWrappers.Transducer; +import frc.robot.subsystems.pneumatics.hardwareWrappers.Transducer; public class ReservoirIOHardware implements ReservoirIO { diff --git a/src/main/java/frc/robot/subsystems/reservoir/ReservoirIOSim.java b/src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirIOSim.java similarity index 96% rename from src/main/java/frc/robot/subsystems/reservoir/ReservoirIOSim.java rename to src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirIOSim.java index da236ea..99ceb9e 100644 --- a/src/main/java/frc/robot/subsystems/reservoir/ReservoirIOSim.java +++ b/src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirIOSim.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.reservoir; +package frc.robot.subsystems.pneumatics.reservoir; import frc.robot.Constants; import java.util.function.BooleanSupplier; diff --git a/src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirTank.java b/src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirTank.java new file mode 100644 index 0000000..3b785ce --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pneumatics/reservoir/ReservoirTank.java @@ -0,0 +1,63 @@ +package frc.robot.subsystems.pneumatics.reservoir; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.BooleanSupplier; +import org.littletonrobotics.junction.Logger; + +/** + * Subsystem of t-shirt cannon robot representing the reservoir tank and its sensors and the + * compressor which fills it. + */ +public class ReservoirTank extends SubsystemBase { + + private final ReservoirIO io; + private final ReservoirIOInputsAutoLogged inputs = new ReservoirIOInputsAutoLogged(); + + /** Create a new ReservoirTank subsystem */ + public ReservoirTank(ReservoirIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("ReservoirTank", inputs); + } + + /** Enables the compressor to fill the reservoir tank. */ + public void startFilling() { + io.startCompressor(); + } + + /** Disables the compressor. */ + public void stopFilling() { + io.stopCompressor(); + } + + /** + * Get whether compressor is currently active and filling reservoir tank + * + * @return compressor state + */ + public boolean isFilling() { + return inputs.compressorRunning; + } + + /** + * Get current pressure of reservoir tank + * + * @return pressure in psi (pound per square inch) + */ + public double getPressure() { + return inputs.tankPSI; + } + + /** + * Sets a supplier that tells the sim whether it is draining. + * + * @param isDrainingSupplier A supplier that returns true if the tank is draining air. + */ + public void setSimDrain(BooleanSupplier isDrainingSupplier) { + this.io.setSimDrain(isDrainingSupplier); + } +} diff --git a/src/main/java/frc/robot/subsystems/reservoir/ReservoirTank.java b/src/main/java/frc/robot/subsystems/reservoir/ReservoirTank.java deleted file mode 100644 index ab11464..0000000 --- a/src/main/java/frc/robot/subsystems/reservoir/ReservoirTank.java +++ /dev/null @@ -1,186 +0,0 @@ -package frc.robot.subsystems.reservoir; - -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.Constants.RobotType; -import frc.robot.utility.ThresholdController; -import java.util.ArrayList; -import java.util.Optional; -import java.util.function.BooleanSupplier; -import org.littletonrobotics.junction.Logger; - -/** - * Subsystem of t-shirt cannon robot representing the reservoir tank and its sensors and the - * compressor which fills it. - */ -public class ReservoirTank extends SubsystemBase { - - private final ReservoirIO io; - private final ReservoirIOInputsAutoLogged inputs = new ReservoirIOInputsAutoLogged(); - - private final ThresholdController controller; - - private final ArrayList pauseConditions; - - /** Create a new ReservoirTank subsystem */ - public ReservoirTank(ReservoirIO io) { - this.io = io; - - pauseConditions = new ArrayList<>(); - - controller = new ThresholdController(); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("ReservoirTank", inputs); - - if (Constants.getRobot() == RobotType.TEST_BOT) { - return; - } - - if (controller.calculate(inputs.tankPSI) > 0 - && !shouldPauseFilling() - && DriverStation.isEnabled()) { - io.startCompressor(); - } else { - io.stopCompressor(); - } - - Logger.recordOutput("ReservoirTank/status", getStatusString()); - } - - // --- Getters --- - - /** - * Get whether compressor is currently active and filling reservoir tank - * - * @return compressor state - */ - public boolean isFilling() { - return inputs.compressorRunning; - } - - /** - * Get current pressure of reservoir tank - * - * @return pressure in psi (pound per square inch) - */ - public double getPressure() { - return inputs.tankPSI; - } - - // --- Setters --- - - /** Sets the setpoint pressure to none. The compressor will not activate. */ - public void stopFilling() { - controller.setThresholds(0, 0); - } - - /** - * Enables the compressor to fill the reservoir tank to the specified pressure. - * - * @param minPressure The minimum pressure in PSI. The compressor will turn on when the pressure - * drops below this value and the pressure switch indicates that the system is not full. Range - * 0-120 PSI. - * @param maxPressure The maximum pressure in PSI. The compressor will turn off when the pressure - * reaches this value or the pressure switch is disconnected or indicates that the system is - * full. Range 0-120 PSI. Must be larger then minPressure. - */ - public void setPressureThresholds(double minPressure, double maxPressure) { - if (minPressure >= maxPressure) { - throw new IllegalArgumentException("maxPressure must be greater than minPressure"); - } - if (minPressure < ReservoirConstants.MIN_ALLOWED_PRESSURE - || minPressure > ReservoirConstants.MAX_ALLOWED_PRESSURE) { - throw new IllegalArgumentException( - String.format( - "minPressure must be between %s and %s PSI, got %s", - ReservoirConstants.MIN_ALLOWED_PRESSURE, - ReservoirConstants.MAX_ALLOWED_PRESSURE, - minPressure)); - } - if (maxPressure < ReservoirConstants.MIN_ALLOWED_PRESSURE - || maxPressure > ReservoirConstants.MAX_ALLOWED_PRESSURE) { - throw new IllegalArgumentException( - String.format( - "maxPressure must be between %s and %s PSI, got %s", - ReservoirConstants.MIN_ALLOWED_PRESSURE, - ReservoirConstants.MAX_ALLOWED_PRESSURE, - maxPressure)); - } - - controller.setThresholds(minPressure, maxPressure); - } - - // --- Sim Drain --- - - /** - * Sets a supplier that tells the sim whether it is draining. - * - * @param isDrainingSupplier A supplier that returns true if the tank is draining air. - */ - public void setSimDrain(BooleanSupplier isDrainingSupplier) { - this.io.setSimDrain(isDrainingSupplier); - } - - // --- Pause Conditions --- - - public void addPauseFillingCondition(BooleanSupplier condition, String reason) { - pauseConditions.add(new PauseCondition(condition, reason)); - } - - public void addPauseFillingCondition( - BooleanSupplier condition, String reason, double debounceTimeSeconds) { - final Debouncer debouncer = new Debouncer(debounceTimeSeconds, DebounceType.kBoth); - pauseConditions.add( - new PauseCondition(() -> debouncer.calculate(condition.getAsBoolean()), reason)); - } - - public boolean shouldPauseFilling() { - return pauseConditions.stream().anyMatch(PauseCondition::isActive); - } - - public Optional getPauseReason() { - return pauseConditions.stream() - .filter(PauseCondition::isActive) - .map(PauseCondition::reason) - .findFirst(); - } - - public String getStatusString() { - if (isFilling()) { - return String.format("Filling to %.2f PSI (End threshold)", controller.getUpperThreshold()); - } else if (!controller.isOn() && getPressure() > controller.getLowerThreshold()) { - return String.format( - "Stopped till %.2f PSI (Start threshold)", controller.getLowerThreshold()); - } - if (shouldPauseFilling()) { - return "Paused: " + getPauseReason().orElse("Unknown"); - } - return "Idle"; - } - - public static record PauseCondition(BooleanSupplier condition, String reason) { - public boolean isActive() { - return condition.getAsBoolean(); - } - } - - - // --- Testing --- - - public void forceOpen() { - System.out.println("Starting Compressor"); - io.startCompressor(); - } - - public void forceClose() { - System.out.println("Stopping Compressor"); - io.stopCompressor(); - } -} diff --git a/src/main/java/frc/robot/utility/ThresholdController.java b/src/main/java/frc/robot/utility/ThresholdController.java index dd87381..2ba6e2e 100644 --- a/src/main/java/frc/robot/utility/ThresholdController.java +++ b/src/main/java/frc/robot/utility/ThresholdController.java @@ -19,6 +19,11 @@ public ThresholdController() { */ public ThresholdController(double lowerThreshold, double upperThreshold) { setThresholds(lowerThreshold, upperThreshold); + reset(); + } + + /** Resets the controller to the OFF state. */ + public void reset() { isOn = false; }