diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 281ad9c..6acfa68 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -45,4 +45,4 @@ "{\"name\":\"Ground Intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.34,\"y\":0.0},\"size\":{\"width\":0.8,\"length\":0.1},\"borderRadius\":0.02,\"strokeWidth\":0.01,\"filled\":false}}", "{\"name\":\"Circle\",\"type\":\"circle\",\"data\":{\"center\":{\"x\":-0.46,\"y\":0.0},\"radius\":0.032,\"strokeWidth\":0.01,\"filled\":true}}" ] -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9ad167f..22bf4d7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -192,8 +192,8 @@ public static final class ElevatorConstants { public static final Distance kL2 = Inches.of(48.125); public static final Distance kL3 = Inches.of(56.25); public static final Distance kL4 = Inches.of(68.125); - public static final Distance KAlgae1 = Inches.of(58.375); - public static final Distance KAlgae2 = Inches.of(65.75); + public static final Distance KAlgaeLower = Inches.of(58.375); + public static final Distance KAlgaeUpper = Inches.of(65.75); // Motion Magic constants public static final LinearVelocity kVelocity = MetersPerSecond.of(1.8); public static final LinearAcceleration kAcceleration = MetersPerSecondPerSecond.of(2.8); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c184183..f3b715c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -265,16 +265,18 @@ public RobotContainer() { .withTimeout(1) .andThen(Commands.run(() -> m_coralScorer.setCoralPercent(.70), m_coralScorer)))); NamedCommands.registerCommand("L3", Commands.print("L3")); // Just print commands for right now. - NamedCommands.registerCommand("L2", Commands.parallel( // Needs to be canceled with a race group right now, the race group wait - // timer is at 0.8 seconds. - new ElevatorCommand( - () -> ElevatorConstants.kL2, // Change this to kL4 or kL3 for those levels - ElevatorConstants.kAcceleration, - ElevatorConstants.kVelocity, - m_elevator), - Commands.run(() -> m_coralScorer.setCoralPercent(.0), m_coralScorer) - .withTimeout(0.4) - .andThen(Commands.run(() -> m_coralScorer.setCoralPercent(.70), m_coralScorer)))); + NamedCommands.registerCommand( + "L2", + Commands.parallel( // Needs to be canceled with a race group right now, the race group wait + // timer is at 0.8 seconds. + new ElevatorCommand( + () -> ElevatorConstants.kL2, // Change this to kL4 or kL3 for those levels + ElevatorConstants.kAcceleration, + ElevatorConstants.kVelocity, + m_elevator), + Commands.run(() -> m_coralScorer.setCoralPercent(.0), m_coralScorer) + .withTimeout(0.4) + .andThen(Commands.run(() -> m_coralScorer.setCoralPercent(.70), m_coralScorer)))); NamedCommands .registerCommand( // Brings the elevator to the ground. Put after the race group to score. diff --git a/src/main/java/frc/robot/subsystems/state_keeper/ReefTarget.java b/src/main/java/frc/robot/subsystems/state_keeper/ReefTarget.java index cb9a535..58a85b1 100644 --- a/src/main/java/frc/robot/subsystems/state_keeper/ReefTarget.java +++ b/src/main/java/frc/robot/subsystems/state_keeper/ReefTarget.java @@ -57,7 +57,6 @@ private ReefTarget() {} /** Periodic function includes logging and publishing to NT */ public synchronized void periodic() { - getElevatorDelay(); // Log to AdvantageKit Logger.recordOutput("ReefTarget/Post_ALL", convertIntToAlphabet(reefPostAll + 1)); Logger.recordOutput("ReefTarget/Post_LR", reefPostLR); @@ -162,16 +161,20 @@ public Distance getElevatorHeight() { } } - public void getElevatorDelay() { - - if (reefLevel == 2) { - elevatorDelay = .35; - } else if (reefLevel == 3) { - elevatorDelay = .55; - } else if (reefLevel == 4) { - elevatorDelay = .85; - } else { - elevatorDelay = .35; + /** + * Return the elevator height needed to access the ALGAE based on the currently selected REEF face + */ + public Distance getElevatorAlgae() { + switch ((reefPostAll / 2) % 2) { + case 0: + // Reef Face A/B, E/F, I/J -- ALGAE between L3 and L4 + return ElevatorConstants.KAlgaeUpper; + case 1: + // Reef Face C/D, G/H, K/L -- ALGAE between L2 and L3 + return ElevatorConstants.KAlgaeLower; + default: + // Shouldn't run, but you know... + return ElevatorConstants.kElevatorZeroHeight; } }