From 95437f2ce69b420f0b95538d9a9438cedfef994e Mon Sep 17 00:00:00 2001
From: Vy Tang <tangthienvy@outlook.com>
Date: Thu, 24 Mar 2022 17:38:48 -0700
Subject: [PATCH 01/11] Start using IR sensor for cargo detection

---
 src/main/java/frc/robot/RobotContainer.java   | 14 ++++++++--
 .../cargo_detector/CargoDetectorIO.java       | 23 ++++++++++++++++
 .../cargo_detector/CargoDetectorIOIR.java     | 16 ++++++++++++
 .../cargo_detector/CargoDetectorIOReplay.java | 14 ++++++++++
 .../cargo_detector/CargoDetectorIOSimIR.java  |  9 +++++++
 .../CargoDetectorSubsystem.java               | 26 +++++++++++++++++++
 6 files changed, 100 insertions(+), 2 deletions(-)
 create mode 100644 src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIO.java
 create mode 100644 src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java
 create mode 100644 src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOReplay.java
 create mode 100644 src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOSimIR.java
 create mode 100644 src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorSubsystem.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index aa8b795..7d7f6c6 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -19,6 +19,10 @@
 import frc.robot.misc.exceptions.UnknownTargetRobotException;
 import frc.robot.superstructure.SuperstructureSubsystem;
 import frc.robot.superstructure.arm.*;
+import frc.robot.superstructure.cargo_detector.CargoDetectorIOIR;
+import frc.robot.superstructure.cargo_detector.CargoDetectorIOReplay;
+import frc.robot.superstructure.cargo_detector.CargoDetectorIOSimIR;
+import frc.robot.superstructure.cargo_detector.CargoDetectorSubsystem;
 import frc.robot.superstructure.commands.ArmDownAndSnarfCommand;
 import frc.robot.superstructure.commands.ArmUpAndSwifferShootCommand;
 import frc.robot.superstructure.lights.*;
@@ -51,8 +55,9 @@ public class RobotContainer {
   private final CargoVisionSubsystem cargoVisionSubsystem;
   private final Swiffer swiffer;
   private final Arm arm;
-  private final SuperstructureSubsystem superstructureSubsystem;
   private final Lights lights;
+  private final SuperstructureSubsystem superstructureSubsystem;
+  private final CargoDetectorSubsystem cargoDetectorSubsystem;
   private final Localization localization;
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
@@ -84,6 +89,7 @@ public RobotContainer() {
               new WheelIOReplay(Corner.FRONT_RIGHT),
               new WheelIOReplay(Corner.REAR_LEFT),
               new WheelIOReplay(Corner.REAR_RIGHT));
+      cargoDetectorSubsystem = new CargoDetectorSubsystem(new CargoDetectorIOReplay());
     } else {
       switch (Constants.getRobot()) {
         case COMP_BOT:
@@ -102,7 +108,9 @@ public RobotContainer() {
                   new WheelIOFalcon500(Corner.FRONT_RIGHT),
                   new WheelIOFalcon500(Corner.REAR_LEFT),
                   new WheelIOFalcon500(Corner.REAR_RIGHT));
+          cargoDetectorSubsystem = new CargoDetectorSubsystem(new CargoDetectorIOReplay());
           break;
+
         case TEST_2020_BOT:
           matchMetadataSubsystem = new MatchMetadataSubsystem(new MatchMetadataIOFms());
           lights = new Lights(new LightsIORoborio());
@@ -118,8 +126,9 @@ public RobotContainer() {
                   imuSubsystem,
                   new WheelIOFalcon500(Corner.FRONT_LEFT),
                   new WheelIOFalcon500(Corner.FRONT_RIGHT),
-                  new WheelIOFalcon500(Corner.REAR_LEFT),
+                  new WheelIOReplay(Corner.REAR_LEFT),
                   new WheelIOFalcon500(Corner.REAR_RIGHT));
+          cargoDetectorSubsystem = new CargoDetectorSubsystem(new CargoDetectorIOIR());
           break;
         case SIM_BOT:
           matchMetadataSubsystem = new MatchMetadataSubsystem(new MatchMetadataIOSim());
@@ -137,6 +146,7 @@ public RobotContainer() {
                   new WheelIOSim(Corner.FRONT_RIGHT),
                   new WheelIOSim(Corner.REAR_LEFT),
                   new WheelIOSim(Corner.REAR_RIGHT));
+          cargoDetectorSubsystem = new CargoDetectorSubsystem(new CargoDetectorIOSimIR());
           break;
         default:
           throw new UnknownTargetRobotException();
diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIO.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIO.java
new file mode 100644
index 0000000..1b85329
--- /dev/null
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIO.java
@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.superstructure.cargo_detector;
+
+import frc.robot.misc.SubsystemIO;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+
+public interface CargoDetectorIO extends SubsystemIO<CargoDetectorIO.Inputs> {
+  public class Inputs implements LoggableInputs {
+    public double distance = 0;
+
+    public void toLog(LogTable table) {
+      table.put("Distance", distance);
+    }
+
+    public void fromLog(LogTable table) {
+      distance = table.getDouble("Distance", distance);
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java
new file mode 100644
index 0000000..09740d6
--- /dev/null
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java
@@ -0,0 +1,16 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.superstructure.cargo_detector;
+
+import edu.wpi.first.wpilibj.AnalogInput;
+
+public class CargoDetectorIOIR implements CargoDetectorIO {
+  private final AnalogInput sensor = new AnalogInput(0);
+
+  @Override
+  public void updateInputs(Inputs inputs) {
+    inputs.distance = sensor.getValue();
+  }
+}
diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOReplay.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOReplay.java
new file mode 100644
index 0000000..b465df2
--- /dev/null
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOReplay.java
@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.superstructure.cargo_detector;
+
+public class CargoDetectorIOReplay implements CargoDetectorIO {
+
+  @Override
+  public void updateInputs(Inputs inputs) {
+    // Intentionally left empty
+
+  }
+}
diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOSimIR.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOSimIR.java
new file mode 100644
index 0000000..b12d8fb
--- /dev/null
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOSimIR.java
@@ -0,0 +1,9 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.superstructure.cargo_detector;
+
+public class CargoDetectorIOSimIR extends CargoDetectorIOIR {
+  // TODO: Implement
+}
diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorSubsystem.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorSubsystem.java
new file mode 100644
index 0000000..20e1def
--- /dev/null
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorSubsystem.java
@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.superstructure.cargo_detector;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.superstructure.cargo_detector.CargoDetectorIO.Inputs;
+import org.littletonrobotics.junction.Logger;
+
+public class CargoDetectorSubsystem extends SubsystemBase {
+  private CargoDetectorIO io;
+  private final Inputs inputs = new Inputs();
+
+  /** Creates a new CargoDetectorSubsystem. */
+  public CargoDetectorSubsystem(CargoDetectorIO io) {
+    this.io = io;
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.getInstance().processInputs("CargoDetector", inputs);
+    // This method will be called once per scheduler run
+  }
+}

From badf870437bd1c7ba7e39d5de46ca2b01ff74ef4 Mon Sep 17 00:00:00 2001
From: Jonah Snider <jonah@jonahsnider.com>
Date: Tue, 29 Mar 2022 09:04:44 -0700
Subject: [PATCH 02/11] Restore rear left wheel on 2020 test bot

---
 src/main/java/frc/robot/RobotContainer.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 7d7f6c6..a0eb3bd 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -126,7 +126,7 @@ public RobotContainer() {
                   imuSubsystem,
                   new WheelIOFalcon500(Corner.FRONT_LEFT),
                   new WheelIOFalcon500(Corner.FRONT_RIGHT),
-                  new WheelIOReplay(Corner.REAR_LEFT),
+                  new WheelIOFalcon500(Corner.REAR_LEFT),
                   new WheelIOFalcon500(Corner.REAR_RIGHT));
           cargoDetectorSubsystem = new CargoDetectorSubsystem(new CargoDetectorIOIR());
           break;

From b01af74cc5ebc281c2a43186a5cb7bcfcd177229 Mon Sep 17 00:00:00 2001
From: Jonah Snider <jonah@jonahsnider.com>
Date: Tue, 29 Mar 2022 09:05:27 -0700
Subject: [PATCH 03/11] Use wildcard import of cargo detector package

---
 src/main/java/frc/robot/RobotContainer.java | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a0eb3bd..47419e7 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -19,10 +19,7 @@
 import frc.robot.misc.exceptions.UnknownTargetRobotException;
 import frc.robot.superstructure.SuperstructureSubsystem;
 import frc.robot.superstructure.arm.*;
-import frc.robot.superstructure.cargo_detector.CargoDetectorIOIR;
-import frc.robot.superstructure.cargo_detector.CargoDetectorIOReplay;
-import frc.robot.superstructure.cargo_detector.CargoDetectorIOSimIR;
-import frc.robot.superstructure.cargo_detector.CargoDetectorSubsystem;
+import frc.robot.superstructure.cargo_detector.*;
 import frc.robot.superstructure.commands.ArmDownAndSnarfCommand;
 import frc.robot.superstructure.commands.ArmUpAndSwifferShootCommand;
 import frc.robot.superstructure.lights.*;

From a6c11ccfd6e2a1777839cfa7c9bd1190e504121d Mon Sep 17 00:00:00 2001
From: Jonah Snider <jonah@jonahsnider.com>
Date: Tue, 29 Mar 2022 13:24:35 -0700
Subject: [PATCH 04/11] Fully implement cargo detection system

---
 src/main/java/frc/robot/RobotContainer.java   | 12 +++----
 .../SuperstructureSubsystem.java              |  6 +++-
 ...ectorSubsystem.java => CargoDetector.java} | 16 ++++++---
 .../cargo_detector/CargoDetectorIO.java       |  9 +++--
 .../cargo_detector/CargoDetectorIOIR.java     |  8 +++--
 .../commands/ArmDownAndSnarfCommand.java      |  8 +++--
 .../commands/ArmUpAndSwifferShootCommand.java |  8 +++--
 .../commands/ArmUpAndSwifferStopCommand.java  |  5 +--
 .../swiffer/commands/SwifferCommand.java      | 10 +++---
 .../swiffer/commands/SwifferShootCommand.java | 33 -------------------
 .../swiffer/commands/SwifferSnarfCommand.java | 22 -------------
 .../swiffer/commands/SwifferStopCommand.java  | 25 --------------
 12 files changed, 54 insertions(+), 108 deletions(-)
 rename src/main/java/frc/robot/superstructure/cargo_detector/{CargoDetectorSubsystem.java => CargoDetector.java} (64%)
 delete mode 100644 src/main/java/frc/robot/superstructure/swiffer/commands/SwifferShootCommand.java
 delete mode 100644 src/main/java/frc/robot/superstructure/swiffer/commands/SwifferSnarfCommand.java
 delete mode 100644 src/main/java/frc/robot/superstructure/swiffer/commands/SwifferStopCommand.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 47419e7..8730207 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -54,8 +54,8 @@ public class RobotContainer {
   private final Arm arm;
   private final Lights lights;
   private final SuperstructureSubsystem superstructureSubsystem;
-  private final CargoDetectorSubsystem cargoDetectorSubsystem;
   private final Localization localization;
+  private final CargoDetector cargoDetector;
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -75,6 +75,7 @@ public RobotContainer() {
       lights = new Lights(new LightsIOReplay());
       arm = new Arm(new ArmIOReplay(), lights);
       swiffer = new Swiffer(new SwifferIOReplay(), lights);
+      cargoDetector = new CargoDetector(new CargoDetectorIOReplay());
       imuSubsystem = new ImuSubsystem(new ImuIOReplay());
       upperVisionSubsystem = new UpperHubVisionSubsystem(new UpperHubVisionIOReplay());
       cargoVisionSubsystem = new CargoVisionSubsystem(new CargoVisionIOReplay(), imuSubsystem);
@@ -86,7 +87,6 @@ public RobotContainer() {
               new WheelIOReplay(Corner.FRONT_RIGHT),
               new WheelIOReplay(Corner.REAR_LEFT),
               new WheelIOReplay(Corner.REAR_RIGHT));
-      cargoDetectorSubsystem = new CargoDetectorSubsystem(new CargoDetectorIOReplay());
     } else {
       switch (Constants.getRobot()) {
         case COMP_BOT:
@@ -94,6 +94,7 @@ public RobotContainer() {
           lights = new Lights(new LightsIOReplay());
           arm = new Arm(new ArmIOReplay(), lights);
           swiffer = new Swiffer(new SwifferIOReplay(), lights);
+          cargoDetector = new CargoDetector(new CargoDetectorIOReplay());
           imuSubsystem = new ImuSubsystem(new ImuIONavx());
           upperVisionSubsystem = new UpperHubVisionSubsystem(new UpperHubVisionIOReplay());
           cargoVisionSubsystem = new CargoVisionSubsystem(new CargoVisionIOReplay(), imuSubsystem);
@@ -105,7 +106,6 @@ public RobotContainer() {
                   new WheelIOFalcon500(Corner.FRONT_RIGHT),
                   new WheelIOFalcon500(Corner.REAR_LEFT),
                   new WheelIOFalcon500(Corner.REAR_RIGHT));
-          cargoDetectorSubsystem = new CargoDetectorSubsystem(new CargoDetectorIOReplay());
           break;
 
         case TEST_2020_BOT:
@@ -113,6 +113,7 @@ public RobotContainer() {
           lights = new Lights(new LightsIORoborio());
           arm = new Arm(new ArmIOReplay(), lights);
           swiffer = new Swiffer(new SwifferIOReplay(), lights);
+          cargoDetector = new CargoDetector(new CargoDetectorIOIR());
           imuSubsystem = new ImuSubsystem(new ImuIOAdis16470());
           upperVisionSubsystem = new UpperHubVisionSubsystem(new UpperHubVisionIOReplay());
           cargoVisionSubsystem =
@@ -125,13 +126,13 @@ public RobotContainer() {
                   new WheelIOFalcon500(Corner.FRONT_RIGHT),
                   new WheelIOFalcon500(Corner.REAR_LEFT),
                   new WheelIOFalcon500(Corner.REAR_RIGHT));
-          cargoDetectorSubsystem = new CargoDetectorSubsystem(new CargoDetectorIOIR());
           break;
         case SIM_BOT:
           matchMetadataSubsystem = new MatchMetadataSubsystem(new MatchMetadataIOSim());
           lights = new Lights(new LightsIOSim());
           arm = new Arm(new ArmIOSimNeos(), lights);
           swiffer = new Swiffer(new SwifferIOSimFalcon500(), lights);
+          cargoDetector = new CargoDetector(new CargoDetectorIOSimIR());
           imuSubsystem = new ImuSubsystem(new ImuIOSim());
           upperVisionSubsystem = new UpperHubVisionSubsystem(new UpperHubVisionIOSim());
           cargoVisionSubsystem = new CargoVisionSubsystem(new CargoVisionIOSim(), imuSubsystem);
@@ -143,14 +144,13 @@ public RobotContainer() {
                   new WheelIOSim(Corner.FRONT_RIGHT),
                   new WheelIOSim(Corner.REAR_LEFT),
                   new WheelIOSim(Corner.REAR_RIGHT));
-          cargoDetectorSubsystem = new CargoDetectorSubsystem(new CargoDetectorIOSimIR());
           break;
         default:
           throw new UnknownTargetRobotException();
       }
     }
 
-    superstructureSubsystem = new SuperstructureSubsystem(swiffer, arm, lights);
+    superstructureSubsystem = new SuperstructureSubsystem(swiffer, arm, lights, cargoDetector);
     localization = new Localization(driveSubsystem, cargoVisionSubsystem, imuSubsystem);
 
     autonomousChooser =
diff --git a/src/main/java/frc/robot/superstructure/SuperstructureSubsystem.java b/src/main/java/frc/robot/superstructure/SuperstructureSubsystem.java
index d072755..d6494dc 100644
--- a/src/main/java/frc/robot/superstructure/SuperstructureSubsystem.java
+++ b/src/main/java/frc/robot/superstructure/SuperstructureSubsystem.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.superstructure.arm.Arm;
+import frc.robot.superstructure.cargo_detector.CargoDetector;
 import frc.robot.superstructure.commands.ArmUpAndSwifferStopCommand;
 import frc.robot.superstructure.lights.Lights;
 import frc.robot.superstructure.swiffer.Swiffer;
@@ -14,12 +15,15 @@ public class SuperstructureSubsystem extends SubsystemBase {
   public final Swiffer swiffer;
   public final Arm arm;
   public final Lights lights;
+  public final CargoDetector cargoDetector;
 
   /** Creates a new SuperstructureSubsystem. */
-  public SuperstructureSubsystem(Swiffer swiffer, Arm arm, Lights lights) {
+  public SuperstructureSubsystem(
+      Swiffer swiffer, Arm arm, Lights lights, CargoDetector cargoDetector) {
     this.swiffer = swiffer;
     this.arm = arm;
     this.lights = lights;
+    this.cargoDetector = cargoDetector;
 
     setDefaultCommand(
         new ArmUpAndSwifferStopCommand(this)
diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorSubsystem.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetector.java
similarity index 64%
rename from src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorSubsystem.java
rename to src/main/java/frc/robot/superstructure/cargo_detector/CargoDetector.java
index 20e1def..d0fa62d 100644
--- a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorSubsystem.java
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetector.java
@@ -8,19 +8,27 @@
 import frc.robot.superstructure.cargo_detector.CargoDetectorIO.Inputs;
 import org.littletonrobotics.junction.Logger;
 
-public class CargoDetectorSubsystem extends SubsystemBase {
+public class CargoDetector extends SubsystemBase {
   private CargoDetectorIO io;
   private final Inputs inputs = new Inputs();
 
-  /** Creates a new CargoDetectorSubsystem. */
-  public CargoDetectorSubsystem(CargoDetectorIO io) {
+  /** Creates a new CargoDetector. */
+  public CargoDetector(CargoDetectorIO io) {
     this.io = io;
   }
 
   @Override
   public void periodic() {
+    // This method will be called once per scheduler run
+
     io.updateInputs(inputs);
     Logger.getInstance().processInputs("CargoDetector", inputs);
-    // This method will be called once per scheduler run
+
+    Logger.getInstance().recordOutput("CargoDetector/CargoCount", getCargoCount());
+  }
+
+  /** Returns the number of cargo held by the robot. */
+  public int getCargoCount() {
+    return (inputs.hasLeftCargo ? 1 : 0) + (inputs.hasRightCargo ? 1 : 0);
   }
 }
diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIO.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIO.java
index 1b85329..4e85efe 100644
--- a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIO.java
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIO.java
@@ -10,14 +10,17 @@
 
 public interface CargoDetectorIO extends SubsystemIO<CargoDetectorIO.Inputs> {
   public class Inputs implements LoggableInputs {
-    public double distance = 0;
+    public boolean hasLeftCargo = false;
+    public boolean hasRightCargo = false;
 
     public void toLog(LogTable table) {
-      table.put("Distance", distance);
+      table.put("HasLeftCargo", hasLeftCargo);
+      table.put("HasRightCargo", hasRightCargo);
     }
 
     public void fromLog(LogTable table) {
-      distance = table.getDouble("Distance", distance);
+      hasLeftCargo = table.getBoolean("HasLeftCargo", hasLeftCargo);
+      hasRightCargo = table.getBoolean("HasRightCargo", hasRightCargo);
     }
   }
 }
diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java
index 09740d6..00e4ab4 100644
--- a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java
@@ -4,13 +4,15 @@
 
 package frc.robot.superstructure.cargo_detector;
 
-import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.DigitalInput;
 
 public class CargoDetectorIOIR implements CargoDetectorIO {
-  private final AnalogInput sensor = new AnalogInput(0);
+  private final DigitalInput leftSensor = new DigitalInput(0);
+  private final DigitalInput rightSensor = new DigitalInput(1);
 
   @Override
   public void updateInputs(Inputs inputs) {
-    inputs.distance = sensor.getValue();
+    inputs.hasLeftCargo = leftSensor.get();
+    inputs.hasRightCargo = rightSensor.get();
   }
 }
diff --git a/src/main/java/frc/robot/superstructure/commands/ArmDownAndSnarfCommand.java b/src/main/java/frc/robot/superstructure/commands/ArmDownAndSnarfCommand.java
index 90fb9d1..04a33bc 100644
--- a/src/main/java/frc/robot/superstructure/commands/ArmDownAndSnarfCommand.java
+++ b/src/main/java/frc/robot/superstructure/commands/ArmDownAndSnarfCommand.java
@@ -8,7 +8,8 @@
 import frc.robot.superstructure.SuperstructureSubsystem;
 import frc.robot.superstructure.arm.ArmPosition;
 import frc.robot.superstructure.arm.commands.ArmCommand;
-import frc.robot.superstructure.swiffer.commands.SwifferSnarfCommand;
+import frc.robot.superstructure.swiffer.SwifferMode;
+import frc.robot.superstructure.swiffer.commands.SwifferCommand;
 
 /** Lowers the swiffer while snarfing. */
 public class ArmDownAndSnarfCommand extends ParallelCommandGroup {
@@ -18,7 +19,10 @@ public ArmDownAndSnarfCommand(SuperstructureSubsystem superstructure) {
         // Start lowering the arm
         new ArmCommand(superstructure.arm, ArmPosition.DOWN),
         // While that's happening we begin spinning up the swiffer
-        new SwifferSnarfCommand(superstructure.swiffer));
+        new SwifferCommand(superstructure.swiffer, SwifferMode.SNARFING)
+            // Stop snarfing once 2 cargo are being carried
+            // The driver can manually cancel the command if they only want to grab 1 cargo
+            .until(() -> superstructure.cargoDetector.getCargoCount() == 2));
 
     addRequirements(superstructure, superstructure.arm, superstructure.swiffer);
   }
diff --git a/src/main/java/frc/robot/superstructure/commands/ArmUpAndSwifferShootCommand.java b/src/main/java/frc/robot/superstructure/commands/ArmUpAndSwifferShootCommand.java
index acbd3af..51a7e4f 100644
--- a/src/main/java/frc/robot/superstructure/commands/ArmUpAndSwifferShootCommand.java
+++ b/src/main/java/frc/robot/superstructure/commands/ArmUpAndSwifferShootCommand.java
@@ -10,7 +10,7 @@
 import frc.robot.superstructure.arm.ArmPosition;
 import frc.robot.superstructure.arm.commands.ArmCommand;
 import frc.robot.superstructure.swiffer.SwifferMode;
-import frc.robot.superstructure.swiffer.commands.SwifferShootCommand;
+import frc.robot.superstructure.swiffer.commands.SwifferCommand;
 
 /** Puts the arm up and shoots all cargo. */
 public class ArmUpAndSwifferShootCommand extends SequentialCommandGroup {
@@ -25,7 +25,11 @@ public ArmUpAndSwifferShootCommand(SuperstructureSubsystem superstructure) {
         // Arm up
         new ArmCommand(superstructure.arm, ArmPosition.UP),
         // Shoot all cargo after the arm is up
-        new SwifferShootCommand(superstructure.swiffer));
+        new SwifferCommand(superstructure.swiffer, SwifferMode.SHOOTING)
+            // Shoot until the sensor reads as empty
+            .until(() -> superstructure.cargoDetector.getCargoCount() == 0)
+            // Add a timeout in case the sensor fails
+            .withTimeout(1.5));
 
     addRequirements(superstructure, superstructure.arm, superstructure.swiffer);
   }
diff --git a/src/main/java/frc/robot/superstructure/commands/ArmUpAndSwifferStopCommand.java b/src/main/java/frc/robot/superstructure/commands/ArmUpAndSwifferStopCommand.java
index 79c6d61..64d6136 100644
--- a/src/main/java/frc/robot/superstructure/commands/ArmUpAndSwifferStopCommand.java
+++ b/src/main/java/frc/robot/superstructure/commands/ArmUpAndSwifferStopCommand.java
@@ -8,7 +8,8 @@
 import frc.robot.superstructure.SuperstructureSubsystem;
 import frc.robot.superstructure.arm.ArmPosition;
 import frc.robot.superstructure.arm.commands.ArmCommand;
-import frc.robot.superstructure.swiffer.commands.SwifferStopCommand;
+import frc.robot.superstructure.swiffer.SwifferMode;
+import frc.robot.superstructure.swiffer.commands.SwifferCommand;
 
 /** Lift the swiffer up and stop the flywheel. */
 public class ArmUpAndSwifferStopCommand extends ParallelCommandGroup {
@@ -18,7 +19,7 @@ public ArmUpAndSwifferStopCommand(SuperstructureSubsystem superstructure) {
         // Arm up
         new ArmCommand(superstructure.arm, ArmPosition.UP),
         // Stop the flywheel
-        new SwifferStopCommand(superstructure.swiffer));
+        new SwifferCommand(superstructure.swiffer, SwifferMode.STOPPED));
 
     addRequirements(superstructure, superstructure.arm, superstructure.swiffer);
   }
diff --git a/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferCommand.java b/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferCommand.java
index b8623dd..5f5ef14 100644
--- a/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferCommand.java
+++ b/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferCommand.java
@@ -8,12 +8,12 @@
 import frc.robot.superstructure.swiffer.Swiffer;
 import frc.robot.superstructure.swiffer.SwifferMode;
 
-abstract class SwifferCommand extends CommandBase {
+public class SwifferCommand extends CommandBase {
   private final Swiffer swiffer;
   private final SwifferMode mode;
 
   /** Creates a new SwifferCommand. */
-  protected SwifferCommand(Swiffer swiffer, SwifferMode mode) {
+  public SwifferCommand(Swiffer swiffer, SwifferMode mode) {
     this.swiffer = swiffer;
     this.mode = mode;
 
@@ -37,8 +37,8 @@ public void end(boolean interrupted) {
   }
 
   // Returns true when the command should end.
-  // This is abstract to ensure you don't forget to override the default `Command interface behavior
-  // of `return false`
   @Override
-  public abstract boolean isFinished();
+  public boolean isFinished() {
+    return swiffer.atGoal(mode);
+  }
 }
diff --git a/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferShootCommand.java b/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferShootCommand.java
deleted file mode 100644
index 6384f13..0000000
--- a/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferShootCommand.java
+++ /dev/null
@@ -1,33 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.superstructure.swiffer.commands;
-
-import edu.wpi.first.wpilibj.Timer;
-import frc.robot.superstructure.swiffer.Swiffer;
-import frc.robot.superstructure.swiffer.SwifferMode;
-
-public class SwifferShootCommand extends SwifferCommand {
-  private final Timer timer = new Timer();
-
-  /** Creates a new SwifferShootCommand. */
-  public SwifferShootCommand(Swiffer swiffer) {
-    super(swiffer, SwifferMode.SHOOTING);
-  }
-
-  @Override
-  public void initialize() {
-    super.initialize();
-
-    timer.reset();
-    timer.start();
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    // TODO: Check if 0 cargo are being held and use that to determine if finished
-    return timer.hasElapsed(1.5);
-  }
-}
diff --git a/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferSnarfCommand.java b/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferSnarfCommand.java
deleted file mode 100644
index edcf5c4..0000000
--- a/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferSnarfCommand.java
+++ /dev/null
@@ -1,22 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.superstructure.swiffer.commands;
-
-import frc.robot.superstructure.swiffer.Swiffer;
-import frc.robot.superstructure.swiffer.SwifferMode;
-
-public class SwifferSnarfCommand extends SwifferCommand {
-  /** Creates a new SwifferSnarfCommand. */
-  public SwifferSnarfCommand(Swiffer swiffer) {
-    super(swiffer, SwifferMode.SNARFING);
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    // TODO: Check if 2 cargo are being held and use that to determine if finished
-    return false;
-  }
-}
diff --git a/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferStopCommand.java b/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferStopCommand.java
deleted file mode 100644
index 4f150e1..0000000
--- a/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferStopCommand.java
+++ /dev/null
@@ -1,25 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.superstructure.swiffer.commands;
-
-import frc.robot.superstructure.swiffer.Swiffer;
-import frc.robot.superstructure.swiffer.SwifferMode;
-
-public class SwifferStopCommand extends SwifferCommand {
-  private final Swiffer swiffer;
-
-  /** Creates a new StopSwifferCommand. */
-  public SwifferStopCommand(Swiffer swiffer) {
-    super(swiffer, SwifferMode.STOPPED);
-
-    this.swiffer = swiffer;
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return swiffer.atGoal(SwifferMode.STOPPED);
-  }
-}

From fc95ed1d90af7f742b41a8e1231d36b2146ced5e Mon Sep 17 00:00:00 2001
From: Jonah Snider <jonah@jonahsnider.com>
Date: Fri, 1 Apr 2022 14:24:21 -0700
Subject: [PATCH 05/11] Format and rename sim class

---
 src/main/java/frc/robot/RobotContainer.java                    | 3 +--
 .../superstructure/cargo_detector/CargoDetectorIOReplay.java   | 1 -
 .../{CargoDetectorIOSimIR.java => CargoDetectorIOSim.java}     | 2 +-
 3 files changed, 2 insertions(+), 4 deletions(-)
 rename src/main/java/frc/robot/superstructure/cargo_detector/{CargoDetectorIOSimIR.java => CargoDetectorIOSim.java} (81%)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 8730207..289a477 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -107,7 +107,6 @@ public RobotContainer() {
                   new WheelIOFalcon500(Corner.REAR_LEFT),
                   new WheelIOFalcon500(Corner.REAR_RIGHT));
           break;
-
         case TEST_2020_BOT:
           matchMetadataSubsystem = new MatchMetadataSubsystem(new MatchMetadataIOFms());
           lights = new Lights(new LightsIORoborio());
@@ -132,7 +131,7 @@ public RobotContainer() {
           lights = new Lights(new LightsIOSim());
           arm = new Arm(new ArmIOSimNeos(), lights);
           swiffer = new Swiffer(new SwifferIOSimFalcon500(), lights);
-          cargoDetector = new CargoDetector(new CargoDetectorIOSimIR());
+          cargoDetector = new CargoDetector(new CargoDetectorIOSim());
           imuSubsystem = new ImuSubsystem(new ImuIOSim());
           upperVisionSubsystem = new UpperHubVisionSubsystem(new UpperHubVisionIOSim());
           cargoVisionSubsystem = new CargoVisionSubsystem(new CargoVisionIOSim(), imuSubsystem);
diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOReplay.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOReplay.java
index b465df2..b13b499 100644
--- a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOReplay.java
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOReplay.java
@@ -9,6 +9,5 @@ public class CargoDetectorIOReplay implements CargoDetectorIO {
   @Override
   public void updateInputs(Inputs inputs) {
     // Intentionally left empty
-
   }
 }
diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOSimIR.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOSim.java
similarity index 81%
rename from src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOSimIR.java
rename to src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOSim.java
index b12d8fb..606a0df 100644
--- a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOSimIR.java
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOSim.java
@@ -4,6 +4,6 @@
 
 package frc.robot.superstructure.cargo_detector;
 
-public class CargoDetectorIOSimIR extends CargoDetectorIOIR {
+public class CargoDetectorIOSim extends CargoDetectorIOIR {
   // TODO: Implement
 }

From e22881a20587b54a13d6e2603c8ff63ae7e06576 Mon Sep 17 00:00:00 2001
From: Jonah Snider <jonah@jonahsnider.com>
Date: Fri, 1 Apr 2022 14:35:33 -0700
Subject: [PATCH 06/11] Use an `enum` to represent cargo inventory state

---
 .../cargo_detector/CargoDetector.java           | 17 +++++++++++++----
 .../cargo_detector/CargoInventoryState.java     | 12 ++++++++++++
 .../commands/ArmDownAndSnarfCommand.java        |  3 ++-
 .../commands/ArmUpAndSwifferShootCommand.java   |  3 ++-
 4 files changed, 29 insertions(+), 6 deletions(-)
 create mode 100644 src/main/java/frc/robot/superstructure/cargo_detector/CargoInventoryState.java

diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetector.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetector.java
index d0fa62d..5ab90ae 100644
--- a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetector.java
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetector.java
@@ -24,11 +24,20 @@ public void periodic() {
     io.updateInputs(inputs);
     Logger.getInstance().processInputs("CargoDetector", inputs);
 
-    Logger.getInstance().recordOutput("CargoDetector/CargoCount", getCargoCount());
+    Logger.getInstance().recordOutput("CargoDetector/CargoCount", getCargoInventory().toString());
   }
 
-  /** Returns the number of cargo held by the robot. */
-  public int getCargoCount() {
-    return (inputs.hasLeftCargo ? 1 : 0) + (inputs.hasRightCargo ? 1 : 0);
+  /** Returns the state of the robot's cargo inventory. */
+  public CargoInventoryState getCargoInventory() {
+    if (inputs.hasLeftCargo) {
+      return inputs.hasRightCargo ? CargoInventoryState.BOTH : CargoInventoryState.ONE;
+    }
+
+    return CargoInventoryState.EMPTY;
+  }
+
+  /** @return Whether the robot's cargo inventory is in the provided state. */
+  public boolean isCarrying(CargoInventoryState state) {
+    return getCargoInventory() == state;
   }
 }
diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoInventoryState.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoInventoryState.java
new file mode 100644
index 0000000..4c0eade
--- /dev/null
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoInventoryState.java
@@ -0,0 +1,12 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.superstructure.cargo_detector;
+
+/** All the possible states the robot's cargo inventory could be in. */
+public enum CargoInventoryState {
+  EMPTY,
+  ONE,
+  BOTH;
+}
diff --git a/src/main/java/frc/robot/superstructure/commands/ArmDownAndSnarfCommand.java b/src/main/java/frc/robot/superstructure/commands/ArmDownAndSnarfCommand.java
index 04a33bc..4cdde3b 100644
--- a/src/main/java/frc/robot/superstructure/commands/ArmDownAndSnarfCommand.java
+++ b/src/main/java/frc/robot/superstructure/commands/ArmDownAndSnarfCommand.java
@@ -8,6 +8,7 @@
 import frc.robot.superstructure.SuperstructureSubsystem;
 import frc.robot.superstructure.arm.ArmPosition;
 import frc.robot.superstructure.arm.commands.ArmCommand;
+import frc.robot.superstructure.cargo_detector.CargoInventoryState;
 import frc.robot.superstructure.swiffer.SwifferMode;
 import frc.robot.superstructure.swiffer.commands.SwifferCommand;
 
@@ -22,7 +23,7 @@ public ArmDownAndSnarfCommand(SuperstructureSubsystem superstructure) {
         new SwifferCommand(superstructure.swiffer, SwifferMode.SNARFING)
             // Stop snarfing once 2 cargo are being carried
             // The driver can manually cancel the command if they only want to grab 1 cargo
-            .until(() -> superstructure.cargoDetector.getCargoCount() == 2));
+            .until(() -> superstructure.cargoDetector.isCarrying(CargoInventoryState.BOTH)));
 
     addRequirements(superstructure, superstructure.arm, superstructure.swiffer);
   }
diff --git a/src/main/java/frc/robot/superstructure/commands/ArmUpAndSwifferShootCommand.java b/src/main/java/frc/robot/superstructure/commands/ArmUpAndSwifferShootCommand.java
index 51a7e4f..afd01a4 100644
--- a/src/main/java/frc/robot/superstructure/commands/ArmUpAndSwifferShootCommand.java
+++ b/src/main/java/frc/robot/superstructure/commands/ArmUpAndSwifferShootCommand.java
@@ -9,6 +9,7 @@
 import frc.robot.superstructure.SuperstructureSubsystem;
 import frc.robot.superstructure.arm.ArmPosition;
 import frc.robot.superstructure.arm.commands.ArmCommand;
+import frc.robot.superstructure.cargo_detector.CargoInventoryState;
 import frc.robot.superstructure.swiffer.SwifferMode;
 import frc.robot.superstructure.swiffer.commands.SwifferCommand;
 
@@ -27,7 +28,7 @@ public ArmUpAndSwifferShootCommand(SuperstructureSubsystem superstructure) {
         // Shoot all cargo after the arm is up
         new SwifferCommand(superstructure.swiffer, SwifferMode.SHOOTING)
             // Shoot until the sensor reads as empty
-            .until(() -> superstructure.cargoDetector.getCargoCount() == 0)
+            .until(() -> superstructure.cargoDetector.isCarrying(CargoInventoryState.EMPTY))
             // Add a timeout in case the sensor fails
             .withTimeout(1.5));
 

From d2609cb901b863e191bfc23a2db5d22f57747105 Mon Sep 17 00:00:00 2001
From: Jonah Snider <jonah@jonahsnider.com>
Date: Fri, 1 Apr 2022 15:18:00 -0700
Subject: [PATCH 07/11] Fix swiffer command end behavior

---
 simgui-window.json                                  |  5 +++++
 simgui.json                                         |  1 +
 src/main/java/frc/robot/RobotContainer.java         |  4 ++--
 .../swiffer/commands/SwifferCommand.java            | 13 +------------
 4 files changed, 9 insertions(+), 14 deletions(-)

diff --git a/simgui-window.json b/simgui-window.json
index e10cbf2..5090cfd 100644
--- a/simgui-window.json
+++ b/simgui-window.json
@@ -36,6 +36,11 @@
       "Pos": "1411,747",
       "Size": "170,274"
     },
+    "###DIO": {
+      "Collapsed": "0",
+      "Pos": "1677,877",
+      "Size": "195,86"
+    },
     "###FMS": {
       "Collapsed": "0",
       "Pos": "414,437",
diff --git a/simgui.json b/simgui.json
index c2cecd1..62a793e 100644
--- a/simgui.json
+++ b/simgui.json
@@ -4,6 +4,7 @@
       "0": {
         "columns": 8
       },
+    "DIO": {
       "window": {
         "visible": true
       }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 289a477..7688ac3 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -174,10 +174,10 @@ private void configureCopilotButtonBindings() {
     copilotController.aButton.whenHeld(new UpperHubAlignCommand(driveSubsystem, localization));
 
     // Snarfing
-    copilotController.rightTrigger.whileHeld(new ArmDownAndSnarfCommand(superstructureSubsystem));
+    copilotController.rightTrigger.whenHeld(new ArmDownAndSnarfCommand(superstructureSubsystem));
 
     // Shooting
-    copilotController.leftTrigger.whileHeld(
+    copilotController.leftTrigger.whenHeld(
         new ArmUpAndSwifferShootCommand(superstructureSubsystem));
   }
 
diff --git a/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferCommand.java b/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferCommand.java
index 5f5ef14..db2ca9f 100644
--- a/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferCommand.java
+++ b/src/main/java/frc/robot/superstructure/swiffer/commands/SwifferCommand.java
@@ -26,19 +26,8 @@ public void initialize() {
     swiffer.setDesiredMode(mode);
   }
 
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {}
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    swiffer.setDesiredMode(SwifferMode.STOPPED);
-  }
-
-  // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return swiffer.atGoal(mode);
+    return false;
   }
 }

From 4cd12c44c037924dbc0cefa3715db891d9e54b8b Mon Sep 17 00:00:00 2001
From: Jonah Snider <jonah@jonahsnider.com>
Date: Sat, 2 Apr 2022 13:24:22 -0700
Subject: [PATCH 08/11] Fully implement IR detection system

---
 .../cargo_detector/CargoDetectorIOIR.java     | 23 +++++++++++++++----
 1 file changed, 19 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java
index 00e4ab4..916bfc0 100644
--- a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java
@@ -5,14 +5,29 @@
 package frc.robot.superstructure.cargo_detector;
 
 import edu.wpi.first.wpilibj.DigitalInput;
+import frc.robot.Constants;
+import frc.robot.misc.exceptions.UnsupportedSubsystemException;
 
 public class CargoDetectorIOIR implements CargoDetectorIO {
-  private final DigitalInput leftSensor = new DigitalInput(0);
-  private final DigitalInput rightSensor = new DigitalInput(1);
+  private final DigitalInput leftSensor;
+  private final DigitalInput rightSensor;
+
+  public CargoDetectorIOIR() {
+    switch (Constants.getRobot()) {
+      case TEST_2020_BOT:
+        leftSensor = new DigitalInput(9);
+        // TODO: Wire this sensor
+        rightSensor = new DigitalInput(25);
+        break;
+      default:
+        throw new UnsupportedSubsystemException(this);
+    }
+  }
 
   @Override
   public void updateInputs(Inputs inputs) {
-    inputs.hasLeftCargo = leftSensor.get();
-    inputs.hasRightCargo = rightSensor.get();
+    // The sensor outputs an off signal when it triggers, so we invert the return value
+    inputs.hasLeftCargo = !leftSensor.get();
+    inputs.hasRightCargo = !rightSensor.get();
   }
 }

From 77aea1edd4dbc786b8b19dbb16490f079c8435e3 Mon Sep 17 00:00:00 2001
From: Jonah Snider <jonah@jonahsnider.com>
Date: Mon, 4 Apr 2022 00:23:28 -0700
Subject: [PATCH 09/11] Fix sim GUI config

---
 simgui.json | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/simgui.json b/simgui.json
index 62a793e..e3e7a25 100644
--- a/simgui.json
+++ b/simgui.json
@@ -3,7 +3,8 @@
     "Addressable LEDs": {
       "0": {
         "columns": 8
-      },
+      }
+    },
     "DIO": {
       "window": {
         "visible": true

From 1bb1c262417589fefa3b615b51a92894bb09f510 Mon Sep 17 00:00:00 2001
From: Jonah Snider <jonah@jonahsnider.com>
Date: Mon, 4 Apr 2022 00:28:03 -0700
Subject: [PATCH 10/11] Support simulating cargo detector

---
 simgui-window.json                                            | 2 +-
 .../superstructure/cargo_detector/CargoDetectorIOIR.java      | 4 ++++
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/simgui-window.json b/simgui-window.json
index 5090cfd..3192e5b 100644
--- a/simgui-window.json
+++ b/simgui-window.json
@@ -39,7 +39,7 @@
     "###DIO": {
       "Collapsed": "0",
       "Pos": "1677,877",
-      "Size": "195,86"
+      "Size": "124,54"
     },
     "###FMS": {
       "Collapsed": "0",
diff --git a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java
index 916bfc0..22776a6 100644
--- a/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java
+++ b/src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java
@@ -19,6 +19,10 @@ public CargoDetectorIOIR() {
         // TODO: Wire this sensor
         rightSensor = new DigitalInput(25);
         break;
+      case SIM_BOT:
+        leftSensor = new DigitalInput(9);
+        rightSensor = new DigitalInput(8);
+        break;
       default:
         throw new UnsupportedSubsystemException(this);
     }

From 561b16c0e1e0edd7c60e76e44ea2da8f61299d80 Mon Sep 17 00:00:00 2001
From: Jonah Snider <jonah@jonahsnider.com>
Date: Thu, 7 Apr 2022 15:13:22 -0700
Subject: [PATCH 11/11] Improve cargo shoot commands

---
 .../commands/ArmDownAndShootCommand.java          | 15 +++++++++++----
 .../commands/ArmUpAndShootCommand.java            |  8 ++++----
 2 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/superstructure/commands/ArmDownAndShootCommand.java b/src/main/java/frc/robot/superstructure/commands/ArmDownAndShootCommand.java
index 220acd4..142e254 100644
--- a/src/main/java/frc/robot/superstructure/commands/ArmDownAndShootCommand.java
+++ b/src/main/java/frc/robot/superstructure/commands/ArmDownAndShootCommand.java
@@ -9,6 +9,7 @@
 import frc.robot.superstructure.SuperstructureSubsystem;
 import frc.robot.superstructure.arm.ArmPosition;
 import frc.robot.superstructure.arm.commands.ArmCommand;
+import frc.robot.superstructure.cargo_detector.CargoInventoryState;
 import frc.robot.superstructure.swiffer.SwifferMode;
 import frc.robot.superstructure.swiffer.commands.SwifferCommand;
 
@@ -17,6 +18,12 @@
  * unwanted cargo, not for scoring.
  */
 public class ArmDownAndShootCommand extends SequentialCommandGroup {
+  /**
+   * The maximum amount of time (in seconds) it will take to shoot any amount of cargo from the SPU
+   * onto the floor.
+   */
+  private static final double MAX_SHOOT_DURATION = 1.5;
+
   /** Creates a new ArmDownAndShootCommand. */
   public ArmDownAndShootCommand(SuperstructureSubsystem superstructure) {
     addCommands(
@@ -27,10 +34,10 @@ public ArmDownAndShootCommand(SuperstructureSubsystem superstructure) {
         // Arm down
         new ArmCommand(superstructure.arm, ArmPosition.DOWN),
         // Shoot all cargo after the arm is down
-        // Since the default superstructure mode is to lift the arm up and since this command is run
-        // continuously, adding an end condition here can cause the arm to jump up briefly as the
-        // command ends and restarts. We rely on the copilot to know when to cancel this command.
-        new SwifferCommand(superstructure.swiffer, SwifferMode.SHOOTING));
+        new SwifferCommand(superstructure.swiffer, SwifferMode.SHOOTING)
+            .until(() -> superstructure.cargoDetector.isCarrying(CargoInventoryState.EMPTY))
+            // Add a timeout in case the sensor fails
+            .withTimeout(MAX_SHOOT_DURATION));
 
     addRequirements(superstructure, superstructure.arm, superstructure.swiffer);
   }
diff --git a/src/main/java/frc/robot/superstructure/commands/ArmUpAndShootCommand.java b/src/main/java/frc/robot/superstructure/commands/ArmUpAndShootCommand.java
index 846bd25..70a96f9 100644
--- a/src/main/java/frc/robot/superstructure/commands/ArmUpAndShootCommand.java
+++ b/src/main/java/frc/robot/superstructure/commands/ArmUpAndShootCommand.java
@@ -16,10 +16,10 @@
 /** Puts the arm up and shoots all cargo. */
 public class ArmUpAndShootCommand extends SequentialCommandGroup {
   /**
-   * The maximum amount of time (in seconds) it will take to shoot any amount of cargo from the SPU.
-   * If we had cargo sensors we'd use those instead of just relying on a timer.
+   * The maximum amount of time (in seconds) it will take to shoot any amount of cargo from the SPU
+   * into the lower hub.
    */
-  private static final double SHOOT_DURATION = 0.75;
+  private static final double MAX_SHOOT_DURATION = 0.75;
 
   /** Creates a new ArmUpAndShootCommand. */
   public ArmUpAndShootCommand(SuperstructureSubsystem superstructure) {
@@ -35,7 +35,7 @@ public ArmUpAndShootCommand(SuperstructureSubsystem superstructure) {
             // Shoot until the sensor reads as empty
             .until(() -> superstructure.cargoDetector.isCarrying(CargoInventoryState.EMPTY))
             // Add a timeout in case the sensor fails
-            .withTimeout(SHOOT_DURATION));
+            .withTimeout(MAX_SHOOT_DURATION));
 
     addRequirements(superstructure, superstructure.arm, superstructure.swiffer);
   }