Skip to content

Commit

Permalink
A little clean up, ready for PR
Browse files Browse the repository at this point in the history
  • Loading branch information
@DragomHype
DragomHype committed Feb 23, 2025
1 parent 1f205da commit 3b5b0a5
Show file tree
Hide file tree
Showing 4 changed files with 214 additions and 201 deletions.
66 changes: 32 additions & 34 deletions src/main/java/frc/robot/Commands/TeleopCommands/PathfindingCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,14 +24,14 @@ public class PathfindingCommands {
* move again. The trajectory will automatically be rotated to the Red alliance.
*
* <p>Since a new trajectory is meant to be generated upon every button press, all the code must
* be inside of the return. This is done by returning a Commands.run() with a block of code inside
* of the lambda function for the {@link Runnable} parameter.
* be inside of the return. This is done by returning a {@code Commands.run()} with a block of
* code inside of the lambda function for the {@link Runnable} parameter.
*
* @param drive Drive subsystem
* @param vision Vision subsystem
* @param drive {@link Drive} subsystem
* @param vision {@link Vision} subsystem
* @param distanceFromTagMeters Distance in front of the AprilTag for the robot to end up.
* @param stopTrigger {@link BooleanSupplier} with the condition to end the Pathfinding command
* @return Command that makes the robot follow a trajectory to in front of the AprilTag.
* @param stopTrigger {@link BooleanSupplier} with the condition to end the Pathfinding command.
* @return {@link Command} that makes the robot follow a trajectory to in front of the AprilTag.
*/
public static Command pathfindToCurrentTag(
Drive drive,
Expand All @@ -40,7 +40,7 @@ public static Command pathfindToCurrentTag(
BooleanSupplier stopTrigger) {
return Commands.run(
() -> {
/**
/*
* Get ID of AprilTag currently seen by the front camera, if any. If an invalid ID is
* given the apriltagPose Optional will be empty
*/
Expand All @@ -51,20 +51,19 @@ public static Command pathfindToCurrentTag(
// If no valid tag returned then return a print messsage instead
if (apriltagPose.isEmpty()) {
Commands.print("Invalid AprilTag ID").until(stopTrigger).schedule();
;
} else {

// Turn 3d AprilTag pose into a 2d pose
var apriltagPose2d = apriltagPose.get().toPose2d();

/**
/*
* The goal pose is the end position for the center of the robot. Transforming by half
* the track width will leave the robot right up against the tag and any additional
* distance can be added
*/
var goalPose =
new Pose2d(
/**
/*
* Multiply the x by cos and y by sin of the tag angle so that the hypot (tag to
* robot) is the desired distance away from the tag
*/
Expand All @@ -89,23 +88,23 @@ public static Command pathfindToCurrentTag(
/**
* Generates a trajectory for the robot to follow to the AprilTag corresponding to the ID inputed
* with an additional distance translation. The trajectory will automatically be rotated to the
* Red alliance.
* red alliance.
*
* @param tagID AprilTag ID of the desired AprilTag to align to.
* @param wallDistanceMeters Distance in front of the AprilTag for the robot to end up.
* @return Command that makes the robot follow a trajectory to in front of the AprilTag.
* @return {@link Command} that makes the robot follow a trajectory to in front of the AprilTag.
*/
public static Command pathfindToAprilTag(IntSupplier tagID, DoubleSupplier wallDistanceMeters) {
// Get the 2d pose of the AprilTag associated with the inputed ID
var apriltagPose =
FieldConstants.APRILTAG_FIELD_LAYOUT.getTagPose(tagID.getAsInt()).get().toPose2d();
/**
/*
* The goal pose is the end position for the center of the robot. Transforming by half the track
* width will leave the robot right up against the tag and any additional distance can be added
*/
var goalPose =
new Pose2d(
/**
/*
* Multiply the x by cos and y by sin of the tag angle so that the hypot (tag to robot)
* is the desired distance away from the tag
*/
Expand All @@ -124,11 +123,11 @@ public static Command pathfindToAprilTag(IntSupplier tagID, DoubleSupplier wallD

/**
* Generates a trajectory for the robot to follow to a specified REEF BRANCH with an additional
* distance translation. The trajectory will automatically be rotated to the Red alliance.
* distance translation. The trajectory will automatically be rotated to the red alliance.
*
* @param branchLetter Letter corresponding to BRANCH to pathfind to.
* @param wallDistanceMeters Distance from the REEF wall in meters.
* @return Command that makes the robot follow a trajectory to in front of the BRANCH.
* @return {@link Command} that makes the robot follow a trajectory to in front of the BRANCH.
*/
public static Command pathfindToBranch(String branchLetter, DoubleSupplier wallDistanceMeters) {
// Position of BRANCH corresponding to zone the robot is in
Expand All @@ -138,10 +137,8 @@ public static Command pathfindToBranch(String branchLetter, DoubleSupplier wallD
// the BRANCH
var goalPose =
new Pose2d(
/**
* Multiply the x by cos and y by sin of the tag angle so that the hypot (tag to robot)
* is the desired distance away from the tag
*/
// Multiply the x by cos and y by sin of the tag angle so that the hypot (tag to robot)
// is the desired distance away from the tag
branchPose.getX()
+ ((DriveConstants.TRACK_WIDTH_M / 2)
+ FieldConstants.BRANCH_TO_WALL_X_M
Expand All @@ -164,13 +161,14 @@ public static Command pathfindToBranch(String branchLetter, DoubleSupplier wallD
* automatically be rotated to the Red alliance.
*
* <p>Since a new trajectory is meant to be generated upon every button press, all the code must
* be inside of the return. This is done by returning a Commands.run() with a block of code inside
* of the lambda function for the {@link Runnable} parameter.
* be inside of the return. This is done by returning a {@code Commands.run()} with a block of
* code inside of the lambda function for the {@link Runnable} parameter.
*
* @param drive Drive subsystem
* @param drive {@link Drive} subsystem
* @param wallDistanceMeters Distance from the REEF wall in meters.
* @param stopTrigger {@link BooleanSupplier} with the condition to end the Pathfinding command
* @return Command that makes the robot follow a trajectory to in front of the nearest BRANCH.
* @param stopTrigger {@link BooleanSupplier} with the condition to end the Pathfinding command.
* @return {@link Command} that makes the robot follow a trajectory to in front of the nearest
* BRANCH.
*/
public static Command pathfindToClosestReef(
Drive drive, DoubleSupplier wallDistanceMeters, BooleanSupplier stopTrigger) {
Expand Down Expand Up @@ -241,13 +239,13 @@ public static Command pathfindToClosestReef(
}
// Position of BRANCH corresponding to zone the robot is in
var branchPose = FieldConstants.BRANCH_POSES.get(branchLetter);

// Translated pose to send to Pathfinder, so that robot isn't commanded to go directly on
// top of
// the BRANCH
/*
* Translated pose to send to Pathfinder, so that robot isn't commanded to go directly on
* top of the BRANCH
*/
var goalPose =
new Pose2d(
/**
/*
* Multiply the x by cos and y by sin of the tag angle so that the hypot (tag to
* robot) is the desired distance away from the tag
*/
Expand All @@ -261,16 +259,16 @@ public static Command pathfindToClosestReef(
+ FieldConstants.BRANCH_TO_WALL_X_M
+ wallDistanceMeters.getAsDouble())
* branchPose.getRotation().getSin(),
// Rotate by 180 as the AprilTag angles are rotated 180 degrees relative to the
// robot
/*
* Rotate by 180 as the AprilTag angles are rotated 180 degrees relative to the
* robot
*/
branchPose.getRotation().plus(Rotation2d.k180deg));

// Create and follow the trajectory to the goal pose
AutoBuilder.pathfindToPoseFlipped(
goalPose, PathPlannerConstants.DEFAULT_PATH_CONSTRAINTS, 0)
.until(stopTrigger)
.alongWith(
Commands.print("Angle to REEF: " + thetaDeg + "\nBRANCH Letter: " + branchLetter))
.schedule();
},
drive);
Expand Down
10 changes: 6 additions & 4 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -175,8 +175,9 @@ public static Optional<Pose3d> getAprilTagPose(int ID) {
/** Angle from AprilTag to BRANCH that the hypotenuse makes */
double ARPILTAG_TO_BRANCH_ANGLE_RAD = Units.degreesToRadians(30);

// Initialize BRANCH poses
for (int i = 0; i < 6; i++) {
// Left BRANCH of face
// Left BRANCH of REEF face
var leftBranch =
new Pose2d(
CENTER_FACES[i].getX()
Expand All @@ -191,7 +192,7 @@ public static Optional<Pose3d> getAprilTagPose(int ID) {
+ CENTER_FACES[i].getRotation().getRadians())),
CENTER_FACES[i].getRotation());

// Right BRANCH of face
// Right BRANCH of REEF face
var rightBranch =
new Pose2d(
CENTER_FACES[i].getX()
Expand All @@ -206,6 +207,7 @@ public static Optional<Pose3d> getAprilTagPose(int ID) {
+ CENTER_FACES[i].getRotation().getRadians())),
CENTER_FACES[i].getRotation());

// Map poses to corresponding BRANCH letter
BRANCH_POSES.put(BRANCH_LETTERS.substring(i, i + 1), leftBranch);
BRANCH_POSES.put(BRANCH_LETTERS.substring(i + 6, i + 7), rightBranch);
}
Expand Down Expand Up @@ -245,7 +247,7 @@ public final class PathPlannerConstants {
public static final PathConstraints DEFAULT_PATH_CONSTRAINTS =
new PathConstraints(
5.2, 5.2, Units.degreesToRadians(515.65), Units.degreesToRadians(262.82));
/** Default distnace away from an AprilTag the robot should be when Pathfinding to it */
public static final double DEFAULT_APRILTAG_DISTANCE_M = Units.inchesToMeters(6);
/** Default distance away from any wall when the robot is Pathfinding towards one */
public static final double DEFAULT_WALL_DISTANCE_M = Units.inchesToMeters(6);
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,7 @@ public Robot() {
* 3: Commit number (of this branch),
* 4: Functionality: 0 = working, 1 = WIP, 2 = doesn't work
*/
SmartDashboard.putString("Version Number", "23.41.28.1");
SmartDashboard.putString("Version Number", "24.0.24.0");
SmartDashboard.putString("Last Deployed: ", BuildConstants.BUILD_DATE);

// Run a warmup command for the Pathfinder because the first command can potentially have a
Expand Down
Loading

0 comments on commit 3b5b0a5

Please sign in to comment.