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.