Added pid_wait_until_index_started (#198)

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

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 :)