diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index 49194bd..f5bc1e1 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -155,25 +155,7 @@ public enum ScoringLevel { this.rotationTransform = rotationTransform; this.armElevatorState = determineArmElevatorState(); } - - /** - * Calculates the target placing position using the clock position and the target reef side. - * The reef side transform will be flipped depending on operator input. - * To make it more intuitive for the operator to input the reef side, - * left will always correspond to the physical left side in the driver station, - * as opposed to "reef relative" left. - * - * @param reefClockPosition the desired clock position of the reef - * @param reefSide the desired side of the reef, left or right (as seen from the driver station) - * @return the target placing position - */ - public FlippablePose2d calculateTargetPlacingPosition(FieldConstants.ReefClockPosition reefClockPosition, FieldConstants.ReefSide reefSide) { - final Pose2d reefCenterPose = new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, reefClockPosition.clockAngle); - final double yTransform = reefSide.shouldFlipYTransform(reefClockPosition) ? -positiveYTransformMeters : positiveYTransformMeters; - final Transform2d transform = new Transform2d(xTransformMeters, yTransform, rotationTransform); - - return new FlippablePose2d(reefCenterPose.plus(transform), true); - } + private ArmElevatorConstants.ArmElevatorState determineArmElevatorState() { return switch (level) { diff --git a/src/main/java/frc/trigon/robot/misc/ReefChooser.java b/src/main/java/frc/trigon/robot/misc/ReefChooser.java index 3c02d96..a7a5b88 100644 --- a/src/main/java/frc/trigon/robot/misc/ReefChooser.java +++ b/src/main/java/frc/trigon/robot/misc/ReefChooser.java @@ -8,13 +8,11 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; -import lib.utilities.flippable.FlippablePose2d; import java.util.function.Supplier; public class ReefChooser { private final CommandGenericHID hid; - private FieldConstants.ReefClockPosition clockPosition = FieldConstants.ReefClockPosition.REEF_6_OCLOCK; private CoralPlacingCommands.ScoringLevel scoringLevel = CoralPlacingCommands.ScoringLevel.L4; private FieldConstants.ReefSide reefSide = FieldConstants.ReefSide.LEFT; @@ -28,21 +26,6 @@ public CoralPlacingCommands.ScoringLevel getScoringLevel() { return scoringLevel; } - public void switchReefSide() { - reefSide = reefSide == FieldConstants.ReefSide.LEFT ? FieldConstants.ReefSide.RIGHT : FieldConstants.ReefSide.LEFT; - } - - public FieldConstants.ReefClockPosition getClockPosition() { - return clockPosition; - } - - public FieldConstants.ReefSide getReefSide() { - return reefSide; - } - - public FlippablePose2d calculateTargetScoringPose() { - return scoringLevel.calculateTargetPlacingPosition(clockPosition, reefSide); - } public ArmElevatorConstants.ArmElevatorState getArmElevatorState() { return scoringLevel.armElevatorState; @@ -74,52 +57,22 @@ private void configureReefHIDBindings() { private Command getSetFaceFromIndexCommand(int index) { return new InstantCommand( - () -> setFaceFromIndex(index) + () -> setFaceSideFromIndex(index) ).ignoringDisable(true); } - private void setFaceFromIndex(int index) { - clockPosition = FieldConstants.ReefClockPosition.values()[index / 2]; + private void setFaceSideFromIndex(int index) { reefSide = FieldConstants.ReefSide.values()[index % 2]; } private void configureFallbackBindings() { - OperatorConstants.SET_TARGET_SCORING_REEF_CLOCK_POSITION_2_OCLOCK_TRIGGER.onTrue(getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition.REEF_2_OCLOCK)); - OperatorConstants.SET_TARGET_SCORING_REEF_CLOCK_POSITION_4_OCLOCK_TRIGGER.onTrue(getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition.REEF_4_OCLOCK)); - OperatorConstants.SET_TARGET_SCORING_REEF_CLOCK_POSITION_6_OCLOCK_TRIGGER.onTrue(getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition.REEF_6_OCLOCK)); - OperatorConstants.SET_TARGET_SCORING_REEF_CLOCK_POSITION_8_OCLOCK_TRIGGER.onTrue(getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition.REEF_8_OCLOCK)); - OperatorConstants.SET_TARGET_SCORING_REEF_CLOCK_POSITION_10_OCLOCK_TRIGGER.onTrue(getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition.REEF_10_OCLOCK)); - OperatorConstants.SET_TARGET_SCORING_REEF_CLOCK_POSITION_12_OCLOCK_TRIGGER.onTrue(getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition.REEF_12_OCLOCK)); - OperatorConstants.SET_TARGET_REEF_SCORING_SIDE_LEFT_TRIGGER.onTrue(getSetTargetReefSideCommand(FieldConstants.ReefSide.LEFT)); OperatorConstants.SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER.onTrue(getSetTargetReefSideCommand(FieldConstants.ReefSide.RIGHT)); } - private Command getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition clockPosition) { - return new InstantCommand( - () -> this.clockPosition = clockPosition - ).ignoringDisable(true); - } - private Command getSetTargetReefSideCommand(FieldConstants.ReefSide reefSide) { return new InstantCommand( () -> this.reefSide = reefSide ).ignoringDisable(true); } - - private CoralPlacingCommands.ScoringLevel getIncrementedScoringLevel() { - return switch (scoringLevel) { - case L2 -> CoralPlacingCommands.ScoringLevel.L3; - case L1 -> CoralPlacingCommands.ScoringLevel.L2; - default -> CoralPlacingCommands.ScoringLevel.L4; - }; - } - - private CoralPlacingCommands.ScoringLevel getDecrementedScoringLevel() { - return switch (scoringLevel) { - case L4 -> CoralPlacingCommands.ScoringLevel.L3; - case L3 -> CoralPlacingCommands.ScoringLevel.L2; - default -> CoralPlacingCommands.ScoringLevel.L1; - }; - } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java index 1ee51ce..a547ab7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java @@ -336,7 +336,7 @@ public enum ArmElevatorState { REST_FOR_CLIMB(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), LOAD_CORAL(Rotation2d.fromDegrees(0), 0.5519, REST, true, 0.7), EJECT(Rotation2d.fromDegrees(60), 0.603, null, false, 0.7), - SCORE_L1(Rotation2d.fromDegrees(45), 0.603, null, false, 1), + SCORE_L1(Rotation2d.fromDegrees(70), 0.4, null, false, 1), SCORE_L2(Rotation2d.fromDegrees(90), 0.3, PREPARE_SCORE_L2, false, 1), SCORE_L3(Rotation2d.fromDegrees(90), 0.7, PREPARE_SCORE_L3, false, 1), SCORE_L4(Rotation2d.fromDegrees(100), 1.2, PREPARE_SCORE_L4, false, 1),