introduced another alternative drive command.

Author: jhhbrown1

src/main/java/frc/robot/Constants.java

@@ -89,7 +89,7 @@ public static final class DriveConstants {
 		public static final double kDriveI = 0;
 		public static final double kDriveD = 0;
 		public static final double kDriveMaxVelocity = 3; // up to 5?
-		public static final double kDriveMaxAcceleration = 3; // up to 10?
+		public static final double kDriveMaxAcceleration = 6; // up to 10?
 
 		public static final double kTurnP = 0.2; // was 0.005 upto 0.2?
 		// public static final double kTurnP = 0.02; // was 0.005 upto 0.2?

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

@@ -5,6 +5,7 @@
 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;
 
@@ -14,33 +15,36 @@
  * @author Jeong-Hyon Hwang (jhhbrown@gmail.com)
  * @author Andrew Hwang (u.andrew.h@gmail.com)
  */
-public class AlignCommand extends DriveCommand {
+public class AlignCommand {
 
 	/**
-	 * Constructs a new {@code DriveCommand} whose purpose is to move the
+	 * Constructs a new {@code Command} 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
+	 *        {@code Command} (i.e., when the scheduler
 	 *        begins to periodically execute this
-	 *        {@code DriveCommand})
+	 *        {@code Command})
 	 * @param distanceTolerance the distance error in meters which is tolerable
 	 * @param angleTolerance the angle error in degrees which is tolerable
+	 * @return a new {@code Command} whose purpose is to move the
+	 *         robot to a certain target pose
 	 */
-	public AlignCommand(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubsystem,
+	public static Command driveCommand(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubsystem,
 			Supplier<Pose2d> targetPoseSupplier,
 			double distanceTolerance,
 			double angleTolerance) {
-		super(driveSubsystem, () -> poseEstimationSubsystem.getEstimatedPose(), targetPoseSupplier, distanceTolerance,
+		return new DriveCommand(driveSubsystem, () -> poseEstimationSubsystem.getEstimatedPose(), targetPoseSupplier,
+				distanceTolerance,
 				angleTolerance);
 	}
 
 	/**
-	 * Constructs a {@code DriveCommand} for moving the robot to the specified
+	 * Constructs a {@code Command} for moving the robot to the specified
 	 * target.
 	 * 
 	 * @param driveSubsystem the {@code DriveSubsystem} to use
@@ -51,35 +55,38 @@ public AlignCommand(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseE
 	 * @return a {@code Commmand} for moving the robot to the specified
 	 *         target
 	 */
-	public static DriveCommand moveTo(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubsystem,
+	public static Command moveTo(DriveSubsystem driveSubsystem,
+			PoseEstimationSubsystem poseEstimationSubsystem,
 			Pose2d targetPose, double distanceTolerance, double angleTolerance) {
 		return moveTo(driveSubsystem, poseEstimationSubsystem, () -> targetPose, distanceTolerance, angleTolerance);
 	}
 
 	/**
-	 * Constructs a {@code DriveCommand} for moving the robot to the specified
+	 * Constructs a {@code Command} 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.,
+	 *        This is used at the commencement of the {@code Command} (i.e.,
 	 *        when the scheduler begins to periodically execute the
-	 *        {@code DriveCommand})
+	 *        {@code Command})
 	 * @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,
+	public static Command moveTo(DriveSubsystem driveSubsystem,
+			PoseEstimationSubsystem poseEstimationSubsystem,
 			Supplier<Pose2d> targetPoseSupplier, double distanceTolerance, double angleTolerance) {
-		return new AlignCommand(driveSubsystem, poseEstimationSubsystem, targetPoseSupplier,
+		return driveCommand(
+				driveSubsystem, poseEstimationSubsystem, targetPoseSupplier,
 				distanceTolerance, angleTolerance);
 	}
 
 	/**
-	 * Constructs a {@code DriveCommand} for turning the robot to the specified
+	 * Constructs a {@code Command} for turning the robot to the specified
 	 * target.
 	 * 
 	 * @param driveSubsystem the {@code DriveSubsystem} to use
@@ -90,30 +97,33 @@ public static DriveCommand moveTo(DriveSubsystem driveSubsystem, PoseEstimationS
 	 * @return a {@code Commmand} for turning the robot to the specified
 	 *         target
 	 */
-	public static DriveCommand turnTo(DriveSubsystem driveSubsystem, PoseEstimationSubsystem poseEstimationSubsystem,
+	public static Command 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
+	 * Constructs a {@code Command} 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.,
+	 *        This is used at the commencement of the {@code Command} (i.e.,
 	 *        when the scheduler begins to periodically execute the
-	 *        {@code DriveCommand})
+	 *        {@code Command})
 	 * @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,
+	public static Command turnTo(DriveSubsystem driveSubsystem,
+			PoseEstimationSubsystem poseEstimationSubsystem,
 			Supplier<Pose2d> targetPoseSupplier, double distanceTolerance, double angleTolerance) {
-		return new AlignCommand(driveSubsystem, poseEstimationSubsystem,
+		return driveCommand(
+				driveSubsystem, poseEstimationSubsystem,
 				() -> {
 					var transform = new Transform2d(0, 0,
 							poseEstimationSubsystem.angularDisplacement(targetPoseSupplier.get()));
@@ -123,7 +133,7 @@ public static DriveCommand turnTo(DriveSubsystem driveSubsystem, PoseEstimationS
 	}
 
 	/**
-	 * Constructs a {@code DriveCommand} for moving the robot toward the specified
+	 * Constructs a {@code Command} for moving the robot toward the specified
 	 * target position while ensuring that the robot is away from the target by the
 	 * specified distance.
 	 * 
@@ -138,7 +148,7 @@ public static DriveCommand turnTo(DriveSubsystem driveSubsystem, PoseEstimationS
 	 * @return a {@code Commmand} for turning the robot to the specified target
 	 *         position
 	 */
-	public static DriveCommand moveToward(DriveSubsystem driveSubsystem,
+	public static Command moveToward(DriveSubsystem driveSubsystem,
 			PoseEstimationSubsystem poseEstimationSubsystem, Translation2d targetPosition, double distanceToTarget,
 			double distanceTolerance, double angleTolerance) {
 		return moveToward(
@@ -147,28 +157,29 @@ public static DriveCommand moveToward(DriveSubsystem driveSubsystem,
 	}
 
 	/**
-	 * Constructs a {@code DriveCommand} for moving the robot toward the specified
+	 * Constructs a {@code Command} 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 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.,
+	 *        This is used at the commencement of the {@code Command} (i.e.,
 	 *        when the scheduler begins to periodically execute the
-	 *        {@code DriveCommand})
+	 *        {@code Command})
 	 * @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,
+	public static Command moveToward(DriveSubsystem driveSubsystem,
 			PoseEstimationSubsystem poseEstimationSubsystem, Supplier<Translation2d> targetPositionSupplier,
 			double distanceToTarget, double distanceTolerance, double angleTolerance) {
-		return new AlignCommand(driveSubsystem, poseEstimationSubsystem,
+		return driveCommand(
+				driveSubsystem, poseEstimationSubsystem,
 				() -> {
 					var transform = poseEstimationSubsystem
 							.transformationToward(targetPositionSupplier.get(), distanceToTarget);
@@ -178,7 +189,7 @@ public static DriveCommand moveToward(DriveSubsystem driveSubsystem,
 	}
 
 	/**
-	 * Constructs a {@code DriveCommand} for turning the robot to the specified
+	 * Constructs a {@code Command} for turning the robot to the specified
 	 * target.
 	 * 
 	 * @param driveSubsystem the {@code DriveSubsystem} to use
@@ -192,7 +203,7 @@ public static DriveCommand moveToward(DriveSubsystem driveSubsystem,
 	 * @return a {@code Commmand} for turning the robot to the specified
 	 *         target
 	 */
-	public static DriveCommand turnToClosestTag(DriveSubsystem driveSubsystem,
+	public static Command turnToClosestTag(DriveSubsystem driveSubsystem,
 			PoseEstimationSubsystem poseEstimationSubsystem, double angleOfCoverageInDegrees,
 			double distanceThresholdInMeters, double distanceTolerance, double angleTolerance) {
 		return turnTo(
@@ -202,7 +213,7 @@ public static DriveCommand turnToClosestTag(DriveSubsystem driveSubsystem,
 	}
 
 	/**
-	 * Constructs a {@code DriveCommand} for moving the robot to the specified
+	 * Constructs a {@code Command} for moving the robot to the specified
 	 * target.
 	 * 
 	 * @param driveSubsystem the {@code DriveSubsystem} to use
@@ -212,13 +223,13 @@ public static DriveCommand turnToClosestTag(DriveSubsystem driveSubsystem,
 	 * @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
+	 *        {@code Pose2d} of the robot for the {@code Command} 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 DriveCommand moveToClosestTag(DriveSubsystem driveSubsystem,
+	public static Command moveToClosestTag(DriveSubsystem driveSubsystem,
 			PoseEstimationSubsystem poseEstimationSubsystem, double angleOfCoverageInDegrees,
 			double distanceThresholdInMeters, Transform2d robotToTarget, double distanceTolerance,
 			double angleTolerance) {