made AlignCommand more explanable

Author: jhhbrown1

src/main/java/frc/robot/Robot.java

@@ -5,13 +5,13 @@
 package frc.robot;
 
 import static frc.robot.Constants.RobotConstants.*;
+import static frc.robot.subsystems.PoseEstimationSubsystem.*;
 
 import java.util.Map;
 
 import org.littletonrobotics.urcl.URCL;
 import org.photonvision.PhotonCamera;
 
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.wpilibj.DataLogManager;
@@ -40,10 +40,10 @@ public class Robot extends TimedRobot {
 	private final PowerDistribution m_pdh = new PowerDistribution();
 	PhotonCamera m_camera1 = RobotBase.isSimulation()
 			? new PhotonCameraSimulator("Camera1", kRobotToCamera1, m_driveSubsystem,
-					new Pose2d(1, 1, Rotation2d.fromDegrees(0)),
+					pose(1, 1, 0),
 					0.01, 3,
 					0.1)
-			: new PhotonCamera("Cool camera"); // TODO: Check the camera name
+			: new PhotonCamera("Cool camera");
 	private final PoseEstimationSubsystem m_poseEstimationSubystem = new PoseEstimationSubsystem(m_driveSubsystem,
 			m_camera1);
 
@@ -60,17 +60,14 @@ public Robot() {
 		bindDriveControls();
 	}
 
-	public static Pose2d pose(double x, double y, double yawInDegrees) {
-		return new Pose2d(x, y, Rotation2d.fromDegrees(yawInDegrees));
-	}
-
 	public void bindDriveControls() {
 		m_driveSubsystem.setDefaultCommand(
 				m_driveSubsystem.driveCommand(
 						() -> -m_driverController.getLeftY(),
 						() -> -m_driverController.getLeftX(),
 						() -> m_driverController.getR2Axis() - m_driverController.getL2Axis(),
 						m_driverController.getHID()::getSquareButton));
+
 		double distanceTolerance = 0.05;
 		double angleTolerance = 5;
 		m_driverController.button(Button.kSquare) // home
@@ -86,12 +83,22 @@ public void bindDriveControls() {
 						new DriveCommand(
 								m_driveSubsystem, pose(2, 0, 0), distanceTolerance, angleTolerance));
 
-		Transform2d robotToTarget = new Transform2d(1.5, 0, Rotation2d.fromDegrees(180));
+		double angleOfCoverageInDegrees = 90;
+		double distanceThresholdInMeters = 4;
 		m_driverController.button(Button.kTriangle)
+				.whileTrue(
+						AlignCommand.turnToClosestTag(
+								m_driveSubsystem, m_poseEstimationSubystem, angleOfCoverageInDegrees,
+								distanceThresholdInMeters,
+								distanceTolerance, angleTolerance));
+
+		Transform2d robotToTarget = new Transform2d(1.5, 0, Rotation2d.fromDegrees(180));
+		m_driverController.button(Button.kLeftBumper)
 				.whileTrue(
 						AlignCommand.moveToClosestTag(
-								m_driveSubsystem, m_poseEstimationSubystem, 90, 3, robotToTarget,
-								.2, 10));
+								m_driveSubsystem, m_poseEstimationSubystem, angleOfCoverageInDegrees,
+								distanceThresholdInMeters, robotToTarget,
+								distanceTolerance, angleTolerance));
 	}
 
 	@Override

src/main/java/frc/robot/commands/AlignCommand.java

@@ -1,14 +1,8 @@
 package frc.robot.commands;
 
-import static frc.robot.Constants.*;
-
-import java.util.Map;
-import java.util.Map.Entry;
-import java.util.Optional;
 import java.util.function.Supplier;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -35,14 +29,102 @@ public class AlignCommand extends Command {
 	 * @return a {@code Commmand} for moving the robot to the specified
 	 *         target
 	 */
-	public static DriveCommand moveTo(DriveSubsystem driveSubsystem,
-			PoseEstimationSubsystem poseEstimationSubsystem,
+	public static DriveCommand moveTo(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubsystem,
 			Pose2d targetPose, double distanceTolerance, double angleTolerance) {
-		return new DriveCommand(driveSubsystem, () -> poseEstimationSubsystem.getPose(),
-				() -> targetPose,
+		return moveTo(driveSubsystem, poseEstimationSubsystem, () -> targetPose, distanceTolerance, angleTolerance);
+	}
+
+	/**
+	 * Constructs a {@code DriveCommand} for moving the robot to the specified
+	 * target.
+	 * 
+	 * @param driveSubsystem the {@code DriveSubsystem} to use
+	 * @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
+	 * @param targetPoseSupplier a {@code Supplier<Pose2d>} that provides the
+	 *        {@code Pose2d} of the target.
+	 *        This is used at the commencement of the {@code DriveCommand} (i.e.,
+	 *        when the scheduler begins to periodically execute the
+	 *        {@code DriveCommand})
+	 * @param distanceTolerance the distance error in meters which is tolerable
+	 * @param angleTolerance the angle error in degrees which is tolerable
+	 * @return a {@code Commmand} for moving the robot to the specified
+	 *         target
+	 */
+	public static DriveCommand moveTo(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubsystem,
+			Supplier<Pose2d> targetPoseSupplier, double distanceTolerance, double angleTolerance) {
+		return new DriveCommand(driveSubsystem, () -> poseEstimationSubsystem.getEstimatedPose(),
+				targetPoseSupplier,
 				distanceTolerance, angleTolerance);
 	}
 
+	/**
+	 * Constructs a {@code DriveCommand} for turning the robot to the specified
+	 * target.
+	 * 
+	 * @param driveSubsystem the {@code DriveSubsystem} to use
+	 * @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
+	 * @param targetPose the {@code Pose2d} of the target
+	 * @param distanceTolerance the distance error in meters which is tolerable
+	 * @param angleTolerance the angle error in degrees which is tolerable
+	 * @return a {@code Commmand} for turning the robot to the specified
+	 *         target
+	 */
+	public static DriveCommand turnTo(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubsystem,
+			Pose2d targetPose, double distanceTolerance, double angleTolerance) {
+		return turnTo(driveSubsystem, poseEstimationSubsystem, () -> targetPose, distanceTolerance, angleTolerance);
+	}
+
+	/**
+	 * Constructs a {@code DriveCommand} for turning the robot to the specified
+	 * target.
+	 * 
+	 * @param driveSubsystem the {@code DriveSubsystem} to use
+	 * @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
+	 * @param targetPoseSupplier a {@code Supplier<Pose2d>} that provides the
+	 *        {@code Pose2d} of the target.
+	 *        This is used at the commencement of the {@code DriveCommand} (i.e.,
+	 *        when the scheduler begins to periodically execute the
+	 *        {@code DriveCommand})
+	 * @param distanceTolerance the distance error in meters which is tolerable
+	 * @param angleTolerance the angle error in degrees which is tolerable
+	 * @return a {@code Commmand} for turning the robot to the specified
+	 *         target
+	 */
+	public static DriveCommand turnTo(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubsystem,
+			Supplier<Pose2d> targetPoseSupplier, double distanceTolerance, double angleTolerance) {
+		return new DriveCommand(driveSubsystem, () -> poseEstimationSubsystem.getEstimatedPose(),
+				() -> {
+					var transform = new Transform2d(0, 0,
+							poseEstimationSubsystem.angularDisplacement(targetPoseSupplier.get()));
+					return poseEstimationSubsystem.getEstimatedPose().plus(transform);
+				},
+				distanceTolerance, angleTolerance);
+	}
+
+	/**
+	 * Constructs a {@code DriveCommand} for moving the robot toward the specified
+	 * target position while ensuring that the robot is away from the target by the
+	 * specified distance.
+	 * 
+	 * @param driveSubsystem the {@code DriveSubsystem} to use
+	 * @param poseEstimaitonSubsystem the {@code PoseEstimationSubsystem} to use
+	 * @param targetPosition a {@code Translation2d} (i.e., the position) of the
+	 *        target.
+	 * @param distanceToTarget the desired distance between the robot and the
+	 *        target position
+	 * @param distanceTolerance the distance error in meters which is tolerable
+	 * @param angleTolerance the angle error in degrees which is tolerable
+	 * @return a {@code Commmand} for turning the robot to the specified target
+	 *         position
+	 */
+	public static DriveCommand moveToward(DriveSubsystem driveSubsystem,
+			PoseEstimationSubsystem poseEstimaitonSubsystem, Translation2d targetPosition, double distanceToTarget,
+			double distanceTolerance, double angleTolerance) {
+		return moveToward(
+				driveSubsystem, poseEstimaitonSubsystem, () -> targetPosition, distanceToTarget, distanceTolerance,
+				angleTolerance);
+	}
+
 	/**
 	 * Constructs a {@code DriveCommand} for moving the robot toward the specified
 	 * target position while ensuring that the robot is away from the target by the
@@ -63,20 +145,20 @@ public static DriveCommand moveTo(DriveSubsystem driveSubsystem,
 	 *         position
 	 */
 	public static DriveCommand moveToward(DriveSubsystem driveSubsystem,
-			PoseEstimationSubsystem poseEstimaitonSubsystem,
-			Supplier<Translation2d> targetPositionSupplier,
-			double distanceToTarget,
-			double distanceTolerance,
-			double angleTolerance) {
-		Supplier<Pose2d> poseSupplier = () -> poseEstimaitonSubsystem.getPose();
+			PoseEstimationSubsystem poseEstimaitonSubsystem, Supplier<Translation2d> targetPositionSupplier,
+			double distanceToTarget, double distanceTolerance, double angleTolerance) {
+		Supplier<Pose2d> poseSupplier = () -> poseEstimaitonSubsystem.getEstimatedPose();
 		return new DriveCommand(driveSubsystem, poseSupplier,
-				() -> poseSupplier.get()
-						.plus(transformationToward(targetPositionSupplier.get(), poseSupplier.get(), distanceToTarget)),
+				() -> {
+					var transform = poseEstimaitonSubsystem
+							.transformationToward(targetPositionSupplier.get(), distanceToTarget);
+					return poseSupplier.get().plus(transform);
+				},
 				distanceTolerance, angleTolerance);
 	}
 
 	/**
-	 * Constructs a {@code DriveCommand} for moving the robot to the specified
+	 * Constructs a {@code DriveCommand} for turning the robot to the specified
 	 * target.
 	 * 
 	 * @param driveSubsystem the {@code DriveSubsystem} to use
@@ -85,112 +167,46 @@ public static DriveCommand moveToward(DriveSubsystem driveSubsystem,
 	 *        which {@code AprilTag}s are considered (maximum: 180)
 	 * @param distanceThresholdInMeters the maximum distance (in meters) within
 	 *        which {@code AprilTag}s are considered
-	 * @param robotToTarget the {@code Pose2d} of the target relative to the
-	 *        {@code Pose2d} of the robot for the {@code DriveCommand} to finish
 	 * @param distanceTolerance the distance error in meters which is tolerable
 	 * @param angleTolerance the angle error in degrees which is tolerable
-	 * @return a {@code Commmand} for moving the robot to the specified
+	 * @return a {@code Commmand} for turning the robot to the specified
 	 *         target
 	 */
-	public static DriveCommand moveToClosestTag(DriveSubsystem driveSubsystem,
+	public static DriveCommand turnToClosestTag(DriveSubsystem driveSubsystem,
 			PoseEstimationSubsystem poseEstimationSubsystem, double angleOfCoverageInDegrees,
-			double distanceThresholdInMeters,
-			Transform2d robotToTarget, double distanceTolerance, double angleTolerance) {
-		return new DriveCommand(driveSubsystem, () -> poseEstimationSubsystem.getPose(),
-				() -> kFieldLayout
-						.getTagPose(
-								closestTagID(
-										poseEstimationSubsystem.getPose(), angleOfCoverageInDegrees,
-										distanceThresholdInMeters))
-						.get()
-						.toPose2d()
-						.transformBy(robotToTarget),
-				distanceTolerance, angleTolerance);
-	}
-
-	/**
-	 * Returns the transformation needed for the robot to face toward the specified
-	 * target position and remain the specified distance away fron the target
-	 * position.
-	 * 
-	 * @param targetPosition the target position whose x and y-coordinate values
-	 *        are in meters
-	 * @param distanceToTarget the desired distance in meters to the target
-	 * @return the transformation needed for the robot to face toward the specified
-	 *         target position and remain the specified distance away fron the
-	 *         target position; {@code null} if it has not been
-	 *         possible to reliably estimate the pose of the robot
-	 */
-	public static Transform2d transformationToward(Translation2d targetPosition, Pose2d currentPose,
-			double distanceToTarget) {
-		Translation2d diff = targetPosition.minus(currentPose.getTranslation());
-		if (diff.getNorm() == 0)
-			return null;
-		var targetPose = new Pose2d(
-				currentPose.getTranslation().plus(diff.times(1 - distanceToTarget / diff.getNorm())),
-				diff.getAngle());
-		return targetPose.minus(currentPose);
+			double distanceThresholdInMeters, double distanceTolerance, double angleTolerance) {
+		return turnTo(
+				driveSubsystem, poseEstimationSubsystem,
+				() -> poseEstimationSubsystem.closestTagPose(angleOfCoverageInDegrees, distanceThresholdInMeters),
+				distanceThresholdInMeters, angleTolerance);
 	}
 
 	/**
-	 * Determines the ID of the {@code AprilTag} that is closest to the specified
-	 * {@code Pose2d} ({@code null} if no such {@code AprilTag}).
+	 * Constructs a {@code DriveCommand} for moving the robot to the specified
+	 * target.
 	 * 
-	 * @param pose a {@code Pose2d}
+	 * @param driveSubsystem the {@code DriveSubsystem} to use
+	 * @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
 	 * @param angleOfCoverageInDegrees the angular coverage (in degrees) within
 	 *        which {@code AprilTag}s are considered (maximum: 180)
 	 * @param distanceThresholdInMeters the maximum distance (in meters) within
 	 *        which {@code AprilTag}s are considered
-	 * @return the ID of the {@code AprilTag} that is closest to the specified
-	 *         {@code Pose2d} ({@code null} if no such {@code AprilTag})
-	 */
-	public static Integer closestTagID(Pose2d pose, double angleOfCoverageInDegrees, double distanceThresholdInMeters) {
-		var s = kFieldLayout.getTags().stream()
-				// consider only the tags facing toward the robot
-				.filter(
-						t -> Math.abs(
-								t.pose.getTranslation().toTranslation2d().minus(pose.getTranslation()).getAngle()
-										.minus(t.pose.toPose2d().getRotation()).getDegrees()) > 90)
-				.filter( // consider only the tags within the angle of coverage
-						t -> Math.abs(
-								angularDisplacement(pose, t.pose.toPose2d()).getDegrees()) < angleOfCoverageInDegrees)
-				.map(t -> Map.entry(t.ID, Math.abs(translationalDisplacement(pose, t.pose.toPose2d())))) // distance
-				.filter(t -> t.getValue() < distanceThresholdInMeters); // only tags sufficently close
-		Optional<Entry<Integer, Double>> closest = s.reduce((e1, e2) -> e1.getValue() < e2.getValue() ? e1 : e2);
-		if (closest.isPresent()) {
-			return closest.get().getKey();
-		} else
-			return null;
-	}
-
-	/**
-	 * Calculates the angular displacement given the initial and last
-	 * {@code Pose2d}s.
-	 * 
-	 * @param initial the initial {@code Pose2d}
-	 * @param last the last {@code Pose2d}
-	 * @return the angular displacement given the initial and last
-	 *         {@code Pose2d}s
-	 */
-	public static Rotation2d angularDisplacement(Pose2d initial, Pose2d last) {
-		var t = last.getTranslation().minus(initial.getTranslation());
-		return t.getAngle().minus(initial.getRotation());
-	}
-
-	/**
-	 * Calculates the translational displacement given the initial and last
-	 * {@code Pose2d}s.
-	 * 
-	 * @param initial the initial {@code Pose2d}
-	 * @param last the last {@code Pose2d}
-	 * @return the translational displacement given the initial and last
-	 *         {@code Pose2d}s
+	 * @param robotToTarget the {@code Pose2d} of the target relative to the
+	 *        {@code Pose2d} of the robot for the {@code DriveCommand} to finish
+	 * @param distanceTolerance the distance error in meters which is tolerable
+	 * @param angleTolerance the angle error in degrees which is tolerable
+	 * @return a {@code Commmand} for moving the robot to the specified
+	 *         target
 	 */
-	public static double translationalDisplacement(Pose2d initial, Pose2d last) {
-		var t = last.getTranslation().minus(initial.getTranslation());
-		return Math.abs(t.getAngle().minus(initial.getRotation()).getDegrees()) > 90
-				? -t.getNorm()
-				: t.getNorm();
+	public static DriveCommand moveToClosestTag(DriveSubsystem driveSubsystem,
+			PoseEstimationSubsystem poseEstimationSubsystem, double angleOfCoverageInDegrees,
+			double distanceThresholdInMeters, Transform2d robotToTarget, double distanceTolerance,
+			double angleTolerance) {
+		return moveTo(
+				driveSubsystem, poseEstimationSubsystem,
+				() -> poseEstimationSubsystem.closestTagPose(angleOfCoverageInDegrees, distanceThresholdInMeters)
+						.plus(robotToTarget),
+				distanceTolerance, angleTolerance);
 	}
 
 }