removed unnecessary test cases.

Author: jhhbrown1

src/main/java/frc/robot/CommandComposer.java

@@ -3,18 +3,17 @@
 import static edu.wpi.first.math.util.Units.*;
 import static edu.wpi.first.wpilibj2.command.Commands.*;
 import static frc.robot.Constants.*;
-import static frc.robot.Constants.DriveConstants.*;
 import static frc.robot.subsystems.PoseEstimationSubsystem.*;
 
 import java.util.LinkedList;
 import java.util.List;
 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.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.DriveCommand;
 import frc.robot.commands.PathDriveCommand;
 import frc.robot.subsystems.DriveSubsystem;
@@ -40,38 +39,6 @@ public static Command testAllSubsystems() {
 				m_driveSubsystem.testCommand());
 	}
 
-	/**
-	 * Returns a {@code Command} for testing the rotation capability of the robot.
-	 * 
-	 * @return a {@code Command} for testing the rotation capability of the robot
-	 */
-	public static Command testRotation() {
-		double rotionalSpeed = kTurnMaxAngularSpeed * 0.9;
-		double duration = 2.0;
-		return sequence(
-				m_driveSubsystem.run(() -> m_driveSubsystem.setModuleAngles(0)).withTimeout(0.1),
-				m_driveSubsystem.run(() -> m_driveSubsystem.drive(.5, 0, rotionalSpeed, true))
-						.withTimeout(duration),
-				m_driveSubsystem.run(() -> m_driveSubsystem.drive(-.5, 0, -rotionalSpeed, true))
-						.withTimeout(duration),
-				m_driveSubsystem.run(() -> m_driveSubsystem.drive(0.05, 0, 0, false))
-						.withTimeout(duration),
-				m_driveSubsystem.run(() -> m_driveSubsystem.drive(0, 0, rotionalSpeed, false))
-						.withTimeout(duration),
-				m_driveSubsystem.run(() -> m_driveSubsystem.drive(0.05, 0, 0, false))
-						.withTimeout(duration),
-				m_driveSubsystem.run(() -> m_driveSubsystem.drive(0, 0, -rotionalSpeed, false))
-						.withTimeout(duration),
-				m_driveSubsystem.run(() -> m_driveSubsystem.drive(0.05, 0, 0, false))
-						.withTimeout(duration),
-				m_driveSubsystem.run(() -> m_driveSubsystem.drive(0, 0, rotionalSpeed, false))
-						.withTimeout(duration),
-				m_driveSubsystem.run(() -> m_driveSubsystem.drive(0.05, 0, 0, false))
-						.withTimeout(duration),
-				m_driveSubsystem.run(() -> m_driveSubsystem.drive(0, 0, -rotionalSpeed, false))
-						.withTimeout(duration));
-	}
-
 	/**
 	 * Returns a {@code Command} for moving forward and then backward.
 	 * 
@@ -81,7 +48,7 @@ public static Command testRotation() {
 	 * 
 	 * @return a {@code Command} for moving forward and then backward.
 	 */
-	public static Command moveForwardBackward2Controllers(double distanceInFeet, double distanceTolerance,
+	public static Command moveForwardBackward(double distanceInFeet, double distanceTolerance,
 			double angleTolerance) {
 		return sequence(
 				new DriveCommand(m_driveSubsystem, distanceTolerance, angleTolerance, pose(0.0, 0, 0)),
@@ -119,58 +86,6 @@ distanceTolerance, angleTolerance, pose(0.0, 0, 0)).withTimeout(1),
 				new DriveCommand(m_driveSubsystem, distanceTolerance, angleTolerance, pose(0.0, 0, 0)));
 	}
 
-	/**
-	 * Returns a {@code Command} for moving the robot on a circle.
-	 * 
-	 * @param radius the radius of the circle in meters
-	 * @param initialAngularIncrement the initial angular increment in degrees which
-	 *        describes how quickly the robot to move on the circle
-	 * @param finalAngularIncrement the final angular increment in degrees which
-	 *        describes how quickly the robot to move on the circle
-	 * @param distanceTolerance the distance error in meters which is tolerable
-	 * @param angleTolerance the angle error in degrees which is tolerable
-	 * @param intermediateDistanceTolerance the distance error in meters which is
-	 *        tolerable for intermeidate target {@code Pose2d}s
-	 * @param intermediateAngleToleranceInDegrees the angle error in degrees which
-	 *        is tolerable for intermeidate target {@code Pose2d}s
-	 * @param poseCount the number of {@code Pose2d}s on the circle
-	 * 
-	 * @return a {@code Command} for moving the robot on a circle
-	 */
-	public static Command moveOnOval(double radius, double initialAngularIncrement, double finalAngularIncrement,
-			double distanceTolerance, double angleTolerance, double intermediateDistanceTolerance,
-			double intermediateAngleToleranceInDegrees, int poseCount) {
-		Rotation2d angle = Rotation2d.kZero;
-		var l = new LinkedList<Pose2d>();
-		for (double i = 0; i < poseCount; i++) {
-			var t = translation(radius, 0).rotateBy(angle);
-			l.add(new Pose2d(translation(t.getX() + radius, t.getY() / 2), angle));
-			double progress = i / poseCount;
-			double angularVelocity = progress * finalAngularIncrement + (1 - progress) * initialAngularIncrement;
-			angle = angle.plus(rotation(angularVelocity));
-		}
-		return new PathDriveCommand(m_driveSubsystem, distanceTolerance, angleTolerance, intermediateDistanceTolerance,
-				intermediateAngleToleranceInDegrees, l.toArray(new Pose2d[0]));
-	}
-
-	/**
-	 * Returns a {@code Command} for turning the robot toward the specified
-	 * {@code AprilTag}.
-	 * 
-	 * @param tagID the ID of the {@code AprilTag}
-	 * @return a {@code Command} for turning the robot toward the specified
-	 *         {@code AprilTag}
-	 */
-	public static Command turnTowardTag(int tagID) {
-		return m_driveSubsystem.run(() -> {
-			Rotation2d a = m_poseEstimationSubsystem.angularDisplacement(1);
-			if (a != null)
-				m_driveSubsystem.drive(
-						0, 0, a.getRadians() * kTurnP,
-						false);
-		}).withTimeout(1);
-	}
-
 	/**
 	 * Returns a {@code Command} for aligning the robot to the specified
 	 * {@code AprilTag}s.
@@ -209,7 +124,7 @@ public static Command alignToTags(double distanceTolerance, double angleToleranc
 					intermediateDistanceTolerance, intermediateAngleToleranceInDegrees, l.stream()
 							.map(p -> (Supplier<Pose2d>) (() -> m_poseEstimationSubsystem.odometryCentricPose(p)))
 							.toList());
-			commands.add(command);
+			commands.add(command.andThen(new WaitCommand(.5)));
 		}
 		return sequence(commands.toArray(new Command[0]));
 	}

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

@@ -72,16 +72,8 @@ public Robot() {
 		double intermediateDistanceTolerance = 0.16;
 		double intermediateAngleToleranceInDegrees = 16.0;
 
-		m_testSelector
-				.addOption(
-						"Check kDriveGearRatio and kWheelDiameter (F/B 6 feet)",
-						CommandComposer.moveForwardBackward2Controllers(6, distanceTolerance, angleToleranceInDegrees));
-		m_testSelector
-				.addOption(
-						"Check PID Constants for Driving (Oval with Max Radius of 1m)",
-						CommandComposer.moveOnOval(
-								1, 8, 16, distanceTolerance, angleToleranceInDegrees, intermediateDistanceTolerance,
-								intermediateAngleToleranceInDegrees, 360 / 12 * 3 / 2));
+		m_testSelector.addOption("Check All Subsystems in Pitt", CommandComposer.testAllSubsystems());
+		m_testSelector.addOption("Check All Subsystems on Field", CommandComposer.testAllSubsystems());
 		m_testSelector
 				.addOption(
 						"Quickly Align to AprilTags 1, 2, 6, 7, and 8",
@@ -91,18 +83,18 @@ public Robot() {
 								List.of(transform(1.5, 0, 180), transform(1.0, 0, 180), transform(.5, 0, 180)),
 								transform(1.5, 0, 180), 7, 6, 1,
 								6, 7, 8, 2, 8, 7));
-		m_testSelector.addOption("Check All Subsystems in Pitt", CommandComposer.testAllSubsystems());
-		m_testSelector.addOption("Check All Subsystems on Field", CommandComposer.testAllSubsystems());
 		m_testSelector
 				.addOption(
-						"Check DriveSubsystem (F/B/L/R/LR/RR and F/B while rotating)", m_driveSubsystem.testCommand());
+						"Check kDriveGearRatio and kWheelDiameter (F/B 6 feet)",
+						CommandComposer.moveForwardBackward(6, distanceTolerance, angleToleranceInDegrees));
 		m_testSelector
 				.addOption(
 						"Check PID Constants for Driving (5'x5' Square)",
 						CommandComposer
 								.moveOnSquare(Units.feetToMeters(5), distanceTolerance, angleToleranceInDegrees, 16));
-		m_testSelector.addOption("Test Rotation", CommandComposer.testRotation());
-		m_testSelector.addOption("Turn toward Tag 1", CommandComposer.turnTowardTag(1));
+		m_testSelector
+				.addOption(
+						"Check DriveSubsystem (F/B/L/R/LR/RR and F/B while rotating)", m_driveSubsystem.testCommand());
 
 		SmartDashboard.putData("Test Selector", m_testSelector);