From 9d6d134d7b9b57bde050ed9369fba424a2bbb84a Mon Sep 17 00:00:00 2001 From: Noah-H3467 Date: Sat, 15 Feb 2025 14:50:37 -0500 Subject: [PATCH] merge --- src/main/java/frc/robot/RobotContainer.java | 74 ++++++------------- .../Claw/ClawRoller/ClawRoller.java | 5 +- 2 files changed, 26 insertions(+), 53 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5851948..52812e2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -375,17 +375,10 @@ private void configureControllerBindings() // Driver Left Trigger + Right Bumper: Algae Intake - // m_driver.leftTrigger().and(isCoralMode.negate()).whileTrue( - // Commands.sequence( - // (FieldConstants.getNearestReefBranch(m_drive.getPose(), ReefSide.RIGHT) - // .getTranslation().getX() > 0) - // ? m_profiledElevator.setStateCommand(Elevator.State.ALGAE_HIGH) - // : m_profiledElevator.setStateCommand(Elevator.State.ALGAE_LOW), - // m_clawRoller.setStateCommand(ClawRoller.State.INTAKE), - // Commands.waitUntil(m_clawRoller.stalled) - // .andThen(m_clawRoller.setStateCommand(ClawRoller.State.OFF))) - // .andThen(m_profiledElevator.setStateCommand(Elevator.State.STOW))); - // m_driver.rightTrigger().whileTrue(m_clawRoller.setStateCommand(ClawRoller.State.SCORE)); + m_driver.leftTrigger().and(isCoralMode.negate()).onTrue( + (m_clawRoller.getState() == ClawRoller.State.ALGAE_INTAKE) + ? m_clawRoller.setStateCommandNoEnd(ClawRoller.State.EJECT) + : m_clawRoller.setStateCommandNoEnd(ClawRoller.State.ALGAE_INTAKE)); // Driver Start Button: Climb Request (toggle) // m_driver.start().onTrue(Commands.runOnce(() -> { @@ -433,9 +426,14 @@ private void configureControllerBindings() // Driver POV Down: Zero the Elevator (HOMING) m_driver.povDown().whileTrue( - m_profiledElevator.setStateCommand(Elevator.State.HOMING) - .until(m_profiledElevator.getHomedTrigger()) - .andThen(m_profiledElevator.zeroSensorCommand())); + Commands.sequence( + // Always move Arm to STOW position before moving Elevator + m_profiledArm.setStateCommand(Arm.State.STOW).until(() -> m_profiledArm.atPosition(0.1)), + // Move Elevator to homing position + Commands.waitUntil(() -> m_profiledArm.atPosition(0.1)) + .andThen(m_profiledElevator.setStateCommand(Elevator.State.HOMING) + .until(m_profiledElevator.getHomedTrigger())) + .andThen(m_profiledElevator.zeroSensorCommand()))); // Driver Right Bumper: Toggle between Coral and Algae Modes. // Make sure the Approach nearest reef face does not mess with this @@ -452,38 +450,23 @@ private void registerNamedCommands() // Go to the L1 Position NamedCommands.registerCommand( "L1", - Commands.parallel( - m_profiledElevator.setStateCommand(Elevator.State.LEVEL_1), - m_profiledArm.setStateCommand(Arm.State.LEVEL_1))); + m_superStruct.getTransitionCommand(Arm.State.LEVEL_1, Elevator.State.LEVEL_1)); // Go to the L2 Position NamedCommands.registerCommand( "L2", - Commands.parallel( - m_profiledElevator.setStateCommand(Elevator.State.LEVEL_2), - m_profiledArm.setStateCommand(Arm.State.LEVEL_2))); + m_superStruct.getTransitionCommand(Arm.State.LEVEL_2, Elevator.State.LEVEL_2)); // Go to the L3 Position NamedCommands.registerCommand( "L3", - Commands.parallel( - m_profiledElevator.setStateCommand(Elevator.State.LEVEL_3), - m_profiledArm.setStateCommand(Arm.State.LEVEL_3))); + m_superStruct.getTransitionCommand(Arm.State.LEVEL_3, Elevator.State.LEVEL_3)); // Go to the L4 Position NamedCommands.registerCommand( "L4", - Commands.parallel( - m_profiledElevator.setStateCommand(Elevator.State.LEVEL_4), - Commands.waitUntil(() -> m_profiledElevator.atPosition(1)) - .andThen(m_profiledArm.setStateCommand(Arm.State.LEVEL_4)))); + m_superStruct.getTransitionCommand(Arm.State.LEVEL_4, Elevator.State.LEVEL_4)); // Go to the Home Position NamedCommands.registerCommand( "Home", - Commands.parallel( - m_profiledElevator.setStateCommand(Elevator.State.STOW), - m_profiledArm.setStateCommand(Arm.State.STOW))); - - // Wait for intake laserCAN to be triggered - NamedCommands.registerCommand("WaitForCoral", - Commands.waitUntil(m_intakeLaserCAN.triggered)); + m_superStruct.getTransitionCommand(Arm.State.STOW, Elevator.State.STOW)); // Wait for intake laserCAN to be triggered NamedCommands.registerCommand("WaitForCoral", @@ -492,23 +475,12 @@ private void registerNamedCommands() // Intake Coral NamedCommands.registerCommand( "IntakeCoral", - m_clawRoller.setStateCommand(ClawRoller.State.INTAKE) - .until(m_clawRollerLaserCAN.triggered) - .andThen(m_clawRoller.setStateCommandNoEnd(ClawRoller.State.HOLDCORAL)) - .andThen(Commands.waitUntil(() -> m_clawRoller.atPosition(1)))); - - // Intake Algae - NamedCommands.registerCommand( - "IntakeAlgae", - Commands.sequence( - (FieldConstants.getNearestReefBranch(m_drive.getPose(), ReefSide.RIGHT) - .getTranslation().getX() > 0) - ? m_profiledElevator.setStateCommand(Elevator.State.ALGAE_HIGH) - : m_profiledElevator.setStateCommand(Elevator.State.ALGAE_LOW), - m_clawRoller.setStateCommand(ClawRoller.State.INTAKE), - Commands.waitUntil(m_clawRoller.stalled) - .andThen(m_clawRoller.setStateCommand(ClawRoller.State.OFF))) - .andThen(m_profiledElevator.setStateCommand(Elevator.State.STOW))); + Commands.parallel( + m_superStruct.getTransitionCommand(Arm.State.CORAL_INTAKE, Elevator.State.CORAL_INTAKE), + m_clawRoller.setStateCommand(ClawRoller.State.INTAKE) + .until(m_clawRollerLaserCAN.triggered) + .andThen(m_clawRoller.setStateCommandNoEnd(ClawRoller.State.HOLDCORAL)) + .andThen(Commands.waitUntil(() -> m_clawRoller.atPosition(1))))); NamedCommands.registerCommand( "Score", diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java index 9ff04e9..d1361e0 100644 --- a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java +++ b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java @@ -21,6 +21,7 @@ public class ClawRoller // numbers static LoggedTunableNumber holdCoralSP = new LoggedTunableNumber("ClawRoller/HoldCoralSP", 0.0); + static LoggedTunableNumber algaeIntakeSP = new LoggedTunableNumber("ClawRoller/AlgaeIntakeSP", -15.0); @RequiredArgsConstructor @Getter @@ -29,8 +30,8 @@ public enum State implements TargetState { INTAKE(() -> 2.0, ProfileType.OPEN_VOLTAGE), EJECT(() -> 6.0, ProfileType.OPEN_VOLTAGE), SCORE(() -> 8.0, ProfileType.OPEN_VOLTAGE), - HOLDCORAL(() -> 0.6, ProfileType.MM_POSITION), // Wheels should spin x rotations before stopping - TUNING(() -> holdCoralSP.getAsDouble(), ProfileType.MM_POSITION); + HOLDCORAL(() -> holdCoralSP.getAsDouble(), ProfileType.MM_POSITION), + ALGAE_INTAKE(() -> algaeIntakeSP.getAsDouble(), ProfileType.OPEN_CURRENT); private final DoubleSupplier output; private final ProfileType profileType;