Cargo detection

simgui-window.json

@@ -51,6 +51,11 @@
       "Pos": "1411,747",
       "Size": "170,274"
     },
+    "###DIO": {
+      "Collapsed": "0",
+      "Pos": "1677,877",
+      "Size": "124,54"
+    },
     "###FMS": {
       "Collapsed": "0",
       "Pos": "414,437",

simgui.json

@@ -3,7 +3,9 @@
     "Addressable LEDs": {
       "0": {
         "columns": 8
-      },
+      }
+    },
+    "DIO": {
       "window": {
         "visible": true
       }

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

@@ -20,6 +20,7 @@
 import frc.robot.misc.exceptions.UnknownTargetRobotException;
 import frc.robot.superstructure.SuperstructureSubsystem;
 import frc.robot.superstructure.arm.*;
+import frc.robot.superstructure.cargo_detector.*;
 import frc.robot.superstructure.commands.ArmDownAndShootCommand;
 import frc.robot.superstructure.commands.ArmDownAndSnarfCommand;
 import frc.robot.superstructure.commands.ArmUpAndShootCommand;
@@ -53,9 +54,10 @@ 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 Localization localization;
+  private final CargoDetector cargoDetector;
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -75,6 +77,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);
@@ -94,6 +97,7 @@ public RobotContainer() {
           arm = new Arm(new ArmIONeos(), lights);
           swiffer = new Swiffer(new SwifferIOFalcon500(), lights);
           imuSubsystem = new ImuSubsystem(new ImuIOAdis16470());
+          cargoDetector = new CargoDetector(new CargoDetectorIOReplay());
           upperVisionSubsystem = new UpperHubVisionSubsystem(new UpperHubVisionIOReplay());
           cargoVisionSubsystem = new CargoVisionSubsystem(new CargoVisionIOReplay(), imuSubsystem);
           driveSubsystem =
@@ -110,6 +114,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 ImuIOReplay());
           upperVisionSubsystem = new UpperHubVisionSubsystem(new UpperHubVisionIOReplay());
           cargoVisionSubsystem = new CargoVisionSubsystem(new CargoVisionIOReplay(), imuSubsystem);
@@ -127,6 +132,7 @@ public RobotContainer() {
           lights = new Lights(new LightsIOSim());
           arm = new Arm(new ArmIOSimNeos(), lights);
           swiffer = new Swiffer(new SwifferIOSimFalcon500(), lights);
+          cargoDetector = new CargoDetector(new CargoDetectorIOSim());
           imuSubsystem = new ImuSubsystem(new ImuIOSim());
           upperVisionSubsystem = new UpperHubVisionSubsystem(new UpperHubVisionIOSim());
           cargoVisionSubsystem = new CargoVisionSubsystem(new CargoVisionIOSim(), imuSubsystem);
@@ -144,7 +150,7 @@ public RobotContainer() {
       }
     }
 
-    superstructureSubsystem = new SuperstructureSubsystem(swiffer, arm, lights);
+    superstructureSubsystem = new SuperstructureSubsystem(swiffer, arm, lights, cargoDetector);
     localization = new Localization(driveSubsystem, cargoVisionSubsystem, imuSubsystem);
 
     autonomousChooser =
@@ -187,16 +193,16 @@ private void configureCopilotButtonBindings() {
     copilotController
         .rightTrigger
         .and(copilotController.bButton.negate())
-        .whileActiveContinuous(new ArmDownAndSnarfCommand(superstructureSubsystem));
+        .whenActive(new ArmDownAndSnarfCommand(superstructureSubsystem));
 
     // Scoring at the hub
-    copilotController.leftTrigger.whileHeld(new ArmUpAndShootCommand(superstructureSubsystem));
+    copilotController.leftTrigger.whenHeld(new ArmUpAndShootCommand(superstructureSubsystem));
 
     // Discard cargo by rolling it on the floor
     copilotController
         .rightTrigger
         .and(copilotController.bButton)
-        .whileActiveContinuous(new ArmDownAndShootCommand(superstructureSubsystem));
+        .whenActive(new ArmDownAndShootCommand(superstructureSubsystem));
   }
 
   /**

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.ArmUpAndStopCommand;
 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 ArmUpAndStopCommand(this)

src/main/java/frc/robot/superstructure/cargo_detector/CargoDetector.java

@@ -0,0 +1,43 @@
+// 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 CargoDetector extends SubsystemBase {
+  private CargoDetectorIO io;
+  private final Inputs inputs = new Inputs();
+
+  /** 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);
+
+    Logger.getInstance().recordOutput("CargoDetector/CargoCount", getCargoInventory().toString());
+  }
+
+  /** 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;
+  }
+}

src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIO.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 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 boolean hasLeftCargo = false;
+    public boolean hasRightCargo = false;
+
+    public void toLog(LogTable table) {
+      table.put("HasLeftCargo", hasLeftCargo);
+      table.put("HasRightCargo", hasRightCargo);
+    }
+
+    public void fromLog(LogTable table) {
+      hasLeftCargo = table.getBoolean("HasLeftCargo", hasLeftCargo);
+      hasRightCargo = table.getBoolean("HasRightCargo", hasRightCargo);
+    }
+  }
+}

src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOIR.java

@@ -0,0 +1,37 @@
+// 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.DigitalInput;
+import frc.robot.Constants;
+import frc.robot.misc.exceptions.UnsupportedSubsystemException;
+
+public class CargoDetectorIOIR implements CargoDetectorIO {
+  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;
+      case SIM_BOT:
+        leftSensor = new DigitalInput(9);
+        rightSensor = new DigitalInput(8);
+        break;
+      default:
+        throw new UnsupportedSubsystemException(this);
+    }
+  }
+
+  @Override
+  public void updateInputs(Inputs inputs) {
+    // The sensor outputs an off signal when it triggers, so we invert the return value
+    inputs.hasLeftCargo = !leftSensor.get();
+    inputs.hasRightCargo = !rightSensor.get();
+  }
+}

src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOReplay.java

@@ -0,0 +1,13 @@
+// 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
+  }
+}

src/main/java/frc/robot/superstructure/cargo_detector/CargoDetectorIOSim.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 CargoDetectorIOSim extends CargoDetectorIOIR {
+  // TODO: Implement
+}

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;
+}

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);
   }

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;
 
@@ -19,7 +20,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 SwifferCommand(superstructure.swiffer, SwifferMode.SNARFING));
+        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.isCarrying(CargoInventoryState.BOTH)));
 
     addRequirements(superstructure, superstructure.arm, superstructure.swiffer);
   }

src/main/java/frc/robot/superstructure/commands/ArmUpAndShootCommand.java

@@ -9,16 +9,17 @@
 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;
 
 /** 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) {
@@ -31,7 +32,10 @@ public ArmUpAndShootCommand(SuperstructureSubsystem superstructure) {
         new ArmCommand(superstructure.arm, ArmPosition.UP),
         // Shoot all cargo after the arm is up
         new SwifferCommand(superstructure.swiffer, SwifferMode.SHOOTING)
-            .withTimeout(SHOOT_DURATION));
+            // Shoot until the sensor reads as empty
+            .until(() -> superstructure.cargoDetector.isCarrying(CargoInventoryState.EMPTY))
+            // Add a timeout in case the sensor fails
+            .withTimeout(MAX_SHOOT_DURATION));
 
     addRequirements(superstructure, superstructure.arm, superstructure.swiffer);
   }

src/main/java/frc/robot/superstructure/swiffer/commands/SwifferCommand.java

@@ -37,7 +37,8 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    // This command never stops unless it's interrupted by the driver
+    // This command never stops unless it's interrupted by something else (the driver or the cargo
+    // detector)
     return false;
   }
 }