From e660c88fa966863ac43aa1e9b6c0951ad0cda79d Mon Sep 17 00:00:00 2001
From: Jess Zarchi <jackfz01@gmail.com>
Date: Fri, 29 Nov 2024 12:46:37 -0800
Subject: [PATCH] Added `pid_wait_until_index_started` (#198)

---
 include/EZ-Template/drive/drive.hpp       | 16 ++++++++++++----
 src/EZ-Template/drive/exit_conditions.cpp |  9 +++++++--
 2 files changed, 19 insertions(+), 6 deletions(-)

diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp
index 1c8bb3b2..d9a914f2 100644
--- a/include/EZ-Template/drive/drive.hpp
+++ b/include/EZ-Template/drive/drive.hpp
@@ -2343,12 +2343,20 @@ class Drive {
   void pid_wait_until_index(int index);
 
   /**
-   * Lock the code in a while loop until this point has been passed.
+   * Lock the code in a while loop until this point becomes the target
    *
-   * \param target
-   *        {x, y}  a pose for the robot to pass through before the while loop is released
+   * \param index
+   *        index of your input points, 0 is the first point in the index.
    */
-  void pid_wait_until_point(pose target);
+  void pid_wait_until_index_started(int index);
+
+      /**
+       * Lock the code in a while loop until this point has been passed.
+       *
+       * \param target
+       *        {x, y}  a pose for the robot to pass through before the while loop is released
+       */
+      void pid_wait_until_point(pose target);
 
   /**
    * Lock the code in a while loop until this point has been passed.  Wrapper for pid_wait_until_point
diff --git a/src/EZ-Template/drive/exit_conditions.cpp b/src/EZ-Template/drive/exit_conditions.cpp
index 1f5857df..cbadaa49 100644
--- a/src/EZ-Template/drive/exit_conditions.cpp
+++ b/src/EZ-Template/drive/exit_conditions.cpp
@@ -391,7 +391,7 @@ void Drive::pid_wait_until(pose target) {
 }
 
 // wait for pp
-void Drive::pid_wait_until_index(int index) {
+void Drive::pid_wait_until_index_started(int index) {
   // Let the PID run at least 1 iteration
   pros::delay(util::DELAY_TIME);
 
@@ -418,8 +418,13 @@ void Drive::pid_wait_until_index(int index) {
 
     pros::delay(util::DELAY_TIME);
   }
+}
 
-  pid_wait_until_point(pp_movements[injected_pp_index[index]].target);
+void Drive::pid_wait_until_index(int index) {
+  pid_wait_until_index_started(index);
+  index += 1;
+  pose target = pp_movements[injected_pp_index[index]].target;
+  pid_wait_until_point(target);
 }
 
 // Pid wait, but quickly :)