made one more pass on commands

Author: jhhbrown1

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

@@ -48,6 +48,10 @@ public static Command testRotation() {
 		double duration = 2.0;
 		return sequence(
 				m_driveSubsystem.run(() -> m_driveSubsystem.setModuleAngles(0)).withTimeout(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, 0, rotionalSpeed, false))
 						.withTimeout(duration),
 				m_driveSubsystem.run(() -> m_driveSubsystem.setModuleAngles(0)).withTimeout(1),
@@ -106,17 +110,14 @@ public static Command moveForwardBackward3Controllers(double distanceInFeet, dou
 	 * 
 	 * @param distanceTolerance the distance error in meters which is tolerable
 	 * @param angleTolerance the angle error in degrees which is tolerable
-	 * @param refinements the number of refinments applied to the path
 	 * @param robotToTag the {@code Tranform2d} representing the pose of the
 	 *        {@code AprilTag} relative to the robot when the robot is aligned
 	 * @param tagIDs the IDs of the {@code AprilTag}s
 	 * @return a {@code Command} for visiting all the specified {@code AprilTag}s
 	 */
-	public static Command visitTags(double distanceTolerance, double angleTolerance, int refinements,
+	public static Command visitTags(double distanceTolerance, double angleTolerance,
 			Transform2d robotToTag, int... tagIDs) {
 		Pose2d[] poses = PoseEstimationSubsystem.tagPoses(tagIDs);
-		for (int i = 0; i < refinements; i++)
-			poses = PoseEstimationSubsystem.refine(poses);
 		var commands = Arrays.stream(poses).map(
 				p -> AlignCommand
 						.moveTo(
@@ -133,18 +134,15 @@ public static Command visitTags(double distanceTolerance, double angleTolerance,
 	 * @param angleTolerance the angle error in degrees which is tolerable
 	 * @param intermediateToleranceRatio the ratio of distance and angle tolerances
 	 *        at intermediate {@code AprilTags} for faster movements
-	 * @param refinements the number of refinments applied to the path
 	 * @param robotToTag the {@code Tranform2d} representing the pose of the
 	 *        {@code AprilTag} relative to the robot when the robot is aligned
 	 * @param tagIDs the IDs of the {@code AprilTag}s
 	 * @return a {@code Command} for visiting all the specified {@code AprilTag}s
 	 */
 	public static Command visitTagsOptimized(double distanceTolerance, double angleTolerance,
-			double intermediateToleranceRatio, int refinements, Transform2d robotToTag,
+			double intermediateToleranceRatio, Transform2d robotToTag,
 			int... tagIDs) {
 		var poses = PoseEstimationSubsystem.tagPoses(tagIDs);
-		for (int i = 0; i < refinements; i++)
-			poses = PoseEstimationSubsystem.refine(poses);
 		poses = Arrays.stream(poses).map(p -> p.plus(robotToTag)).toList().toArray(new Pose2d[0]);
 		return AlignCommand.follow(
 				m_driveSubsystem, m_poseEstimationSubsystem, distanceTolerance, angleTolerance,

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

@@ -68,36 +68,27 @@ public Robot() {
 				CommandComposer.moveForwardBackward3Controllers(6, 0.03, 3));
 		m_autoSelector.addOption(
 				"Move around the Red Reef",
-				CommandComposer.visitTags(0.03, 3, 0, transform(1.5, 0, 180), 11, 6, 7, 8, 9, 10, 11));
+				CommandComposer.visitTags(0.03, 3, transform(1.5, 0, 180), 11, 6, 7, 8, 9, 10, 11));
 		m_autoSelector.addOption(
 				"Move around the Blue Reef",
-				CommandComposer.visitTags(0.03, 3, 0, transform(1.5, 0, 180), 22, 17, 18, 19, 20, 21, 22));
-		m_autoSelector.addOption(
-				"Move around the Red Reef (Refined)",
-				CommandComposer.visitTags(0.03, 3, 2, transform(1.5, 0, 180), 11, 6, 7, 8, 9, 10, 11));
-		m_autoSelector.addOption(
-				"Move around the Blue Reef (Refined)",
-				CommandComposer.visitTags(0.03, 3, 2, transform(1.5, 0, 180), 22, 17, 18, 19, 20, 21, 22));
-		m_autoSelector.addOption(
-				"Move around the Red Reef (Faster)",
-				CommandComposer.visitTagsOptimized(0.03, 3, 5, 0, transform(1.5, 0, 180), 11, 6, 7, 8, 9, 10, 11));
-		m_autoSelector.addOption(
-				"Move around the Blue Reef (Faster)",
-				CommandComposer.visitTagsOptimized(0.03, 3, 5, 0, transform(1.5, 0, 180), 22, 17, 18, 19, 20, 21, 22));
-		m_autoSelector.addOption(
-				"Move around the Red Reef (Faster, Refined)",
-				CommandComposer.visitTagsOptimized(0.03, 3, 5, 2, transform(1.5, 0, 180), 11, 6, 7, 8, 9, 10, 11));
-		m_autoSelector.addOption(
-				"Move around the Blue Reef (Faster, Refined)",
-				CommandComposer.visitTagsOptimized(0.03, 3, 5, 2, transform(1.5, 0, 180), 22, 17, 18, 19, 20, 21, 22));
+				CommandComposer.visitTags(0.03, 3, transform(1.5, 0, 180), 22, 17, 18, 19, 20, 21, 22));
 		m_autoSelector.addOption(
 				"Move around the Red Reef (Complex)",
 				CommandComposer.visitTags(
-						0.03, 3, 20, transform(1.2, 0, 180), transform(0.5, 0, 180), 11, 6, 7, 8, 9, 10, 11));
+						0.03, 3, 10, transform(1.2, 0, 180), transform(0.5, 0, 180), 11, 6, 7, 8, 9, 10, 11));
 		m_autoSelector.addOption(
 				"Move around the Blue Reef (Complex)",
 				CommandComposer.visitTags(
-						0.03, 3, 20, transform(1.2, 0, 180), transform(0.5, 0, 180), 22, 17, 18, 19, 20, 21, 22));
+						0.03, 3, 10, transform(1.2, 0, 180), transform(0.5, 0, 180), 22, 17, 18, 19, 20, 21, 22));
+		m_autoSelector.addOption(
+				"Quickly Visit Red Team Tags",
+				CommandComposer
+						.visitTagsOptimized(0.03, 2, 5, transform(1.5, 0, 180), 11, 1, 6, 7, 8, 2, 8, 7, 6, 11));
+		m_autoSelector.addOption(
+				"Quickly Visit Blue Team Tags",
+				CommandComposer
+						.visitTagsOptimized(
+								0.03, 2, 5, transform(1.5, 0, 180), 22, 12, 17, 18, 19, 13, 19, 18, 17, 22));
 		SmartDashboard.putData(m_autoSelector);
 
 		SmartDashboard.putData(m_pdh);
@@ -187,7 +178,6 @@ public void disabledExit() {
 	@Override
 	public void autonomousInit() {
 		m_autonomousCommand = m_autoSelector.getSelected();
-
 		if (m_autonomousCommand != null) {
 			m_autonomousCommand.schedule();
 		}