From 50f402f36f718ecadbb9edb07621d112568566ac Mon Sep 17 00:00:00 2001 From: Team 2648 Date: Tue, 18 Mar 2025 19:02:16 -0400 Subject: [PATCH] algae barge shot is in, but needs tuning --- src/main/java/frc/robot/RobotContainer.java | 11 +++++++++++ .../robot/constants/ManipulatorPivotConstants.java | 2 ++ 2 files changed, 13 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index aa03c21..1e16628 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,6 +34,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; public class RobotContainer { private ClimberPivot climberPivot; @@ -226,6 +227,8 @@ public class RobotContainer { .until(() -> driver.a().getAsBoolean()) ); + operator.rightTrigger().onTrue(shootAlgae()); + } private void configureNamedCommands() { new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)); @@ -489,6 +492,14 @@ public class RobotContainer { .raceWith(elevator.maintainPosition())); } + private Command shootAlgae(){ + return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition) + .andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition) + .raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>30).andThen(manipulator.runManipulator(() -> -1, false), + elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition) + .raceWith(elevator.maintainPosition())); + } + @SuppressWarnings("unused") private Command startingConfig() { return moveManipulatorUtil(0, 0, false, true) diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index 8f21adb..b28d6e6 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -48,6 +48,8 @@ public class ManipulatorPivotConstants { public static final double kNetPosition = Units.degreesToRadians(175.0+90); /**The closest position to the elevator brace without hitting it */ public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0+90); + + public static final double kBargeShotPosition = Units.degreesToRadians(222); /**The forward rotation limit of the pivot */ public static final double kRotationLimit = Units.degreesToRadians(175.0+90);