Shot Test

Co-Authored-By: Michael Lesirge <[email protected]>
redshiftrobotics · Dec 14, 2024 · dc860a6 · dc860a6
1 parent 1ac8b93
commit dc860a6
Show file tree

Hide file tree

Showing 8 changed files with 37 additions and 33 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.SIM_BOT;
+  private static RobotType robotType = RobotType.TEST_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
@@ -2,7 +2,6 @@
 
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -125,7 +124,7 @@ public RobotContainer() {
                 new ModuleIO() {},
                 new ModuleIO() {});
         vision = new AprilTagVision();
-        reservoirTank = new ReservoirTank(new ReservoirIO() {});
+        reservoirTank = new ReservoirTank(new ReservoirIOHardware());
         gatewayTank = new GatewayTank(new GatewayIOHardware());
         firingTube = new FiringTube(new CannonIOHardware(15), "Main");
         break;
@@ -159,7 +158,7 @@ public RobotContainer() {
         });
 
     // Set up reservoir tank
-    reservoirTank.setPressureThresholds(10, 15);
+    reservoirTank.setPressureThresholds(15, 20);
     new Trigger(
             () ->
                 NormUtil.norm(drive.getRobotSpeeds()) > drive.getMaxLinearSpeedMetersPerSec() * 0.75
@@ -174,7 +173,7 @@ public RobotContainer() {
     reservoirTank.setSimDrain(gatewayTank::isFilling);
 
     // Set up gateway tank
-    gatewayTank.setDesiredPSI(10);
+    gatewayTank.setDesiredPSI(20);
     new Trigger(firingTube::isOpen)
         .or(firingTube::isWaitingToFire)
         .debounce(0.5, DebounceType.kBoth)
@@ -325,7 +324,8 @@ private void configureDriverControllerBindings() {
       boolean includeDiagonalPOV = true;
       for (int pov = 0; pov < 360; pov += includeDiagonalPOV ? 45 : 90) {
 
-        // POV angles are in Clock Wise degrees, needs to be flipped to get correct rotation2d
+        // POV angles are in Clock Wise degrees, needs to be flipped to get correct
+        // rotation2d
         final Rotation2d angle = Rotation2d.fromDegrees(-pov);
         final String name = String.format("%d\u00B0", pov);
 
@@ -401,7 +401,8 @@ private void configureDriverControllerBindings() {
                     .withName(String.format("DriveLockedHeading %s", name)));
       }
 
-      // While X is held down go into stop and go into the cross position to resistent movement,
+      // While X is held down go into stop and go into the cross position to resistent
+      // movement,
       // then once X button is let go put modules forward
       driverXbox
           .x()
@@ -410,7 +411,8 @@ private void configureDriverControllerBindings() {
                   .startEnd(drive::stopUsingBrakeArrangement, drive::stopUsingForwardArrangement)
                   .withName("StopWithX"));
 
-      // When be is pressed stop the drivetrain then idle it, cancelling all incoming commands.
+      // When be is pressed stop the drivetrain then idle it, cancelling all incoming
+      // commands.
       // Also do this when robot is disabled
       driverXbox
           .b()
@@ -422,7 +424,8 @@ private void configureDriverControllerBindings() {
                   .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)
                   .withName("StopCancel"));
 
-      // When right (Gas) trigger is held down or left stick (sprint) is pressed, put in boost
+      // When right (Gas) trigger is held down or left stick (sprint) is pressed, put
+      // in boost
       // (fast) mode
       driverXbox
           .rightTrigger(0.5)
@@ -432,7 +435,8 @@ private void configureDriverControllerBindings() {
                   () -> speedController.pushSpeedLevel(SpeedLevel.BOOST),
                   () -> speedController.removeSpeedLevel(SpeedLevel.BOOST)));
 
-      // When left (Brake) trigger is held down or right stick (crouch) is pressed put in precise
+      // When left (Brake) trigger is held down or right stick (crouch) is pressed put
+      // in precise
       // (slow) mode
       driverXbox
           .leftTrigger(0.5)
@@ -469,8 +473,10 @@ 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.
+      // 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()
@@ -488,15 +494,15 @@ private void configureOperatorControllerBindings() {
 
       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("Adjust Target Distance"));
+      // gatewayTank.setDefaultCommand(
+      // gatewayTank
+      // .run(
+      // () -> {
+      // gatewayTank.setTargetLaunchDistance(
+      // gatewayTank.getTargetLaunchDistance()
+      // - MathUtil.applyDeadband(operatorXbox.getLeftY(), 0.1) * 0.4);
+      // })
+      // .withName("Adjust Target Distance"));
     }
   }
 

diff --git a/src/main/java/frc/robot/subsystems/cannon/CannonIOHardware.java b/src/main/java/frc/robot/subsystems/cannon/CannonIOHardware.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class CannonIOHardware implements CannonIO {
 
@@ -18,11 +19,13 @@ public void updateInputs(CannonIOInputs inputs) {
 
   @Override
   public void open() {
+    SmartDashboard.putBoolean("Cannon", false);
     solenoid.set(true);
   }
 
   @Override
   public void close() {
+    SmartDashboard.putBoolean("Cannon", false);
     solenoid.set(false);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/cannon/FiringTube.java b/src/main/java/frc/robot/subsystems/cannon/FiringTube.java
@@ -23,6 +23,7 @@ public FiringTube(CannonIO io, String name) {
     this.name = name;
     fireTimer = new Timer();
     fireRequirement = () -> true;
+    io.close();
   }
 
   public void periodic() {

diff --git a/src/main/java/frc/robot/subsystems/gateway/GatewayIOHardware.java b/src/main/java/frc/robot/subsystems/gateway/GatewayIOHardware.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.hardwareWrappers.Transducer;
 
 public class GatewayIOHardware implements GatewayIO {
@@ -22,11 +23,13 @@ public void updateInputs(GatewayIOInputs inputs) {
 
   @Override
   public void beganFilling() {
+    SmartDashboard.putBoolean("Gateway", true);
     solenoid.set(true);
   }
 
   @Override
   public void stopFilling() {
+    SmartDashboard.putBoolean("Gateway", false);
     solenoid.set(false);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/gateway/GatewayTank.java b/src/main/java/frc/robot/subsystems/gateway/GatewayTank.java
@@ -3,8 +3,6 @@
 import edu.wpi.first.math.MathUtil;
 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.function.BooleanSupplier;
 import org.littletonrobotics.junction.Logger;
@@ -42,10 +40,6 @@ public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("GatewayTank", inputs);
 
-    if (Constants.getRobot() == RobotType.TEST_BOT) {
-      return;
-    }
-
     if (controller.calculate(inputs.tankPSI) > 0 && !paused && DriverStation.isEnabled()) {
       io.beganFilling();
     } else {

diff --git a/src/main/java/frc/robot/subsystems/reservoir/ReservoirIOHardware.java b/src/main/java/frc/robot/subsystems/reservoir/ReservoirIOHardware.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj.Relay;
 import edu.wpi.first.wpilibj.Relay.Direction;
 import edu.wpi.first.wpilibj.Relay.Value;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.hardwareWrappers.Transducer;
 
 public class ReservoirIOHardware implements ReservoirIO {
@@ -23,11 +24,13 @@ public void updateInputs(ReservoirIOInputs inputs) {
 
   @Override
   public void startCompressor() {
+    SmartDashboard.putBoolean("Reservoir", true);
     output.set(Value.kOn);
   }
 
   @Override
   public void stopCompressor() {
+    SmartDashboard.putBoolean("Reservoir", false);
     output.set(Value.kOff);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/reservoir/ReservoirTank.java b/src/main/java/frc/robot/subsystems/reservoir/ReservoirTank.java
@@ -2,8 +2,6 @@
 
 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.function.BooleanSupplier;
 import org.littletonrobotics.junction.Logger;
@@ -35,10 +33,6 @@ public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("ReservoirTank", inputs);
 
-    if (Constants.getRobot() == RobotType.TEST_BOT) {
-      return;
-    }
-
     if (controller.calculate(inputs.tankPSI) > 0 && !paused && DriverStation.isEnabled()) {
       io.startCompressor();
     } else {