added reset() in PathDriveCommand

team20 · Feb 14, 2025 · 4524ffa · 4524ffa
1 parent eb7c83d
commit 4524ffa
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/CommandComposer.java b/src/main/java/frc/robot/CommandComposer.java
@@ -155,7 +155,7 @@ distanceTolerance, angleTolerance, pose(0.0, 0, 0)).withTimeout(1),
 	 * 
 	 * @return a {@code Command} for moving the robot on a circle
 	 */
-	public static Command moveOnEllipse(double radius, double initialAngularIncrement, double finalAngularIncrement,
+	public static Command moveOnOval(double radius, double initialAngularIncrement, double finalAngularIncrement,
 			double distanceTolerance, double angleTolerance, double intermediateToleranceRatio, int poseCount) {
 		Rotation2d angle = Rotation2d.kZero;
 		var l = new LinkedList<Pose2d>();

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -74,8 +74,8 @@ public Robot() {
 						CommandComposer.moveForwardBackward2Controllers(6, distanceTolerance, angleToleranceInDegrees));
 		m_testSelector
 				.addOption(
-						"Check PID Constants for Driving (Ellipe with Max Radius of 1m)",
-						CommandComposer.moveOnEllipse(
+						"Check PID Constants for Driving (Oval with Max Radius of 1m)",
+						CommandComposer.moveOnOval(
 								1, 8, 16, distanceTolerance, angleToleranceInDegrees, 16, 360 / 12 * 3 / 2));
 		m_testSelector
 				.addOption(

diff --git a/src/main/java/frc/robot/commands/PathDriveCommand.java b/src/main/java/frc/robot/commands/PathDriveCommand.java
@@ -70,6 +70,7 @@ public PathDriveCommand(DriveSubsystem driveSubsystem,
 	 */
 	@Override
 	public void initialize() {
+		targetPoseSupplier().reset();
 		moveToNextTargetPose();
 		Pose2d pose = m_driveSubsystem.getPose();
 		m_controllerXY.reset(m_targetPose.minus(pose).getTranslation().getNorm());
@@ -169,6 +170,13 @@ public IterativeTargetPoseSupplier(List<Supplier<Pose2d>> targetPoseSuppliers) {
 			m_targetPoseSuppliers = targetPoseSuppliers;
 		}
 
+		/**
+		 * Resets this {@code IterativeTargetPoseSupplier}.
+		 */
+		public void reset() {
+			m_targetPoseIndex = 0;
+		}
+
 		/**
 		 * Returns the curent target {@code Pose2d}.
 		 *