Set up templates for subsystems

shuffleboardLayout.json

@@ -77,9 +77,9 @@
               "Visuals/Orientation": "VERTICAL"
             }
           },
-          "5,0": {
+          "7,0": {
             "size": [
-              3,
+              2,
               1
             ],
             "content": {
@@ -138,6 +138,79 @@
               "_glyph": 148,
               "_showGlyph": false
             }
+          },
+          "9,1": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Command",
+              "_source0": "network_table:///SmartDashboard/Reset Pose",
+              "_title": "Reset Pose",
+              "_glyph": 148,
+              "_showGlyph": false
+            }
+          },
+          "9,2": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Command",
+              "_source0": "network_table:///SmartDashboard/Zero Gyro",
+              "_title": "Zero Gyro",
+              "_glyph": 148,
+              "_showGlyph": false
+            }
+          },
+          "9,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Command",
+              "_source0": "network_table:///SmartDashboard/Swerve Offsets",
+              "_title": "Swerve Offsets",
+              "_glyph": 148,
+              "_showGlyph": false
+            }
+          },
+          "5,2": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_source0": "network_table:///SmartDashboard/Reservoir Filling",
+              "_title": "Reservoir Filling",
+              "_glyph": 148,
+              "_showGlyph": false,
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          },
+          "5,0": {
+            "size": [
+              1,
+              2
+            ],
+            "content": {
+              "_type": "Number Bar",
+              "_source0": "network_table:///SmartDashboard/Reservoir Pressure",
+              "_title": "Reservoir Pressure",
+              "_glyph": 148,
+              "_showGlyph": false,
+              "Range/Min": 0.0,
+              "Range/Max": 100.0,
+              "Range/Center": 0.0,
+              "Visuals/Num tick marks": 5,
+              "Visuals/Show text": true,
+              "Visuals/Orientation": "VERTICAL"
+            }
           }
         }
       }
@@ -157,9 +230,9 @@
     }
   ],
   "windowGeometry": {
-    "x": 56.79999923706055,
-    "y": 98.4000015258789,
-    "width": 1386.4000244140625,
-    "height": 565.5999755859375
+    "x": -7.199999809265137,
+    "y": -7.199999809265137,
+    "width": 1550.4000244140625,
+    "height": 830.4000244140625
   }
 }

src/main/java/frc/robot/Constants.java

@@ -18,7 +18,7 @@ private Constants() {}
 
   public static final boolean TUNING_MODE = false;
 
-  private static RobotType robotType = RobotType.CANNON_BOT;
+  private static RobotType robotType = RobotType.SIM_BOT;
 
   public static RobotType getRobot() {
     if (RobotBase.isReal() && robotType == RobotType.SIM_BOT) {

src/main/java/frc/robot/RobotContainer.java

@@ -19,6 +19,7 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.Mode;
+import frc.robot.Constants.RobotType;
 import frc.robot.commands.SwerveModuleOffsetReader;
 import frc.robot.subsystems.dashboard.DriverDashboard;
 import frc.robot.subsystems.drive.Drive;
@@ -31,8 +32,10 @@
 import frc.robot.subsystems.drive.controllers.SpeedController.SpeedLevel;
 import frc.robot.subsystems.drive.controllers.TeleopDriveController;
 import frc.robot.subsystems.reservoir.ReservoirIO;
+import frc.robot.subsystems.reservoir.ReservoirIOSim;
 import frc.robot.subsystems.reservoir.ReservoirIOSolenoid;
 import frc.robot.subsystems.reservoir.ReservoirTank;
+import frc.robot.subsystems.vision.AprilTagVision;
 import frc.robot.utility.OverrideSwitch;
 import frc.robot.utility.logging.Alert;
 import frc.robot.utility.logging.Alert.AlertType;
@@ -50,13 +53,18 @@ public class RobotContainer {
   // Subsystems
   private final Drive drive;
   private final ReservoirTank reservoirTank;
+  private final AprilTagVision vision;
 
   // Controller
   private final CommandGenericHID driverController = new CommandXboxController(0);
   private final CommandGenericHID operatorController = new CommandXboxController(1);
 
-  // Dashboard inputs
-  private final LoggedDashboardChooser<Command> autoChooser;
+  // Driver Information
+  private final DriverDashboard dashboard = DriverDashboard.getInstance();
+
+  // Auto
+  private final LoggedDashboardChooser<Command> autoChooser =
+      new LoggedDashboardChooser<>("Auto Chooser", new SendableChooser<Command>());
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -71,6 +79,7 @@ public RobotContainer() {
                 new ModuleIO() {},
                 new ModuleIO() {});
         reservoirTank = new ReservoirTank(new ReservoirIOSolenoid());
+        vision = new AprilTagVision();
         break;
 
       case SIM_BOT:
@@ -82,7 +91,8 @@ public RobotContainer() {
                 new ModuleIOSim(DriveConstants.FRONT_RIGHT_MODULE_CONFIG),
                 new ModuleIOSim(DriveConstants.BACK_LEFT_MODULE_CONFIG),
                 new ModuleIOSim(DriveConstants.BACK_RIGHT_MODULE_CONFIG));
-        reservoirTank = new ReservoirTank(new ReservoirIO() {});
+        reservoirTank = new ReservoirTank(new ReservoirIOSim());
+        vision = new AprilTagVision();
         break;
 
       default:
@@ -95,34 +105,22 @@ public RobotContainer() {
                 new ModuleIO() {},
                 new ModuleIO() {});
         reservoirTank = new ReservoirTank(new ReservoirIO() {});
+        vision = new AprilTagVision();
         break;
     }
 
-    // Can also use AutoBuilder.buildAutoChooser(); instead of SendableChooser to
-    // auto populate
-    autoChooser = new LoggedDashboardChooser<>("Auto Chooser", new SendableChooser<Command>());
+    vision.setRobotPoseSupplier(drive::getPose);
+    vision.addVisionEstimateConsumer(
+        (visionEstimate) -> {
+          if (visionEstimate.isSuccess() && Constants.getRobot() != RobotType.SIM_BOT) {
+            drive.addVisionMeasurement(
+                visionEstimate.robotPose2d(),
+                visionEstimate.timestampSeconds(),
+                visionEstimate.standardDeviations());
+          }
+        });
 
-    // Set up SysId routines
-    // https://docs.wpilib.org/en/stable/docs/software/advanced-controls/system-identification/introduction.html
-    autoChooser.addOption(
-        "Drive SysId (Quasistatic Forward)",
-        drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
-    autoChooser.addOption(
-        "Drive SysId (Quasistatic Reverse)",
-        drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
-    autoChooser.addOption(
-        "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
-    autoChooser.addOption(
-        "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
-
-    // Set up named commands for path planner auto
-    // https://pathplanner.dev/pplib-named-commands.html
-    NamedCommands.registerCommand("StopWithX", drive.runOnce(drive::stopUsingBrakeArrangement));
-
-    // Path planner Autos
-    // https://pathplanner.dev/gui-editing-paths-and-autos.html#autos
-    autoChooser.addOption("Triangle Auto", new PathPlannerAuto("Triangle Auto"));
-    autoChooser.addOption("Rotate Auto", new PathPlannerAuto("Rotate Auto"));
+    reservoirTank.setDesiredPressureToFull();
 
     // Alerts for constants to avoid using them in competition
     if (Constants.TUNING_MODE) {
@@ -135,22 +133,30 @@ public RobotContainer() {
     }
 
     // Put dashboard defaults
+
     initDashboard();
 
+    // Configure autos
+    configureAutos();
+
     // Configure the button bindings
     configureControllerBindings();
+
+    // Print out robot and mode
+    System.out.printf("Robot %s, Mode: %s", Constants.getRobot(), Constants.getMode());
   }
 
   /** Configure drive dashboard object */
   private void initDashboard() {
-    DriverDashboard dashboard = DriverDashboard.getInstance();
     dashboard.addSubsystem(drive);
     dashboard.setPoseSupplier(drive::getPose);
-    dashboard.setRobotSupplier(drive::getRobotSpeeds);
+    dashboard.setRobotSpeedsSupplier(drive::getRobotSpeeds);
     dashboard.setSpeedLevelSupplier(() -> SpeedController.SpeedLevel.NO_LEVEL);
     dashboard.setFieldRelativeSupplier(() -> false);
     dashboard.setAngleDrivenSupplier(() -> false);
 
+    dashboard.setReservoirTank(reservoirTank::isFilling, reservoirTank::getPressure);
+
     dashboard.addCommand("Reset Pose", () -> drive.resetPose(new Pose2d()), true);
     dashboard.addCommand("Zero Gyro", drive::zeroGyro, true);
 
@@ -160,6 +166,8 @@ private void initDashboard() {
   /** Define button->command mappings. */
   private void configureControllerBindings() {
     CommandScheduler.getInstance().getActiveButtonLoop().clear();
+    configureDriverControllerBindings();
+    configureOperatorControllerBindings();
   }
 
   private void configureDriverControllerBindings() {
@@ -388,6 +396,30 @@ private void configureOperatorControllerBindings() {
     }
   }
 
+  public void configureAutos() {
+    // Set up SysId routines
+    // https://docs.wpilib.org/en/stable/docs/software/advanced-controls/system-identification/introduction.html
+    autoChooser.addOption(
+        "Drive SysId (Quasistatic Forward)",
+        drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    autoChooser.addOption(
+        "Drive SysId (Quasistatic Reverse)",
+        drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    autoChooser.addOption(
+        "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    autoChooser.addOption(
+        "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
+    // Set up named commands for path planner auto
+    // https://pathplanner.dev/pplib-named-commands.html
+    NamedCommands.registerCommand("StopWithX", drive.runOnce(drive::stopUsingBrakeArrangement));
+
+    // Path planner Autos
+    // https://pathplanner.dev/gui-editing-paths-and-autos.html#autos
+    autoChooser.addOption("Triangle Auto", new PathPlannerAuto("Triangle Auto"));
+    autoChooser.addOption("Rotate Auto", new PathPlannerAuto("Rotate Auto"));
+  }
+
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *

src/main/java/frc/robot/subsystems/cannon/CannonConstants.java

@@ -0,0 +1,5 @@
+package frc.robot.subsystems.cannon;
+
+public class CannonConstants {
+  public static final double FIRE_TUBE_OPEN_TIME_SECONDS = 0.4;
+}

src/main/java/frc/robot/subsystems/cannon/CannonIO.java

@@ -0,0 +1,16 @@
+package frc.robot.subsystems.cannon;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface CannonIO {
+  @AutoLog
+  public static class CannonIOInputs {
+    boolean isOpen;
+  }
+
+  public void updateInputs(CannonIOInputs inputs);
+
+  public void open();
+
+  public void close();
+}

src/main/java/frc/robot/subsystems/cannon/CannonIOHardware.java

@@ -0,0 +1,19 @@
+package frc.robot.subsystems.cannon;
+
+public class CannonIOHardware implements CannonIO {
+
+    @Override
+    public void updateInputs(CannonIOInputs inputs) {
+      inputs.isOpen = false;
+    }
+
+    @Override
+    public void open() {
+
+    }
+
+    @Override
+    public void close() {
+
+    }
+}

src/main/java/frc/robot/subsystems/cannon/CannonIOSim.java

@@ -0,0 +1,21 @@
+package frc.robot.subsystems.cannon;
+
+public class CannonIOSim implements CannonIO {
+
+  boolean isOpen = false;
+
+  @Override
+  public void updateInputs(CannonIOInputs inputs) {
+    inputs.isOpen = false;
+  }
+
+  @Override
+  public void open() {
+    isOpen = true;
+  }
+
+  @Override
+  public void close() {
+    isOpen = false;
+  }
+}