scoraling

src/main/java/org/sciborgs1155/lib/Beambreak.java

@@ -0,0 +1,44 @@
+package org.sciborgs1155.lib;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import java.util.function.BooleanSupplier;
+
+/**
+ * a beambreak wrapper that contains two main elements: 1. A BooleanSupplier detailing the
+ * beambreak's state; true for unbroken, false for broken 2. A close runnable that will close the
+ * resource if necessary
+ */
+public class Beambreak {
+  private final BooleanSupplier beambreak;
+  private final Runnable close;
+
+  public Beambreak(BooleanSupplier beambreak, Runnable close) {
+    this.beambreak = beambreak;
+    this.close = close;
+  }
+
+  /**
+   * generates a beambreak wrapper based off a channel
+   *
+   * @param channel the channel for the beambreak
+   */
+  public static Beambreak real(int channel) {
+    DigitalInput beambreak = new DigitalInput(channel);
+    return new Beambreak(() -> beambreak.get(), beambreak::close);
+  }
+
+  /** Generates a beambreak that does not have hardware This beambreak will always return true. */
+  public static Beambreak none() {
+    return new Beambreak(() -> true, () -> {});
+  }
+
+  /** the value of the beambreak; true for unbroken, false for broken */
+  public boolean get() {
+    return beambreak.getAsBoolean();
+  }
+
+  /** closes all resources if necessary */
+  public void close() {
+    close.run();
+  }
+}

src/main/java/org/sciborgs1155/robot/Constants.java

@@ -61,7 +61,7 @@ public static boolean inField(Pose3d pose) {
           && pose.getY() < Field.WIDTH.in(Meters));
     }
 
-    public enum Level {
+    public static enum Level {
       L1(Meters.of(.3)),
       L2(Meters.of(.7)),
       L3(Meters.of(1)),
@@ -73,5 +73,8 @@ public enum Level {
         this.height = height;
       }
     }
+
+    /** Offset to add to the level to be at algae's height */
+    public static final Distance algaeOffset = Meters.of(0);
   }
 }

src/main/java/org/sciborgs1155/robot/commands/Scoraling.java

@@ -1,7 +1,6 @@
 package org.sciborgs1155.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import org.sciborgs1155.robot.Constants.Field.Level;
 import org.sciborgs1155.robot.elevator.Elevator;
 import org.sciborgs1155.robot.hopper.Hopper;
@@ -16,31 +15,55 @@ public Scoraling(Hopper hopper, Scoral scoral, Elevator elevator) {
     this.hopper = hopper;
     this.scoral = scoral;
     this.elevator = elevator;
+
+    // will stop the intaking if it reaches the desired state (between the hps and scoral
+    // beambreaks)
+    hopper
+        .beambreakTrigger
+        .negate()
+        .or(scoral.beambreakTrigger)
+        .onFalse(stop().onlyIf(() -> hopper.getCurrentCommand().getName().equals("intakingHPS")));
   }
 
+  /** A command which intakes from the human player station */
   public Command hpsIntake() {
-    // elevator goes to default position with retract
-    // andThen -> hopper and scoral outtakes (since outtakaing will intake this way) until the
-    // scoral beambreak is false OR the hps beambreak is true
-    // onlyIf -> scoral beambreak is true (don't have a coral)
-
-    return Commands.idle(hopper, scoral, elevator);
+    return elevator
+        .retract()
+        .andThen(runRollers())
+        .onlyIf(scoral.beambreakTrigger)
+        .withName("intakingHPS");
   }
 
+  /**
+   * A command which scores a coral at the given level, assuming you are already at the correct
+   * branch
+   *
+   * @param level the level the scoral scores in
+   */
   public Command scoral(Level level) {
-    // elevator goes to the level selected
-    // andThen -> scoral outtakes until its beambreak is true
-
-    return Commands.idle(scoral, elevator);
+    return elevator.scoreLevel(level).andThen(scoral.outtake()).withName("scoraling");
   }
 
-  public Command grabAlgae(double algaeLevel) {
-    // will make a constant or something for the algae heights needed
+  /**
+   * A command which grabs the algae from above the level given; only L2 and L3 are allowed
+   *
+   * @param level the level to score above
+   */
+  public Command cleanAlgae(Level level) {
+    return elevator
+        .clean(level)
+        .andThen(scoral.intake())
+        .onlyIf(scoral.beambreakTrigger)
+        .withName("cleanAlgae");
+  }
 
-    // elevator goes to the algae height
-    // andThen -> intake with scoral
-    // onlyIf -> scoral beambreak is true (don't have a coral)
+  /** A command which halts both the hopper and the scoral */
+  public Command stop() {
+    return hopper.stop().alongWith(scoral.stop()).withName("stopping");
+  }
 
-    return Commands.idle(scoral, elevator);
+  /** A command which runs the hps + scoral rollers forward (intaking) */
+  public Command runRollers() {
+    return hopper.intake().alongWith(scoral.outtake()).withName("runningRollers");
   }
 }

src/main/java/org/sciborgs1155/robot/elevator/Elevator.java

@@ -5,6 +5,7 @@
 import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
 import static edu.wpi.first.units.Units.Volts;
 import static org.sciborgs1155.lib.Assertion.eAssert;
+import static org.sciborgs1155.robot.Constants.Field.algaeOffset;
 import static org.sciborgs1155.robot.elevator.ElevatorConstants.*;
 
 import com.ctre.phoenix6.SignalLogger;
@@ -23,6 +24,8 @@
 import monologue.Annotations.Log;
 import monologue.Logged;
 import org.sciborgs1155.lib.Assertion;
+import org.sciborgs1155.lib.FaultLogger;
+import org.sciborgs1155.lib.FaultLogger.FaultType;
 import org.sciborgs1155.lib.Test;
 import org.sciborgs1155.robot.Constants.Field.Level;
 import org.sciborgs1155.robot.Robot;
@@ -102,6 +105,19 @@ public Command scoreLevel(Level level) {
     return goTo(level.height.in(Meters));
   }
 
+  /** Goes to an offset height above the level given to clean algae ONLY L2 and L3! */
+  public Command clean(Level level) {
+    if (level == Level.L1 || level == Level.L4) {
+      retract();
+      FaultLogger.report(
+          "Algae level fault",
+          "an invalid level has been passed to the clean command, L1 or L4",
+          FaultType.WARNING);
+    }
+
+    return goTo(level.height.plus(algaeOffset).in(Meters));
+  }
+
   /**
    * Drives elevator to the desired height, within its physical boundaries.
    *

src/main/java/org/sciborgs1155/robot/hopper/Hopper.java

@@ -6,22 +6,29 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
-import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import org.sciborgs1155.lib.Beambreak;
 import org.sciborgs1155.lib.SimpleMotor;
 import org.sciborgs1155.robot.Robot;
 
 public class Hopper extends SubsystemBase implements AutoCloseable {
   private final SimpleMotor motor;
-  private final DigitalInput beambreak = new DigitalInput(BEAMBREAK);
+  private final Beambreak beambreak;
   public final Trigger beambreakTrigger;
 
+  /** creates a Hopper based on whether it is simulated or real hardware */
   public static Hopper create() {
-    return new Hopper(Robot.isReal() ? realMotor() : Hopper.none());
+    return Robot.isReal() ? new Hopper(realMotor(), Beambreak.real(BEAMBREAK)) : none();
   }
 
+  /** creates a hopper sans hardware or simulation */
+  public static Hopper none() {
+    return new Hopper(SimpleMotor.none(), Beambreak.none());
+  }
+
+  /** generates a simple motor with the appropriate configurations as real hardware */
   private static SimpleMotor realMotor() {
     TalonFXConfiguration config = new TalonFXConfiguration();
 
@@ -31,12 +38,9 @@ private static SimpleMotor realMotor() {
     return SimpleMotor.talon(new TalonFX(MOTOR), config);
   }
 
-  public static SimpleMotor none() {
-    return SimpleMotor.none();
-  }
-
-  public Hopper(SimpleMotor motor) {
+  public Hopper(SimpleMotor motor, Beambreak beambreak) {
     this.motor = motor;
+    this.beambreak = beambreak;
     this.beambreakTrigger = new Trigger(beambreak::get);
   }
 

src/main/java/org/sciborgs1155/robot/scoral/Scoral.java

@@ -7,24 +7,30 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
-import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import java.util.Optional;
 import monologue.Annotations.Log;
 import monologue.Logged;
+import org.sciborgs1155.lib.Beambreak;
 import org.sciborgs1155.lib.SimpleMotor;
 import org.sciborgs1155.robot.Robot;
 
 public class Scoral extends SubsystemBase implements Logged, AutoCloseable {
   private final SimpleMotor hardware;
 
-  private final DigitalInput beambreak = new DigitalInput(BEAMBREAK);
-  public final Trigger beambreakTrigger = new Trigger(() -> beambreak());
+  private final Beambreak beambreak;
+  public final Trigger beambreakTrigger;
 
+  /** creates a Scoral based on if it is simulated or real hardware */
   public static Scoral create() {
-    return new Scoral(Robot.isReal() ? realMotor() : Scoral.none());
+    return Robot.isReal() ? new Scoral(realMotor(), Beambreak.real(BEAMBREAK)) : none();
+  }
+
+  /** creates a Scoral without hardware or simulation */
+  public static Scoral none() {
+    return new Scoral(SimpleMotor.none(), Beambreak.none());
   }
 
   private static SimpleMotor realMotor() {
@@ -37,25 +43,25 @@ private static SimpleMotor realMotor() {
     return SimpleMotor.talon(new TalonFX(ROLLER), config);
   }
 
-  public static SimpleMotor none() {
-    return SimpleMotor.none();
-  }
-
-  public Scoral(SimpleMotor hardware) {
+  public Scoral(SimpleMotor hardware, Beambreak beambreak) {
     this.hardware = hardware;
+    this.beambreak = beambreak;
+    beambreakTrigger = new Trigger(beambreak::get);
   }
 
-  /**
-   * Runs the motor to move a coral out of the scoral outwards. Ends when the beam is no longer
-   * broken.
-   */
+  /** Runs the motor to move a coral out of the scoral outwards. */
   public Command outtake() {
-    return run(() -> hardware.set(POWER)).until(() -> beambreak()).withName("outtake");
+    return run(() -> hardware.set(POWER)).withName("outtake");
   }
 
-  /** Runs the motor to move a coral into the scoral. Ends when the beam is broken. */
+  /** Runs the motor to move a coral into the scoral. */
   public Command intake() {
-    return run(() -> hardware.set(POWER)).until(() -> !beambreak()).withName("intake");
+    return run(() -> hardware.set(POWER)).withName("intake");
+  }
+
+  /** Stops the motor */
+  public Command stop() {
+    return run(() -> hardware.set(0)).withName("stop");
   }
 
   /** Returns the value of the beambreak, which is false when the beam is broken. */

src/test/java/org/sciborgs1155/robot/ScoralingTest.java

@@ -0,0 +1,89 @@
+package org.sciborgs1155.robot;
+
+import static edu.wpi.first.units.Units.Meters;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.sciborgs1155.lib.UnitTestingUtil.*;
+import static org.sciborgs1155.robot.Constants.Field.algaeOffset;
+import static org.sciborgs1155.robot.elevator.ElevatorConstants.MIN_HEIGHT;
+
+import java.util.stream.Stream;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+import org.sciborgs1155.robot.Constants.Field.Level;
+import org.sciborgs1155.robot.commands.Scoraling;
+import org.sciborgs1155.robot.elevator.Elevator;
+import org.sciborgs1155.robot.elevator.ElevatorConstants;
+import org.sciborgs1155.robot.hopper.Hopper;
+import org.sciborgs1155.robot.scoral.Scoral;
+
+public class ScoralingTest {
+  Scoral scoral;
+  Hopper hopper;
+  Elevator elevator;
+  Scoraling scoraling;
+
+  @BeforeEach
+  public void setup() {
+    setupTests();
+    scoral = Scoral.create();
+    elevator = Elevator.create();
+    hopper = Hopper.create();
+
+    scoraling = new Scoraling(hopper, scoral, elevator);
+  }
+
+  @AfterEach
+  public void destroy() throws Exception {
+    reset(scoral, elevator, hopper);
+  }
+
+  @Test
+  public void hpsIntakeTest() {
+    run(scoraling.hpsIntake());
+    fastForward();
+    assertEquals(
+        MIN_HEIGHT.in(Meters),
+        elevator.position(),
+        ElevatorConstants.POSITION_TOLERANCE.in(Meters));
+    assertTrue(hopper.getCurrentCommand().getName() == "intakingHPS");
+    assertTrue(scoral.getCurrentCommand().getName() == "intakingHPS");
+  }
+
+  @ParameterizedTest
+  @MethodSource("levels")
+  public void scoralTest(Level level) {
+    run(scoraling.scoral(level));
+    fastForward();
+    assertEquals(
+        level.height.in(Meters),
+        elevator.position(),
+        ElevatorConstants.POSITION_TOLERANCE.in(Meters));
+    assertTrue(scoral.getCurrentCommand().getName() == "scoraling");
+  }
+
+  @ParameterizedTest
+  @MethodSource("levels")
+  public void algaeCleanTest(Level level) {
+    if (level == Level.L1 || level == Level.L4) {}
+    run(scoraling.cleanAlgae(level));
+    fastForward();
+    assertEquals(
+        level.height.plus(algaeOffset).in(Meters),
+        elevator.position(),
+        ElevatorConstants.POSITION_TOLERANCE.in(Meters));
+    assertTrue(scoral.getCurrentCommand().getName() == "cleanAlgae");
+  }
+
+  private static Stream<Arguments> levels() {
+    return Stream.of(
+        Arguments.of(Level.L1),
+        Arguments.of(Level.L2),
+        Arguments.of(Level.L3),
+        Arguments.of(Level.L4));
+  }
+}