From 5e01a3d4cef673281d7fa295cb1bf6291cf1b0f4 Mon Sep 17 00:00:00 2001 From: MysticalApple Date: Wed, 3 Apr 2024 14:30:36 -0700 Subject: [PATCH] added command to auto align with stage/chain --- .../robot/commands/swerve/AlignCommand.java | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/src/main/java/frc/robot/commands/swerve/AlignCommand.java b/src/main/java/frc/robot/commands/swerve/AlignCommand.java index 69e12133..f6e2f888 100644 --- a/src/main/java/frc/robot/commands/swerve/AlignCommand.java +++ b/src/main/java/frc/robot/commands/swerve/AlignCommand.java @@ -1,5 +1,12 @@ package frc.robot.commands.swerve; +import static frc.robot.Constants.AutoAlignConstants.BLUE_STAGE_BACK_POSE; +import static frc.robot.Constants.AutoAlignConstants.BLUE_STAGE_LEFT_POSE; +import static frc.robot.Constants.AutoAlignConstants.BLUE_STAGE_RIGHT_POSE; +import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_BACK_POSE; +import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_LEFT_POSE; +import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_RIGHT_POSE; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.math.geometry.Pose2d; @@ -65,4 +72,32 @@ public static Command getAmpAlignCommand(SwerveSubsystem swerveSubsystem, Boolea return command; } + + /** + * Generates a holonomic path from the robot to the nearest stage face. The robot should end up with its climb hooks + * aligned directly above the chain. + * + * @param swerveSubsystem The swerve drive to move the robot to its target. + * @param isRed True if the robot is on the red alliance, false if on the blue alliance. + * @return Pathfinding Command to bring the robot to its target position. + */ + public static Command getStageAlignCommand(SwerveSubsystem swerveSubsystem, Boolean isRed) { + Pose2d currentPose = swerveSubsystem.getRobotPosition(); + Pose2d targetPose = new Pose2d(); + double distance = Double.MAX_VALUE; + + Pose2d[] possibleTargets = isRed + ? new Pose2d[]{RED_STAGE_BACK_POSE, RED_STAGE_LEFT_POSE, RED_STAGE_RIGHT_POSE} + : new Pose2d[]{BLUE_STAGE_BACK_POSE, BLUE_STAGE_LEFT_POSE, BLUE_STAGE_RIGHT_POSE}; + + for (Pose2d possibleTarget : possibleTargets) { + double possibleDistance = currentPose.getTranslation().getDistance(possibleTarget.getTranslation()); + if (possibleDistance < distance) { + distance = possibleDistance; + targetPose = possibleTarget; + } + } + + return getAlignCommand(targetPose, swerveSubsystem); + } } \ No newline at end of file