made DriveCommand and AlignCommand more explanable

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

@@ -76,12 +76,10 @@ public void bindDriveControls() {
 								m_driveSubsystem, pose(0, 0, 0), distanceTolerance, angleTolerance));
 		m_driverController.button(Button.kX) // 1m away
 				.whileTrue(
-						new DriveCommand(
-								m_driveSubsystem, pose(1, 0, 0), distanceTolerance, angleTolerance));
+						DriveCommand.moveForward(m_driveSubsystem, 1, distanceTolerance, angleTolerance));
 		m_driverController.button(Button.kCircle) // 2m away
 				.whileTrue(
-						new DriveCommand(
-								m_driveSubsystem, pose(2, 0, 0), distanceTolerance, angleTolerance));
+						DriveCommand.moveForward(m_driveSubsystem, 2, distanceTolerance, angleTolerance));
 
 		double angleOfCoverageInDegrees = 90;
 		double distanceThresholdInMeters = 4;

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

@@ -5,7 +5,6 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.PoseEstimationSubsystem;
 
@@ -15,7 +14,30 @@
  * @author Jeong-Hyon Hwang ([email protected])
  * @author Andrew Hwang ([email protected])
  */
-public class AlignCommand extends Command {
+public class AlignCommand extends DriveCommand {
+
+	/**
+	 * Constructs a new {@code DriveCommand} whose purpose is to move the
+	 * robot to a certain target pose.
+	 * 
+	 * @param driveSubsystem the {@code DriveSubsystem} to use
+	 * @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
+	 * @param targetPoseSupplier a {@code Supplier<Pose2d>} that provides the
+	 *        target pose to which the robot should move.
+	 *        This is used at the commencement of this
+	 *        {@code DriveCommand} (i.e., when the scheduler
+	 *        begins to periodically execute this
+	 *        {@code DriveCommand})
+	 * @param distanceTolerance the distance error in meters which is tolerable
+	 * @param angleTolerance the angle error in degrees which is tolerable
+	 */
+	public AlignCommand(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubsystem,
+			Supplier<Pose2d> targetPoseSupplier,
+			double distanceTolerance,
+			double angleTolerance) {
+		super(driveSubsystem, () -> poseEstimationSubsystem.getEstimatedPose(), targetPoseSupplier, distanceTolerance,
+				angleTolerance);
+	}
 
 	/**
 	 * Constructs a {@code DriveCommand} for moving the robot to the specified
@@ -52,8 +74,7 @@ public static DriveCommand moveTo(DriveSubsystem driveSubsystem, PoseEstimationS
 	 */
 	public static DriveCommand moveTo(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubsystem,
 			Supplier<Pose2d> targetPoseSupplier, double distanceTolerance, double angleTolerance) {
-		return new DriveCommand(driveSubsystem, () -> poseEstimationSubsystem.getEstimatedPose(),
-				targetPoseSupplier,
+		return new AlignCommand(driveSubsystem, poseEstimationSubsystem, targetPoseSupplier,
 				distanceTolerance, angleTolerance);
 	}
 
@@ -92,7 +113,7 @@ public static DriveCommand turnTo(DriveSubsystem driveSubsystem, PoseEstimationS
 	 */
 	public static DriveCommand turnTo(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubsystem,
 			Supplier<Pose2d> targetPoseSupplier, double distanceTolerance, double angleTolerance) {
-		return new DriveCommand(driveSubsystem, () -> poseEstimationSubsystem.getEstimatedPose(),
+		return new AlignCommand(driveSubsystem, poseEstimationSubsystem,
 				() -> {
 					var transform = new Transform2d(0, 0,
 							poseEstimationSubsystem.angularDisplacement(targetPoseSupplier.get()));
@@ -107,7 +128,7 @@ public static DriveCommand turnTo(DriveSubsystem driveSubsystem, PoseEstimationS
 	 * specified distance.
 	 * 
 	 * @param driveSubsystem the {@code DriveSubsystem} to use
-	 * @param poseEstimaitonSubsystem the {@code PoseEstimationSubsystem} to use
+	 * @param poseEstimationSubsystem 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
@@ -118,10 +139,10 @@ public static DriveCommand turnTo(DriveSubsystem driveSubsystem, PoseEstimationS
 	 *         position
 	 */
 	public static DriveCommand moveToward(DriveSubsystem driveSubsystem,
-			PoseEstimationSubsystem poseEstimaitonSubsystem, Translation2d targetPosition, double distanceToTarget,
+			PoseEstimationSubsystem poseEstimationSubsystem, Translation2d targetPosition, double distanceToTarget,
 			double distanceTolerance, double angleTolerance) {
 		return moveToward(
-				driveSubsystem, poseEstimaitonSubsystem, () -> targetPosition, distanceToTarget, distanceTolerance,
+				driveSubsystem, poseEstimationSubsystem, () -> targetPosition, distanceToTarget, distanceTolerance,
 				angleTolerance);
 	}
 
@@ -131,7 +152,7 @@ public static DriveCommand moveToward(DriveSubsystem driveSubsystem,
 	 * specified distance.
 	 * 
 	 * @param driveSubsystem the {@code DriveSubsystem} to use
-	 * @param poseEstimaitonSubsystem the {@code PoseEstimationSubsystem} to use
+	 * @param poseEstimationSubsystem the {@code PoseEstimationSubsystem} to use
 	 * @param targetPositionSupplier a {@code Supplier<Pose2d>} that provides the
 	 *        target position.
 	 *        This is used at the commencement of the {@code DriveCommand} (i.e.,
@@ -145,14 +166,13 @@ public static DriveCommand moveToward(DriveSubsystem driveSubsystem,
 	 *         position
 	 */
 	public static DriveCommand moveToward(DriveSubsystem driveSubsystem,
-			PoseEstimationSubsystem poseEstimaitonSubsystem, Supplier<Translation2d> targetPositionSupplier,
+			PoseEstimationSubsystem poseEstimationSubsystem, Supplier<Translation2d> targetPositionSupplier,
 			double distanceToTarget, double distanceTolerance, double angleTolerance) {
-		Supplier<Pose2d> poseSupplier = () -> poseEstimaitonSubsystem.getEstimatedPose();
-		return new DriveCommand(driveSubsystem, poseSupplier,
+		return new AlignCommand(driveSubsystem, poseEstimationSubsystem,
 				() -> {
-					var transform = poseEstimaitonSubsystem
+					var transform = poseEstimationSubsystem
 							.transformationToward(targetPositionSupplier.get(), distanceToTarget);
-					return poseSupplier.get().plus(transform);
+					return poseEstimationSubsystem.getEstimatedPose().plus(transform);
 				},
 				distanceTolerance, angleTolerance);
 	}

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

@@ -7,6 +7,7 @@
 import edu.wpi.first.math.controller.ProfiledPIDController;
 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.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.DriveSubsystem;
@@ -68,12 +69,31 @@ public class DriveCommand extends Command {
 	 * @param distanceTolerance the distance error in meters which is tolerable
 	 * @param angleTolerance the angle error in degrees which is tolerable
 	 */
-	public DriveCommand(DriveSubsystem driveSubsystem, Pose2d targetPose,
-			double distanceTolerance,
+	public DriveCommand(DriveSubsystem driveSubsystem, Pose2d targetPose, double distanceTolerance,
 			double angleTolerance) {
 		this(driveSubsystem, () -> driveSubsystem.getPose(), () -> targetPose, distanceTolerance, angleTolerance);
 	}
 
+	/**
+	 * Constructs a {@code DriveCommand} for moving the robot forward/backward by
+	 * the specified displacement.
+	 * 
+	 * @param driveSubsystem the {@code DriveSubsystem} to use
+	 * @param translationalDisplacement the displacement by which the robot to move
+	 *        (positive: forward, negative: backward) in meters
+	 * @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 moveForward(DriveSubsystem driveSubsystem, double translationalDisplacement,
+			double distanceTolerance, double angleTolerance) {
+		return new DriveCommand(driveSubsystem, () -> driveSubsystem.getPose(), () -> {
+			var transform = new Transform2d(translationalDisplacement, 0, Rotation2d.kZero);
+			return driveSubsystem.getPose().plus(transform);
+		}, distanceTolerance, angleTolerance);
+	}
+
 	/**
 	 * Constructs a new {@code DriveCommand} whose purpose is to move the
 	 * robot to a certain target pose.

src/main/java/frc/robot/subsystems/PoseEstimationSubsystem.java

@@ -26,6 +26,10 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
+/**
+ * A {@code PoseEstimationSubsystem} continuously estimates the {@code Pose2d}
+ * of the robot on the field.
+ */
 public class PoseEstimationSubsystem extends SubsystemBase {
 
 	/**
@@ -80,7 +84,7 @@ public class PoseEstimationSubsystem extends SubsystemBase {
 	 * 
 	 * @param driveSubsystem {@code DriveSubsystem} to be used by the
 	 *        {@code PoseEstimationSubsystem}
-	 * @param camera the {@code PhotonCamera}s to be used by the
+	 * @param cameras the {@code PhotonCamera}s to be used by the
 	 *        {@code PoseEstimationSubsystem} (found in the PhotonVision UI)
 	 */
 	public PoseEstimationSubsystem(DriveSubsystem driveSubsystem, PhotonCamera... cameras) {
@@ -134,7 +138,6 @@ public Pose2d getEstimatedPose() {
 	 * Finds the {@code Pose2d} of the {@code AprilTag} that is closest to the robot
 	 * ({@code null} if no such {@code AprilTag}).
 	 * 
-	 * @param pose a {@code Pose2d}
 	 * @param angleOfCoverageInDegrees the angular coverage (in degrees) within
 	 *        which {@code AprilTag}s are considered (maximum: 180)
 	 * @param distanceThresholdInMeters the maximum distance (in meters) within
@@ -151,7 +154,6 @@ public Pose2d closestTagPose(double angleOfCoverageInDegrees, double distanceThr
 	 * Determines the ID of the {@code AprilTag} that is closest to the robot
 	 * ({@code null} if no such {@code AprilTag}).
 	 * 
-	 * @param pose a {@code Pose2d}
 	 * @param angleOfCoverageInDegrees the angular coverage (in degrees) within
 	 *        which {@code AprilTag}s are considered (maximum: 180)
 	 * @param distanceThresholdInMeters the maximum distance (in meters) within