diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fa59748..f6c4252 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,7 +40,7 @@ public class RobotContainer { // used to determine which section of the reef to score on, defaults at start to pole to front and // left of driver private int reefSection = 7; - + public static boolean stopGrab = false; // slows down robot during pickup private Command pickup = coral @@ -206,19 +206,19 @@ private void configureBindings() { // when b is pressed and if auto is disabled, raises elevator for l1 scoring, when left stick is // pressed, completes scoring motion // is auto is enabled, automatically goes to previously selected reef section and l1 scores - controller - .b() - .whileTrue( - Commands.deferredProxy( - () -> { - if (autoDisabled) { - return Commands.startEnd( - () -> speedMultiplier = 0.25, () -> speedMultiplier = 1.0) - .withDeadline(coral.l1Score(controller.leftStick())); - } else { - return coral.goToReef(drivetrain, () -> reefSection, () -> 0); - } - })); + /* controller + .b() + .whileTrue( + Commands.deferredProxy( + () -> { + if (autoDisabled) { + return Commands.startEnd( + () -> speedMultiplier = 0.25, () -> speedMultiplier = 1.0) + .withDeadline(coral.l1Score(controller.leftStick())); + } else { + return coral.goToReef(drivetrain, () -> reefSection, () -> 0); + } + }));*/ // clearing algae off reef, down on plus for l1 clear, right on plus for l2 clear controller.povDown().onTrue(coral.l1ReefClear(controller.leftStick())); @@ -248,6 +248,8 @@ private void configureBindings() { // when left plus pressed, disable or enable auto, defaults at disabled controller.povLeft().onTrue(Commands.runOnce(() -> autoDisabled = !autoDisabled)); controller.back().onTrue(Commands.runOnce(() -> gyro.zeroYaw())); + controller.leftStick().onTrue(Commands.runOnce(()->stopGrab = true)); + controller.leftStick().onFalse(Commands.runOnce(()->stopGrab = false)); } // deadbands for driving diff --git a/src/main/java/frc/robot/config/CoralConfig.java b/src/main/java/frc/robot/config/CoralConfig.java index c0b376b..b2aba7a 100644 --- a/src/main/java/frc/robot/config/CoralConfig.java +++ b/src/main/java/frc/robot/config/CoralConfig.java @@ -12,12 +12,12 @@ public class CoralConfig { public static final double VELOCITY_TOLERANCE = 1.0; public static final double INTAKE_HEIGHT = 0.065; - public static final Rotation2d INTAKE_ANGLE_UPPER = Rotation2d.fromRadians(5.9); - public static final Rotation2d INTAKE_ANGLE_LOWER = Rotation2d.fromRadians(5.7); + public static final Rotation2d INTAKE_ANGLE_UPPER = Rotation2d.fromRadians(1.65); + public static final Rotation2d INTAKE_ANGLE_LOWER = Rotation2d.fromRadians(1.7); // all heights are in meters - public static final double INTAKE_HEIGHT_SOURCE = 0.490; - public static final Rotation2d INTAKE_ANGLE_SOURCE = Rotation2d.fromRadians(.497); + public static final double INTAKE_HEIGHT_SOURCE = 0.55; + public static final Rotation2d INTAKE_ANGLE_SOURCE = Rotation2d.fromRadians(.75); public static final double MOVEMENT_DISTANCE = 1.75; public static final double L4_OFFSET = Units.inchesToMeters(-18); diff --git a/src/main/java/frc/robot/subsystems/Coral.java b/src/main/java/frc/robot/subsystems/Coral.java index abc9c02..3382a87 100644 --- a/src/main/java/frc/robot/subsystems/Coral.java +++ b/src/main/java/frc/robot/subsystems/Coral.java @@ -8,11 +8,13 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; import frc.robot.config.CoralConfig; import frc.robot.config.Positions; import java.util.Set; import java.util.function.BooleanSupplier; import java.util.function.IntSupplier; +import frc.robot.RobotContainer; @Logged public class Coral extends SubsystemBase { @@ -22,6 +24,7 @@ public class Coral extends SubsystemBase { new PIDController(CoralConfig.MOVEMENT_P, CoralConfig.MOVEMENT_I, CoralConfig.MOVEMENT_D); private CoralPivot coralPivot = new CoralPivot(); + // heights for levels private double[] elevatorScoringHeights = {0.0, 0.900, 1.29, 1.360}; @@ -33,10 +36,10 @@ public class Coral extends SubsystemBase { Rotation2d.fromRadians(0.3) }; private Rotation2d[] reefClearRotationsPrimary = { - Rotation2d.fromDegrees(-35), Rotation2d.fromDegrees(-35) + Rotation2d.fromRadians(1.5), Rotation2d.fromRadians(1.5) }; private Rotation2d[] reefClearRotationsSecondary = { - Rotation2d.fromDegrees(30), Rotation2d.fromDegrees(30) + Rotation2d.fromRadians(0.95), Rotation2d.fromRadians(0.95) }; private double[] reefClearHeights = {0.65, 1.0564}; @@ -48,7 +51,7 @@ public Coral() { grabber.setDefaultCommand( Commands.run( () -> { - if (!elevator.isAtPosition() || !coralPivot.isPivotReady()) { + if (!elevator.isAtPosition() || !coralPivot.isPivotReady() || !RobotContainer.stopGrab) { grabber.run(); } else { grabber.stop(); @@ -62,7 +65,6 @@ private Command score(int idx, BooleanSupplier completeScore) { return Commands.waitSeconds(0.3) .andThen(coralPivot.setPivotAngle(() -> coralScoringRotations[idx])) .alongWith(elevator.setElevatorHeight(() -> elevatorScoringHeights[idx])) - .alongWith(Commands.run(grabber::run, grabber)) .withDeadline( Commands.waitUntil(elevator::isAtPosition) .andThen(Commands.waitUntil(coralPivot::isPivotReady), Commands.waitSeconds(0.8))