From c2e31ac89981288bd876af814af1f0862d89a80d Mon Sep 17 00:00:00 2001 From: Noah-H3467 Date: Sat, 1 Feb 2025 13:49:42 -0500 Subject: [PATCH] Cardinal (targets like processor) and Heading (reef orbit) --- src/main/java/frc/robot/RobotContainer.java | 23 ++++++++ src/main/java/frc/robot/RobotState.java | 62 ++++++++++++++++++++- 2 files changed, 82 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 377cb82..22e4ebc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -398,7 +398,30 @@ private void configureControllerBindings() MetersPerSecond.of(1), // The coral is ejected vertically downwards Degrees.of(-75))))); + + // Driver START Button: Auto Angle to closest Reef target TODO: FIX + m_driver + .povRight() + .whileTrue( + Commands.parallel( + RobotState.getInstance().setTargetCommand(RobotState.getInstance().chooseReefTarget()), + DriveCommands.joystickDriveAtAngle( + m_drive, + () -> -m_driver.getLeftY(), + () -> -m_driver.getLeftX(), + () -> RobotState.getInstance().getAngleToTarget(m_drive.getPose().getTranslation())))); + // Driver Right bumper: Cardinal direction to PROCESSOR + m_driver + .povUp() + .whileTrue( + Commands.parallel( + RobotState.getInstance().setTargetCommand(RobotState.TARGET.PROCESSOR), + DriveCommands.joystickDriveAtAngle( + m_drive, + () -> -m_driver.getLeftY(), + () -> -m_driver.getLeftX(), + () -> RobotState.getInstance().getAngleOfTarget().plus(Rotation2d.fromDegrees(180))))); } /** diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index efe6254..51d77f8 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -9,6 +9,10 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.Setter; @@ -28,11 +32,20 @@ public class RobotState { @RequiredArgsConstructor @Getter public enum TARGET { - LEFT_CORAL_STATION(FieldConstants.CoralStation.leftCenterFace.getTranslation()), - RIGHT_CORAL_STATION(FieldConstants.CoralStation.rightCenterFace.getTranslation()), - REEF(FieldConstants.Reef.center); + LEFT_CORAL_STATION(FieldConstants.CoralStation.leftCenterFace.getTranslation(), FieldConstants.CoralStation.leftCenterFace.getRotation()), + RIGHT_CORAL_STATION(FieldConstants.CoralStation.rightCenterFace.getTranslation(), FieldConstants.CoralStation.rightCenterFace.getRotation()), + REEF(FieldConstants.Reef.center, null), + REEF_AB(FieldConstants.Reef.centerFaces[0].getTranslation(), FieldConstants.Reef.centerFaces[0].getRotation()), + REEF_CD(FieldConstants.Reef.centerFaces[5].getTranslation(), FieldConstants.Reef.centerFaces[5].getRotation()), + REEF_EF(FieldConstants.Reef.centerFaces[4].getTranslation(), FieldConstants.Reef.centerFaces[4].getRotation()), + REEF_GH(FieldConstants.Reef.centerFaces[3].getTranslation(), FieldConstants.Reef.centerFaces[3].getRotation()), + REEF_IJ(FieldConstants.Reef.centerFaces[2].getTranslation(), FieldConstants.Reef.centerFaces[2].getRotation()), + REEF_KL(FieldConstants.Reef.centerFaces[1].getTranslation(), FieldConstants.Reef.centerFaces[1].getRotation()), + PROCESSOR(FieldConstants.Processor.centerFace.getTranslation(), FieldConstants.Processor.centerFace.getRotation()), + BARGE(FieldConstants.Barge.middleCage, Rotation2d.fromDegrees(180));; private final Translation2d targetTranslation; + private final Rotation2d targetRotation; } @Getter @@ -53,6 +66,44 @@ public Rotation2d getAngleToTarget(Translation2d currentTranslation) return target.targetTranslation.minus(currentTranslation).getAngle(); } + // For Cardinal targets, such as the Processor, Net, Barge, and Coral Substations + public Rotation2d getAngleOfTarget() { + if (target.targetRotation == null) { + return null; + } + // Return the angle to align to target + return target.targetRotation; + } + + public TARGET chooseReefTarget() { + + // Get the angle of the vector from the robot to the reef + double angle = getAngleToTarget(TARGET.REEF.getTargetTranslation()).getDegrees(); + + // Now get the angle of the nearest target + if (((angle >= -30) && (angle < 30))) { + return (TARGET.REEF_GH); + } else if ((angle >= 30) && (angle < 90)) { + return (TARGET.REEF_IJ); + } else if ((angle >= 90) && (angle < 150)) { + return (TARGET.REEF_KL); + } else if (((angle >= 150) && (angle <= 180)) || ((angle > -180) && (angle < -150))) { + return (TARGET.REEF_AB); + } else if ((angle >= -150) && (angle < -90)) { + return (TARGET.REEF_CD); + } else if ((angle >= -90) && (angle < -30)) { + return (TARGET.REEF_EF); + } else { + return (TARGET.REEF); // Catch all + } + } + + public double getDistanceToTarget(Pose2d currentPose) { + return currentPose + .getTranslation() + .getDistance(target.targetTranslation); + } + private Translation2d getFuturePose() { // If magnitude of velocity is low enough, use current pose @@ -91,4 +142,9 @@ private Translation2d getFuturePose() feedArmAngleMap.put(6.0, -10.0); feedArmAngleMap.put(7.0, -19.0); } + + public Command setTargetCommand(TARGET target) { + return Commands.startEnd(() -> setTarget(target), () -> setTarget(TARGET.REEF)) + .withName("Set Robot Target: " + target.toString()); + } }