done some commands fr

SciBorgs · Jan 28, 2025 · 69a01a2 · 69a01a2
1 parent 0e77fb8
commit 69a01a2
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 39 deletions.
diff --git a/src/main/java/org/sciborgs1155/robot/commands/Alignment.java b/src/main/java/org/sciborgs1155/robot/commands/Alignment.java
@@ -3,7 +3,6 @@
 import static org.sciborgs1155.robot.Constants.Field.*;
 import static org.sciborgs1155.robot.drive.DriveConstants.*;
 
-import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.commands.FollowPathCommand;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
@@ -54,9 +53,16 @@ public Alignment(Drive drive, Elevator elevator, Scoral scoral) {
    * @return A command to quickly prepare and then score in the reef.
    */
   public Command reef(Level level, Branch branch) {
-    return directPathfollow(branch.pose)
-        .alongWith(elevator.scoreLevel(level).until(() -> elevator.atPosition(level.height)))
-        .andThen(scoral.outtake().onlyWhile(scoral::beambreak));
+    return (directPathfollow(branch.pose)
+            .andThen(
+                Commands.waitUntil(() -> elevator.atPosition(level.height))
+                    .andThen(
+                        scoral
+                            .outtake()
+                            .until(scoral::beambreak)
+                            .withTimeout(
+                                1)))) // timeout needed because sim beambreak does not change value
+        .deadlineFor(elevator.scoreLevel(level));
   }
 
   /**
@@ -69,8 +75,11 @@ public Command reef(Level level, Branch branch) {
    */
   public Command safeReef(Level level, Branch branch) {
     return directPathfollow(branch.pose)
-        .andThen(elevator.scoreLevel(level).until(() -> elevator.atPosition(level.height)))
-        .andThen(scoral.outtake());
+        .andThen(elevator.scoreLevel(level))
+        .until(scoral::beambreak)
+        .withTimeout(1)
+        .deadlineFor(
+            Commands.waitUntil(() -> elevator.atPosition(level.height)).andThen(scoral.outtake()));
   }
 
   /**
@@ -90,18 +99,7 @@ public Command nearReef(Level level) {
    * @return A command to align with the human player station source.
    */
   public Command source() {
-    return directPathfollow(drive.pose().nearest(List.of(LEFT_SOURCE, RIGHT_SOURCE)))
-        .alongWith(elevator.retract());
-  }
-
-  public Command processor() {
-    return directPathfollow(PROCESSOR);
-    // TODO idk how this would work. research this a bit more
-  }
-
-  public Command cage() {
-    return directPathfollow(nearestCage(drive.pose()));
-    // TODO it should do more. when we have a plan, update this
+    return directPathfollow(drive.pose().nearest(List.of(LEFT_SOURCE, RIGHT_SOURCE)));
   }
 
   /**
@@ -126,19 +124,10 @@ public PathPlannerPath directPath(Pose2d goal) {
     return path;
   }
 
-  public Command pathfind(PathPlannerPath path) {
-    // PathPlannerPath path = directPathfind(goal);
-    // System.out.println(path.getPoint(1));
-    // System.out.println(path.generateTrajectory(drive.robotRelativeChassisSpeeds(),
-    // drive.heading(), ROBOT_CONFIG).getStates().size());
-    // System.out.println(path.getAllPathPoints().size() + "\n");
-    return AutoBuilder.pathfindThenFollowPath(path, PATH_CONSTRAINTS);
-  }
-
   /**
-   * Follows a given pathplanner path.
+   * Creates a pathplanner path to a goal, and then follows that path.
    *
-   * @param path A pathplanner path.
+   * @param goal The goal pose of the robot at the end of the path.
    * @return A command to follow a path.
    */
   public Command directPathfollow(Pose2d goal) {
@@ -157,7 +146,10 @@ public Command directPathfollow(Pose2d goal) {
                     () -> false,
                     drive)
                 .withInterruptBehavior(InterruptionBehavior.kCancelSelf)
-                .andThen(drive.driveTo(goal)),
+                .andThen(
+                    drive.driveTo(
+                        goal)), // pathplanner isnt precise enough so we gotta fix it ourselves
+        // (just adjusts a little bit)
         Set.of(drive));
   }
 }
diff --git a/src/test/java/org/sciborgs1155/robot/AlignTest.java b/src/test/java/org/sciborgs1155/robot/AlignTest.java
@@ -3,12 +3,9 @@
 import static edu.wpi.first.units.Units.Centimeters;
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.Radian;
-import static edu.wpi.first.units.Units.Seconds;
 import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-import static org.sciborgs1155.lib.UnitTestingUtil.fastForward;
 import static org.sciborgs1155.lib.UnitTestingUtil.reset;
-import static org.sciborgs1155.lib.UnitTestingUtil.run;
+import static org.sciborgs1155.lib.UnitTestingUtil.runToCompletion;
 import static org.sciborgs1155.lib.UnitTestingUtil.setupTests;
 import static org.sciborgs1155.robot.Constants.Field.Branch.*;
 import static org.sciborgs1155.robot.Constants.allianceReflect;
@@ -88,15 +85,13 @@ public void reflectionTest() throws Exception {
   @MethodSource("pathGoals")
   public void pathfindTest(Branch branch) {
     Command testcmd = align.reef(Level.L4, branch);
-    run(testcmd);
-    fastForward(Seconds.of(5));
-    assertTrue(() -> !testcmd.isScheduled());
+    runToCompletion(testcmd);
     assertEquals(branch.pose.getX(), drive.pose().getX(), Translation.TOLERANCE.in(Meters));
     assertEquals(branch.pose.getY(), drive.pose().getY(), Translation.TOLERANCE.in(Meters));
     assertEquals(
-        branch.pose.getRotation().minus(drive.pose().getRotation()).getRadians(),
         0,
-        Rotation.TOLERANCE.in(Radian) * 2);
+        branch.pose.getRotation().minus(drive.pose().getRotation()).getRadians(),
+        Rotation.TOLERANCE.in(Radian));
   }
 
   private static Stream<Arguments> pathGoals() {