diff --git a/src/main/java/org/sciborgs1155/robot/commands/Scoraling.java b/src/main/java/org/sciborgs1155/robot/commands/Scoraling.java index 07e86e3..a8454c6 100644 --- a/src/main/java/org/sciborgs1155/robot/commands/Scoraling.java +++ b/src/main/java/org/sciborgs1155/robot/commands/Scoraling.java @@ -16,28 +16,36 @@ public Scoraling(Hopper hopper, Scoral scoral, Elevator elevator) { this.scoral = scoral; this.elevator = elevator; - hopper.beambreakTrigger.onTrue(hopper.stop().alongWith(scoral.stop())); - scoral.beambreakTrigger.onFalse(hopper.stop().alongWith(scoral.stop())); + hopper.beambreakTrigger.onTrue(stop()); + scoral.beambreakTrigger.onFalse(stop()); } + /** intakes from the human player station */ public Command hpsIntake() { - return elevator.retract().andThen(run()).onlyIf(scoral.beambreakTrigger); + return elevator.retract().andThen(runRollers()).onlyIf(scoral.beambreakTrigger); } + /** + * 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) { return elevator.scoreLevel(level).andThen(scoral.outtake()); } - /** ONLY L2 and L3 */ + /** grabs the algae from the level given (goes above the level) ONLY L2 and L3 */ public Command grabAlgae(Level level) { return elevator.clean(level).andThen(scoral.intake()).onlyIf(scoral.beambreakTrigger); } - public Command halt() { + /** halts both the hopper and the scoral */ + public Command stop() { return hopper.stop().alongWith(scoral.stop()); } - public Command run() { + /** runs the hps + scoral rollers forward (intaking) */ + public Command runRollers() { return hopper.intake().alongWith(scoral.outtake()); } } diff --git a/src/main/java/org/sciborgs1155/robot/elevator/Elevator.java b/src/main/java/org/sciborgs1155/robot/elevator/Elevator.java index d24e810..07b92e1 100644 --- a/src/main/java/org/sciborgs1155/robot/elevator/Elevator.java +++ b/src/main/java/org/sciborgs1155/robot/elevator/Elevator.java @@ -24,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; @@ -106,7 +108,11 @@ public Command scoreLevel(Level level) { /** 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) { - throw new RuntimeException("Not a valid algae height! Only L2 and L3!"); + 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));