From 9102514d1d56277b63954ac136df396603645152 Mon Sep 17 00:00:00 2001
From: sigalrmp <sigalrmp@gmail.com>
Date: Fri, 7 Apr 2023 22:10:43 -0400
Subject: [PATCH] resetting during follow

---
 simgui.json                                   |  3 +++
 .../sciborgs1155/robot/commands/Autos.java    | 19 ++++++---------
 .../sciborgs1155/robot/subsystems/Drive.java  | 24 ++++++++++++++-----
 3 files changed, 28 insertions(+), 18 deletions(-)

diff --git a/simgui.json b/simgui.json
index 7d42fc0b..ebe09c9d 100644
--- 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
index 03d950ad..b3a0e346 100644
--- 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
index 30a59a69..ab507aef 100644
--- 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 */