Motion Chaining and Quicker Exiting

[ssejrog]

Is your feature request related to a problem? Please describe.

EZ's exit conditions are inherently slow. There are multiple timers that have to tick a certain amount for it to allow the code to continue. Users can solve this by:

1. making the timer values extremely low This isn't ideal because it makes it clunky to switch between exiting quickly and exiting slower for more consistency. You'd need to switch between two sets of constants whenever you exit.
2. using pid_wait_until() everywhere This can work. At worlds, some teams were using the code below which tells the PID to go a little farther than you input, but use a pid_wait_until() exit when you're where you wanted to be. Not every user will figure out how to implement this.

   
   double last_set_angle = 0.0;
   double added_angle = 3.0;
   double added_distance = 3.0;
   void swing(ez::e_swing swing_type, double angle, int speed, int curve = 0, bool slew = false) {
   last_set_angle = angle;
   double temp_added_angle = ez::util::sgn(angle - chassis.drive_imu_get()) * added_angle;
   chassis.pid_swing_set(swing_type, angle + temp_added_angle, speed, curve, slew);
   chassis.pid_wait_until(angle);
   }

void turn(double angle, int speed, bool slew = false) { last_set_angle = angle; double temp_added_angle = ez::util::sgn(angle - chassis.drive_imu_get()) * added_angle; chassis.pid_turn_set(angle + temp_added_angle, speed, slew); chassis.pid_wait_until(angle); }

void drive(double distance, int speed, bool slew = false) { if (chassis.turnPID.target != last_set_angle) { chassis.pid_drive_toggle(false); chassis.pid_turn_set(last_set_angle, 0, false); } chassis.pid_drive_set(distance + added_distance, speed, slew); chassis.pid_drive_toggle(true); chassis.pid_wait_until(distance); }

#### Describe the solution you'd like
Two new functions, `pid_wait_quick()` and `pid_wait_chain()`.  

`pid_wait_quick()` will be a wrapper for `pid_wait_until(target)`, where `target` is the target from the previously run `pid_x_set(...)`
`pid_wait_chain()` will have the robot try to go farther than you input, but will exit with `pid_wait_quick()`

This allows users to use `pid_wait()` and `pid_wait_until()` like they have been, and use the exit that best fits what they're doing.