resetting during follow

SciBorgs · Apr 8, 2023 · 9102514 · 9102514
1 parent 671ea20
commit 9102514
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 18 deletions.
diff --git a/simgui.json b/simgui.json
@@ -108,5 +108,8 @@
         "open": true
       }
     }
+  },
+  "NetworkTables View": {
+    "visible": false
   }
 }
diff --git a/src/main/java/org/sciborgs1155/robot/commands/Autos.java b/src/main/java/org/sciborgs1155/robot/commands/Autos.java
@@ -110,18 +110,8 @@ public Command twoGamepiece() {
   }
 
   public Command fullBalance() {
-    var trajectory = PathPlanner.loadPath("balance", Constants.Drive.CONSTRAINTS);
-    // var initialState =
-    //               PathPlannerTrajectory.transformStateForAlliance(
-    //                   trajectory.getInitialState(), DriverStation.getAlliance());
-    // Pose2d initialPose = new Pose2d(
-    //   initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
-
-    // return Commands.runOnce(() -> drive.resetOdometry(initialPose), drive)
-    //     .andThen(drive.follow(trajectory, false, true))
     return drive
-        .follow("balance", false, true)
-        // followAutoPath("balance")
+        .follow("balance", true, true)
         .andThen(drive.balance())
         .withTimeout(10)
         .andThen(Commands.print("balanced!"))
@@ -218,6 +208,11 @@ public Command scoreOneMeterTest() {
   }
 
   public Command defaultOdometryReset(GamePiece gamePiece, Rotation2d rotation) {
+    return defaultOdometryReset(gamePiece, rotation, startingPosChooser.getSelected());
+  }
+
+  public Command defaultOdometryReset(
+      GamePiece gamePiece, Rotation2d rotation, StartingPos startingPos) {
     return Commands.runOnce(
         () ->
             drive.resetOdometry(
@@ -227,7 +222,7 @@ public Command defaultOdometryReset(GamePiece gamePiece, Rotation2d rotation) {
                       case Red -> 14.67;
                       case Invalid -> -1; // should never happen!
                     },
-                    switch (startingPosChooser.getSelected()) {
+                    switch (startingPos) {
                       case SUBSTATION -> switch (gamePiece) {
                         case CONE -> 5.0;
                         case CUBE -> 4.42;

diff --git a/src/main/java/org/sciborgs1155/robot/subsystems/Drive.java b/src/main/java/org/sciborgs1155/robot/subsystems/Drive.java
@@ -20,9 +20,11 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import io.github.oblarg.oblog.Loggable;
 import io.github.oblarg.oblog.annotations.Log;
@@ -234,10 +236,7 @@ public Command setSpeedMultiplier(SpeedMultiplier multiplier) {
    *     following
    * @return The command that follows the trajectory
    */
-  public Command follow(
-      PathPlannerTrajectory trajectory, boolean resetPosition, boolean useAllianceColor) {
-    // if (resetPosition) resetOdometry(trajectory.getInitialPose());
-
+  public Command follow(PathPlannerTrajectory trajectory, boolean useAllianceColor) {
     return new PPSwerveControllerCommand(
             trajectory,
             this::getPose,
@@ -253,7 +252,20 @@ public Command follow(
   /** Follows the specified path planner path given a path name */
   public Command follow(String pathName, boolean resetPosition, boolean useAllianceColor) {
     PathPlannerTrajectory loadedPath = PathPlanner.loadPath(pathName, CONSTRAINTS);
-    return follow(loadedPath, resetPosition, useAllianceColor);
+    return (resetPosition ? pathOdometryReset(loadedPath, useAllianceColor) : Commands.none())
+        .andThen(follow(loadedPath, useAllianceColor));
+  }
+
+  /** Resets odometry to first pose in path, using ppl to reflects if using alliance color */
+  public Command pathOdometryReset(PathPlannerTrajectory trajectory, boolean useAllianceColor) {
+    var initialState =
+        useAllianceColor
+            ? PathPlannerTrajectory.transformStateForAlliance(
+                trajectory.getInitialState(), DriverStation.getAlliance())
+            : trajectory.getInitialState();
+    Pose2d initialPose =
+        new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
+    return Commands.runOnce(() -> resetOdometry(initialPose), this);
   }
 
   /** Drives robot based on three double suppliers (x,y and rot) */
@@ -296,7 +308,7 @@ public Command driveToPose(Pose2d startPose, Pose2d desiredPose, boolean useAlli
     PathPoint goal =
         new PathPoint(desiredPose.getTranslation(), heading, desiredPose.getRotation());
     PathPlannerTrajectory trajectory = PathPlanner.generatePath(CONSTRAINTS, start, goal);
-    return follow(trajectory, false, useAllianceColor);
+    return follow(trajectory, useAllianceColor);
   }
 
   /** Creates and follows trajectory for swerve from current pose to desiredPose */