Initial commit

src/EZ-Template/PID.cpp

@@ -0,0 +1,260 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/api.hpp"
+#include "api.h"
+
+using namespace ez;
+
+void PID::variables_reset() {
+  output = 0;
+  target = 0;
+  error = 0;
+  prev_error = 0;
+  integral = 0;
+  time = 0;
+  prev_time = 0;
+}
+
+PID::PID() {
+  variables_reset();
+  constants_set(0, 0, 0, 0);
+}
+
+PID::Constants PID::constants_get() { return constants; }
+
+// PID constructor with constants
+PID::PID(double p, double i, double d, double start_i, std::string name) {
+  variables_reset();
+  constants_set(p, i, d, start_i);
+  name_set(name);
+}
+
+// Set PID constants
+void PID::constants_set(double p, double i, double d, double p_start_i) {
+  constants.kp = p;
+  constants.ki = i;
+  constants.kd = d;
+  constants.start_i = p_start_i;
+}
+
+bool PID::constants_set_check() {
+  if (constants.kp == 0.0 && constants.ki == 0.0 && constants.kd == 0.0 && constants.start_i == 0.0)
+    return false;
+  return true;
+}
+
+// Set exit condition timeouts
+void PID::exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout) {
+  exit.small_exit_time = p_small_exit_time;
+  exit.small_error = p_small_error;
+  exit.big_exit_time = p_big_exit_time;
+  exit.big_error = p_big_error;
+  exit.velocity_exit_time = p_velocity_exit_time;
+  exit.mA_timeout = p_mA_timeout;
+}
+
+void PID::target_set(double input) { target = input; }
+double PID::target_get() { return target; }
+
+void PID::i_reset_toggle(bool toggle) { reset_i_sgn = toggle; }
+bool PID::i_reset_get() { return reset_i_sgn; };
+
+double PID::compute(double current) {
+  return compute_error(target - current, current);
+}
+
+double PID::compute_error(double err, double current) {
+  error = err;
+  cur = current;
+
+  return raw_compute();
+}
+
+double PID::raw_compute() {
+  // calculate derivative on measurement instead of error to avoid "derivative kick"
+  // https://www.isa.org/intech-home/2023/june-2023/features/fundamentals-pid-control
+  derivative = cur - prev_current;
+
+  if (constants.ki != 0) {
+    // Only compute i when within a threshold of target
+    if (fabs(error) < constants.start_i)
+      integral += error;
+
+    // Reset i when the sign of error flips
+    if (util::sgn(error) != util::sgn(prev_current) && reset_i_sgn)
+      integral = 0;
+  }
+
+  output = (error * constants.kp) + (integral * constants.ki) - (derivative * constants.kd);
+
+  prev_current = cur;
+  prev_error = error;
+
+  return output;
+}
+
+void PID::timers_reset() {
+  i = 0;
+  k = 0;
+  j = 0;
+  l = 0;
+  m = 0;
+  is_mA = false;
+}
+
+void PID::name_set(std::string p_name) {
+  name = p_name;
+  name_active = name == "" ? false : true;
+}
+
+void PID::exit_condition_print(ez::exit_output exit_type) {
+  std::cout << " ";
+  if (name_active)
+    std::cout << name << " PID " << exit_to_string(exit_type) << " Exit.\n";
+  else
+    std::cout << exit_to_string(exit_type) << " Exit.\n";
+}
+
+void PID::velocity_sensor_secondary_toggle_set(bool toggle) { use_second_sensor = toggle; }
+bool PID::velocity_sensor_secondary_toggle_get() { return use_second_sensor; }
+
+void PID::velocity_sensor_secondary_set(double secondary_sensor) { second_sensor = secondary_sensor; }
+double PID::velocity_sensor_secondary_get() { return second_sensor; }
+
+void PID::velocity_sensor_main_exit_set(double zero) { velocity_zero_main = zero; }
+double PID::velocity_sensor_main_exit_get() { return velocity_zero_main; }
+
+void PID::velocity_sensor_secondary_exit_set(double zero) { velocity_zero_secondary = zero; }
+double PID::velocity_sensor_secondary_exit_get() { return velocity_zero_secondary; }
+
+exit_output PID::exit_condition(bool print) {
+  // If this function is called while all exit constants are 0, print an error
+  if (exit.small_error == 0 && exit.small_exit_time == 0 && exit.big_error == 0 && exit.big_exit_time == 0 && exit.velocity_exit_time == 0 && exit.mA_timeout == 0) {
+    exit_condition_print(ERROR_NO_CONSTANTS);
+    return ERROR_NO_CONSTANTS;
+  }
+
+  // If the robot gets within the target, make sure it's there for small_timeout amount of time
+  if (exit.small_error != 0) {
+    if (abs(error) < exit.small_error) {
+      j += util::DELAY_TIME;
+      i = 0;  // While this is running, don't run big thresh
+      if (j > exit.small_exit_time) {
+        timers_reset();
+        if (print) exit_condition_print(SMALL_EXIT);
+        return SMALL_EXIT;
+      }
+    } else {
+      j = 0;
+    }
+  }
+
+  // If the robot is close to the target, start a timer.  If the robot doesn't get closer within
+  // a certain amount of time, exit and continue.  This does not run while small_timeout is running
+  else if (exit.big_error != 0 && exit.big_exit_time != 0) {  // Check if this condition is enabled
+    if (abs(error) < exit.big_error) {
+      i += util::DELAY_TIME;
+      if (i > exit.big_exit_time) {
+        timers_reset();
+        if (print) exit_condition_print(BIG_EXIT);
+        return BIG_EXIT;
+      }
+    } else {
+      i = 0;
+    }
+  }
+
+  // If the motor velocity is 0, the code will timeout and set interfered to true.
+  if (exit.velocity_exit_time != 0) {  // Check if this condition is enabled
+    if (abs(derivative) <= velocity_zero_main) {
+      k += util::DELAY_TIME;
+      if (k > exit.velocity_exit_time) {
+        timers_reset();
+        if (print) exit_condition_print(VELOCITY_EXIT);
+        return VELOCITY_EXIT;
+      }
+    } else {
+      k = 0;
+    }
+  }
+
+  if (!use_second_sensor)
+    return RUNNING;
+
+  // If the secondary sensors velocity is 0, the code will timeout and set interfered to true.
+  if (exit.velocity_exit_time != 0) {  // Check if this condition is enabled
+    if (abs(second_sensor) <= velocity_zero_secondary) {
+      m += util::DELAY_TIME;
+      if (m > exit.velocity_exit_time) {
+        timers_reset();
+        if (print) exit_condition_print(VELOCITY_EXIT);
+        return VELOCITY_EXIT;
+      }
+    } else {
+      m = 0;
+    }
+  }
+
+  // printf("j: %i   i: %i   k: %i   m: %i\n", j, i, k, m);
+
+  return RUNNING;
+}
+
+exit_output PID::exit_condition(pros::Motor sensor, bool print) {
+  // If the motors are pulling too many mA, the code will timeout and set interfered to true.
+  if (exit.mA_timeout != 0) {  // Check if this condition is enabled
+    if (sensor.is_over_current()) {
+      l += util::DELAY_TIME;
+      if (l > exit.mA_timeout) {
+        timers_reset();
+        if (print) exit_condition_print(mA_EXIT);
+        return mA_EXIT;
+      }
+    } else {
+      l = 0;
+    }
+  }
+
+  return exit_condition(print);
+}
+
+exit_output PID::exit_condition(std::vector<pros::Motor> sensor, bool print) {
+  // If the motors are pulling too many mA, the code will timeout and set interfered to true.
+  if (exit.mA_timeout != 0) {  // Check if this condition is enabled
+    for (auto i : sensor) {
+      // Check if 1 motor is pulling too many mA
+      if (i.is_over_current()) {
+        is_mA = true;
+        break;
+      }
+      // If all of the motors aren't drawing too many mA, keep bool false
+      else {
+        is_mA = false;
+      }
+    }
+    if (is_mA) {
+      l += util::DELAY_TIME;
+      if (l > exit.mA_timeout) {
+        timers_reset();
+        if (print) exit_condition_print(mA_EXIT);
+        return mA_EXIT;
+      }
+    } else {
+      l = 0;
+    }
+  }
+
+  return exit_condition(print);
+}
+
+exit_output PID::exit_condition(pros::MotorGroup sensor, bool print) {
+  std::vector<pros::Motor> vector_sensor;
+  for (i = 0; i < sensor.size(); i++) {
+    vector_sensor.push_back(pros::Motor(sensor.get_port(i)));
+  }
+  return exit_condition(vector_sensor, print);
+}

src/EZ-Template/auton.cpp

@@ -0,0 +1,16 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/api.hpp"
+
+Auton::Auton() {
+  Name = "";
+  auton_call = nullptr;
+}
+Auton::Auton(std::string name, std::function<void()> callback) {
+  Name = name;
+  auton_call = callback;
+}

src/EZ-Template/auton_selector.cpp

@@ -0,0 +1,38 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/api.hpp"
+
+AutonSelector::AutonSelector() {
+  auton_count = 0;
+  auton_page_current = 0;
+  Autons = {};
+}
+
+AutonSelector::AutonSelector(std::vector<Auton> autons) {
+  auton_count = autons.size();
+  auton_page_current = 0;
+  Autons = {};
+  Autons.assign(autons.begin(), autons.end());
+}
+
+void AutonSelector::selected_auton_print() {
+  if (auton_count == 0) return;
+  for (int i = 0; i < 8; i++)
+    pros::lcd::clear_line(i);
+  ez::screen_print("Page " + std::to_string(auton_page_current + 1) + "\n" + Autons[auton_page_current].Name);
+}
+
+void AutonSelector::selected_auton_call() {
+  if (auton_count == 0) return;
+  Autons[last_auton_page_current].auton_call();
+}
+
+void AutonSelector::autons_add(std::vector<Auton> autons) {
+  auton_count += autons.size();
+  auton_page_current = 0;
+  Autons.assign(autons.begin(), autons.end());
+}

src/EZ-Template/drive/drive.cpp

@@ -0,0 +1,477 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include <list>
+
+#include "EZ-Template/api.hpp"
+#include "okapi/api/units/QAngle.hpp"
+#include "pros/llemu.hpp"
+#include "pros/screen.hpp"
+
+using namespace ez;
+
+// Constructor for integrated encoders
+Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports,
+             int imu_port, double wheel_diameter, double ticks, double ratio)
+    : imu(imu_port),
+      left_tracker(-1, -1, false),   // Default value
+      right_tracker(-1, -1, false),  // Default value
+      left_rotation(-1),
+      right_rotation(-1),
+      ez_auto([this] { this->ez_auto_task(); }) {
+  is_tracker = DRIVE_INTEGRATED;
+
+  // Set ports to a global vector
+  for (auto i : left_motor_ports) {
+    pros::Motor temp((std::int8_t)abs(i));
+    temp.set_reversed(util::reversed_active(i));
+    left_motors.push_back(temp);
+  }
+  for (auto i : right_motor_ports) {
+    pros::Motor temp((std::int8_t)abs(i));
+    temp.set_reversed(util::reversed_active(i));
+    right_motors.push_back(temp);
+  }
+
+  // Set constants for tick_per_inch calculation
+  WHEEL_DIAMETER = wheel_diameter;
+  RATIO = ratio;
+  CARTRIDGE = ticks;
+  TICK_PER_INCH = drive_tick_per_inch();
+
+  drive_defaults_set();
+}
+
+// Constructor for tracking wheels plugged into the brain
+Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports,
+             int imu_port, double wheel_diameter, double ticks, double ratio,
+             std::vector<int> left_tracker_ports, std::vector<int> right_tracker_ports)
+    : imu(imu_port),
+      left_tracker(abs(left_tracker_ports[0]), abs(left_tracker_ports[1]), util::reversed_active(left_tracker_ports[0])),
+      right_tracker(abs(right_tracker_ports[0]), abs(right_tracker_ports[1]), util::reversed_active(right_tracker_ports[0])),
+      left_rotation(-1),
+      right_rotation(-1),
+      ez_auto([this] { this->ez_auto_task(); }) {
+  is_tracker = DRIVE_ADI_ENCODER;
+
+  // Set ports to a global vector
+  for (auto i : left_motor_ports) {
+    pros::Motor temp(abs(i));
+    temp.set_reversed(util::reversed_active(i));
+    left_motors.push_back(temp);
+  }
+  for (auto i : right_motor_ports) {
+    pros::Motor temp(abs(i));
+    temp.set_reversed(util::reversed_active(i));
+    right_motors.push_back(temp);
+  }
+
+  // Set constants for tick_per_inch calculation
+  WHEEL_DIAMETER = wheel_diameter;
+  RATIO = ratio;
+  CARTRIDGE = ticks;
+  TICK_PER_INCH = drive_tick_per_inch();
+
+  drive_defaults_set();
+}
+
+// Constructor for tracking wheels plugged into a 3 wire expander
+Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports,
+             int imu_port, double wheel_diameter, double ticks, double ratio,
+             std::vector<int> left_tracker_ports, std::vector<int> right_tracker_ports, int expander_smart_port)
+    : imu(imu_port),
+      left_tracker({expander_smart_port, abs(left_tracker_ports[0]), abs(left_tracker_ports[1])}, util::reversed_active(left_tracker_ports[0])),
+      right_tracker({expander_smart_port, abs(right_tracker_ports[0]), abs(right_tracker_ports[1])}, util::reversed_active(right_tracker_ports[0])),
+      left_rotation(-1),
+      right_rotation(-1),
+      ez_auto([this] { this->ez_auto_task(); }) {
+  is_tracker = DRIVE_ADI_ENCODER;
+
+  // Set ports to a global vector
+  for (auto i : left_motor_ports) {
+    pros::Motor temp(abs(i));
+    temp.set_reversed(util::reversed_active(i));
+    left_motors.push_back(temp);
+  }
+  for (auto i : right_motor_ports) {
+    pros::Motor temp(abs(i));
+    temp.set_reversed(util::reversed_active(i));
+    right_motors.push_back(temp);
+  }
+
+  // Set constants for tick_per_inch calculation
+  WHEEL_DIAMETER = wheel_diameter;
+  RATIO = ratio;
+  CARTRIDGE = ticks;
+  TICK_PER_INCH = drive_tick_per_inch();
+
+  drive_defaults_set();
+}
+
+// Constructor for rotation sensors
+Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports,
+             int imu_port, double wheel_diameter, double ratio,
+             int left_rotation_port, int right_rotation_port)
+    : imu(imu_port),
+      left_tracker(-1, -1, false),   // Default value
+      right_tracker(-1, -1, false),  // Default value
+      left_rotation(abs(left_rotation_port)),
+      right_rotation(abs(right_rotation_port)),
+      ez_auto([this] { this->ez_auto_task(); }) {
+  is_tracker = DRIVE_ROTATION;
+  left_rotation.set_reversed(util::reversed_active(left_rotation_port));
+  right_rotation.set_reversed(util::reversed_active(right_rotation_port));
+
+  // Set ports to a global vector
+  for (auto i : left_motor_ports) {
+    pros::Motor temp(abs(i));
+    temp.set_reversed(util::reversed_active(i));
+    left_motors.push_back(temp);
+  }
+  for (auto i : right_motor_ports) {
+    pros::Motor temp(abs(i));
+    temp.set_reversed(util::reversed_active(i));
+    right_motors.push_back(temp);
+  }
+
+  // Set constants for tick_per_inch calculation
+  WHEEL_DIAMETER = wheel_diameter;
+  RATIO = ratio;
+  CARTRIDGE = 36000;
+  TICK_PER_INCH = drive_tick_per_inch();
+
+  drive_defaults_set();
+}
+
+void Drive::drive_defaults_set() {
+  imu.set_data_rate(5);
+
+  std::cout << std::fixed;
+  std::cout << std::setprecision(2);
+
+  // PID Constants
+  pid_drive_constants_set(20.0, 0.0, 100.0);
+  pid_heading_constants_set(11.0, 0.0, 20.0);
+  pid_turn_constants_set(3.0, 0.05, 20.0, 15.0);
+  pid_swing_constants_set(6.0, 0.0, 65.0);
+  pid_odom_angular_constants_set(6.5, 0.0, 52.5);
+  pid_odom_boomerang_constants_set(5.8, 0.0, 32.5);
+  pid_turn_min_set(30);
+  pid_swing_min_set(30);
+
+  // Path Constants
+  odom_path_smooth_constants_set(0.75, 0.03, 0.0001);
+  odom_path_spacing_set(0.5_in);
+  odom_turn_bias_set(0.9);
+  odom_look_ahead_set(7_in);
+  odom_boomerang_distance_set(16_in);
+  odom_boomerang_dlead_set(0.625);
+
+  // Slew constants
+  slew_turn_constants_set(3_deg, 70);
+  slew_drive_constants_set(3_in, 70);
+  slew_swing_constants_set(3_in, 80);
+
+  // Exit condition constants
+  pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms);
+  pid_swing_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms);
+  pid_drive_exit_condition_set(90_ms, 1_in, 250_ms, 3_in, 500_ms, 500_ms);
+  pid_odom_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms);
+  pid_odom_drive_exit_condition_set(90_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms);
+
+  pid_odom_behavior_set(ez::shortest);  // Default odom turning to shortest
+
+  // Motion chaining
+  pid_turn_chain_constant_set(3_deg);
+  pid_swing_chain_constant_set(5_deg);
+  pid_drive_chain_constant_set(3_in);
+
+  // Modify joystick curve on controller (defaults to disabled)
+  opcontrol_curve_buttons_toggle(true);
+
+  // Left / Right modify buttons
+  opcontrol_curve_buttons_left_set(pros::E_CONTROLLER_DIGITAL_LEFT, pros::E_CONTROLLER_DIGITAL_RIGHT);
+  opcontrol_curve_buttons_right_set(pros::E_CONTROLLER_DIGITAL_Y, pros::E_CONTROLLER_DIGITAL_A);
+
+  // Enable auto printing and drive motors moving
+  pid_drive_toggle(true);
+  pid_print_toggle(true);
+
+  // Disables limit switch for auto selector
+  as::limit_switch_lcd_initialize(nullptr, nullptr);
+}
+
+double Drive::drive_tick_per_inch() {
+  if (is_tracker == ODOM_TRACKER)
+    return odom_tracker_right->ticks_per_inch();
+
+  CIRCUMFERENCE = WHEEL_DIAMETER * M_PI;
+
+  if (is_tracker == DRIVE_ADI_ENCODER || is_tracker == DRIVE_ROTATION)
+    TICK_PER_REV = CARTRIDGE * RATIO;
+  else if (is_tracker == DRIVE_INTEGRATED)
+    TICK_PER_REV = (50.0 * (3600.0 / CARTRIDGE)) * RATIO;  // with no cart, the encoder reads 50 counts per rotation
+
+  TICK_PER_INCH = (TICK_PER_REV / CIRCUMFERENCE);
+  return TICK_PER_INCH;
+}
+
+void Drive::drive_ratio_set(double ratio) { RATIO = ratio; }
+double Drive::drive_ratio_get() { return RATIO; }
+void Drive::drive_rpm_set(double rpm) { CARTRIDGE = rpm; }
+double Drive::drive_rpm_get() { return CARTRIDGE; }
+
+void Drive::private_drive_set(int left, int right) {
+  if (pros::millis() < 1500) return;
+
+  for (auto i : left_motors) {
+    if (!pto_check(i)) i.move_voltage(left * (12000.0 / 127.0));  // If the motor is in the pto list, don't do anything to the motor.
+  }
+  for (auto i : right_motors) {
+    if (!pto_check(i)) i.move_voltage(right * (12000.0 / 127.0));  // If the motor is in the pto list, don't do anything to the motor.
+  }
+}
+
+void Drive::drive_set(int left, int right) {
+  drive_mode_set(DISABLE, false);
+  private_drive_set(left, right);
+}
+
+std::vector<int> Drive::drive_get() {
+  int left = left_motors[0].get_voltage() / (12000.0 / 127.0);
+  int right = right_motors[0].get_voltage() / (12000.0 / 127.0);
+  return {left, right};
+}
+
+void Drive::drive_current_limit_set(int mA) {
+  if (abs(mA) > 2500) {
+    mA = 2500;
+  }
+  CURRENT_MA = mA;
+  for (auto i : left_motors) {
+    if (!pto_check(i)) i.set_current_limit(abs(mA));  // If the motor is in the pto list, don't do anything to the motor.
+  }
+  for (auto i : right_motors) {
+    if (!pto_check(i)) i.set_current_limit(abs(mA));  // If the motor is in the pto list, don't do anything to the motor.
+  }
+}
+
+int Drive::drive_current_limit_get() {
+  return CURRENT_MA;
+}
+
+// Motor telemetry
+void Drive::drive_sensor_reset() {
+  // Update active brake constants
+  left_activebrakePID.target_set(0.0);
+  right_activebrakePID.target_set(0.0);
+
+  // Reset odom stuff
+  h_last = 0.0;
+  l_last = 0.0;
+  r_last = 0.0;
+  t_last = 0.0;
+
+  // Reset sensors
+  left_motors.front().tare_position();
+  right_motors.front().tare_position();
+  if (odom_tracker_left_enabled) odom_tracker_left->reset();
+  if (odom_tracker_right_enabled) odom_tracker_right->reset();
+  if (odom_tracker_front_enabled) odom_tracker_front->reset();
+  if (odom_tracker_back_enabled) odom_tracker_back->reset();
+  if (is_tracker == DRIVE_ADI_ENCODER) {
+    left_tracker.reset();
+    right_tracker.reset();
+    return;
+  } else if (is_tracker == DRIVE_ROTATION) {
+    left_rotation.reset_position();
+    right_rotation.reset_position();
+    return;
+  }
+}
+
+int Drive::drive_sensor_right_raw() {
+  if (is_tracker == DRIVE_ADI_ENCODER)
+    return right_tracker.get_value();
+  else if (is_tracker == DRIVE_ROTATION)
+    return right_rotation.get_position();
+  else if (is_tracker == ODOM_TRACKER)
+    return odom_tracker_right->get_raw();
+  return right_motors.front().get_position();
+}
+double Drive::drive_sensor_right() {
+  if (is_tracker == ODOM_TRACKER)
+    return odom_tracker_right->get();
+  return drive_sensor_right_raw() / drive_tick_per_inch();
+}
+int Drive::drive_velocity_right() { return right_motors.front().get_actual_velocity(); }
+double Drive::drive_mA_right() { return right_motors.front().get_current_draw(); }
+bool Drive::drive_current_right_over() { return right_motors.front().is_over_current(); }
+
+int Drive::drive_sensor_left_raw() {
+  if (is_tracker == DRIVE_ADI_ENCODER)
+    return left_tracker.get_value();
+  else if (is_tracker == DRIVE_ROTATION)
+    return left_rotation.get_position();
+  else if (is_tracker == ODOM_TRACKER)
+    return odom_tracker_left->get_raw();
+  return left_motors.front().get_position();
+}
+double Drive::drive_sensor_left() {
+  if (is_tracker == ODOM_TRACKER)
+    return odom_tracker_left->get();
+  return drive_sensor_left_raw() / drive_tick_per_inch();
+}
+int Drive::drive_velocity_left() { return left_motors.front().get_actual_velocity(); }
+double Drive::drive_mA_left() { return left_motors.front().get_current_draw(); }
+bool Drive::drive_current_left_over() { return left_motors.front().is_over_current(); }
+
+void Drive::drive_imu_reset(double new_heading) {
+  imu.set_rotation(new_heading);
+  angle_rad = util::to_rad(new_heading);
+  t_last = angle_rad;
+}
+double Drive::drive_imu_get() { return imu.get_rotation() * IMU_SCALER; }
+double Drive::drive_imu_accel_get() { return imu.get_accel().x + imu.get_accel().y; }
+
+void Drive::drive_imu_scaler_set(double scaler) { IMU_SCALER = scaler; }
+double Drive::drive_imu_scaler_get() { return IMU_SCALER; }
+
+void Drive::drive_imu_display_loading(int iter) {
+  // If the lcd is already initialized, don't run this function
+  if (pros::lcd::is_initialized()) return;
+
+  // Border
+  int border = 50;
+
+  // Create the border
+  pros::screen::set_pen(pros::c::COLOR_WHITE);
+  for (int i = 1; i < 3; i++) {
+    pros::screen::draw_rect(border + i, border + i, 480 - border - i, 240 - border - i);
+  }
+
+  // While IMU is loading
+  if (iter < 2000) {
+    static int last_x1 = border;
+    pros::screen::set_pen(0x00FF6EC7);  // EZ Pink
+    int x1 = (iter * ((480 - (border * 2)) / 2000.0)) + border;
+    pros::screen::fill_rect(last_x1, border, x1, 240 - border);
+    last_x1 = x1;
+  }
+  // Failsafe time
+  else {
+    static int last_x1 = border;
+    pros::screen::set_pen(pros::c::COLOR_RED);
+    int x1 = ((iter - 2000) * ((480 - (border * 2)) / 1000.0)) + border;
+    pros::screen::fill_rect(last_x1, border, x1, 240 - border);
+    last_x1 = x1;
+  }
+}
+
+bool Drive::drive_imu_calibrate(bool run_loading_animation) {
+  imu_calibration_complete = false;
+  imu.reset();
+  int iter = 0;
+  bool current_status = imu.is_calibrating();
+  bool last_status = current_status;
+  bool successful = false;
+  while (true) {
+    iter += util::DELAY_TIME;
+
+    if (run_loading_animation) drive_imu_display_loading(iter);
+
+    if (!successful) {
+      last_status = current_status;
+      current_status = imu.is_calibrating();
+      successful = !current_status && last_status ? true : false;
+    }
+
+    if (iter >= 2000) {
+      if (successful) {
+        break;
+      }
+      if (iter >= 3000) {
+        printf("No IMU plugged in, (took %d ms to realize that)\n", iter);
+        imu_calibrate_took_too_long = true;
+        return false;
+      }
+    }
+    pros::delay(util::DELAY_TIME);
+  }
+  printf("IMU is done calibrating (took %d ms)\n", iter);
+  imu_calibration_complete = true;
+  imu_calibrate_took_too_long = iter > 2000 ? true : false;
+  return true;
+}
+
+bool Drive::drive_imu_calibrated() {
+  if (imu_calibration_complete && !imu_calibrate_took_too_long)
+    return true;
+  return false;
+}
+
+// Brake modes
+void Drive::drive_brake_set(pros::motor_brake_mode_e_t brake_type) {
+  CURRENT_BRAKE = brake_type;
+  for (auto i : left_motors) {
+    if (!pto_check(i)) i.set_brake_mode(brake_type);  // If the motor is in the pto list, don't do anything to the motor.
+  }
+  for (auto i : right_motors) {
+    if (!pto_check(i)) i.set_brake_mode(brake_type);  // If the motor is in the pto list, don't do anything to the motor.
+  }
+}
+
+// Get brake
+pros::motor_brake_mode_e_t Drive::drive_brake_get() {
+  return CURRENT_BRAKE;
+}
+
+void Drive::initialize() {
+  opcontrol_curve_sd_initialize();
+  drive_imu_calibrate();
+  drive_sensor_reset();
+}
+
+void Drive::odom_tracker_left_set(tracking_wheel* input) {
+  if (input == nullptr) return;
+
+  odom_tracker_left = input;
+  odom_tracker_left_enabled = true;
+
+  // Assume the user input a positive number and set it to a negative number
+  odom_tracker_left->distance_to_center_flip_set(true);
+
+  // If the user has input a left and right tracking wheel,
+  // the tracking wheels become the new sensors always
+  if (odom_tracker_right_enabled)
+    is_tracker = ODOM_TRACKER;
+}
+void Drive::odom_tracker_right_set(tracking_wheel* input) {
+  if (input == nullptr) return;
+
+  odom_tracker_right = input;
+  odom_tracker_right_enabled = true;
+
+  // If the user has input a left and right tracking wheel,
+  // the tracking wheels become the new sensors always
+  if (odom_tracker_left_enabled)
+    is_tracker = ODOM_TRACKER;
+}
+void Drive::odom_tracker_front_set(tracking_wheel* input) {
+  if (input == nullptr) return;
+
+  odom_tracker_front = input;
+  odom_tracker_front_enabled = true;
+}
+void Drive::odom_tracker_back_set(tracking_wheel* input) {
+  if (input == nullptr) return;
+
+  odom_tracker_back = input;
+  odom_tracker_back_enabled = true;
+
+  // Set the center distance to be negative
+  odom_tracker_back->distance_to_center_flip_set(true);
+}

src/EZ-Template/drive/exit_conditions.cpp

@@ -0,0 +1,534 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/drive/drive.hpp"
+#include "EZ-Template/util.hpp"
+
+using namespace ez;
+
+void Drive::pid_drive_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout, bool use_imu) {
+  leftPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
+  rightPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
+  leftPID.velocity_sensor_secondary_toggle_set(use_imu);
+  rightPID.velocity_sensor_secondary_toggle_set(use_imu);
+  internal_leftPID.exit = leftPID.exit;
+  internal_rightPID.exit = rightPID.exit;
+}
+
+void Drive::pid_drive_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QLength p_small_error, okapi::QTime p_big_exit_time, okapi::QLength p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout, bool use_imu) {
+  // Convert okapi units to doubles
+  double se = p_small_error.convert(okapi::inch);
+  double be = p_big_error.convert(okapi::inch);
+  int set = p_small_exit_time.convert(okapi::millisecond);
+  int bet = p_big_exit_time.convert(okapi::millisecond);
+  int vet = p_velocity_exit_time.convert(okapi::millisecond);
+  int mAt = p_mA_timeout.convert(okapi::millisecond);
+
+  pid_drive_exit_condition_set(set, se, bet, be, vet, mAt, use_imu);
+}
+
+void Drive::pid_turn_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout, bool use_imu) {
+  turnPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
+  turnPID.velocity_sensor_secondary_toggle_set(use_imu);
+}
+
+void Drive::pid_turn_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QAngle p_small_error, okapi::QTime p_big_exit_time, okapi::QAngle p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout, bool use_imu) {
+  // Convert okapi units to doubles
+  double se = p_small_error.convert(okapi::degree);
+  double be = p_big_error.convert(okapi::degree);
+  int set = p_small_exit_time.convert(okapi::millisecond);
+  int bet = p_big_exit_time.convert(okapi::millisecond);
+  int vet = p_velocity_exit_time.convert(okapi::millisecond);
+  int mAt = p_mA_timeout.convert(okapi::millisecond);
+
+  pid_turn_exit_condition_set(set, se, bet, be, vet, mAt, use_imu);
+}
+
+void Drive::pid_swing_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout, bool use_imu) {
+  swingPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
+  swingPID.velocity_sensor_secondary_toggle_set(use_imu);
+}
+
+void Drive::pid_swing_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QAngle p_small_error, okapi::QTime p_big_exit_time, okapi::QAngle p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout, bool use_imu) {
+  // Convert okapi units to doubles
+  double se = p_small_error.convert(okapi::degree);
+  double be = p_big_error.convert(okapi::degree);
+  int set = p_small_exit_time.convert(okapi::millisecond);
+  int bet = p_big_exit_time.convert(okapi::millisecond);
+  int vet = p_velocity_exit_time.convert(okapi::millisecond);
+  int mAt = p_mA_timeout.convert(okapi::millisecond);
+
+  pid_swing_exit_condition_set(set, se, bet, be, vet, mAt, use_imu);
+}
+
+void Drive::pid_odom_drive_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout, bool use_imu) {
+  xyPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
+  xyPID.velocity_sensor_secondary_toggle_set(use_imu);
+}
+
+void Drive::pid_odom_drive_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QLength p_small_error, okapi::QTime p_big_exit_time, okapi::QLength p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout, bool use_imu) {
+  // Convert okapi units to doubles
+  double se = p_small_error.convert(okapi::inch);
+  double be = p_big_error.convert(okapi::inch);
+  int set = p_small_exit_time.convert(okapi::millisecond);
+  int bet = p_big_exit_time.convert(okapi::millisecond);
+  int vet = p_velocity_exit_time.convert(okapi::millisecond);
+  int mAt = p_mA_timeout.convert(okapi::millisecond);
+
+  pid_odom_drive_exit_condition_set(set, se, bet, be, vet, mAt, use_imu);
+}
+
+void Drive::pid_odom_turn_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout, bool use_imu) {
+  current_a_odomPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
+  current_a_odomPID.velocity_sensor_secondary_toggle_set(use_imu);
+}
+
+void Drive::pid_odom_turn_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QAngle p_small_error, okapi::QTime p_big_exit_time, okapi::QAngle p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout, bool use_imu) {
+  // Convert okapi units to doubles
+  double se = p_small_error.convert(okapi::degree);
+  double be = p_big_error.convert(okapi::degree);
+  int set = p_small_exit_time.convert(okapi::millisecond);
+  int bet = p_big_exit_time.convert(okapi::millisecond);
+  int vet = p_velocity_exit_time.convert(okapi::millisecond);
+  int mAt = p_mA_timeout.convert(okapi::millisecond);
+
+  pid_odom_turn_exit_condition_set(set, se, bet, be, vet, mAt, use_imu);
+}
+
+// User wrapper for exit condition
+void Drive::pid_wait() {
+  // Let the PID run at least 1 iteration
+  pros::delay(util::DELAY_TIME);
+
+  if (mode == DRIVE) {
+    exit_output left_exit = RUNNING;
+    exit_output right_exit = RUNNING;
+    while (left_exit == RUNNING || right_exit == RUNNING) {
+      leftPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+      rightPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+      left_exit = left_exit != RUNNING ? left_exit : leftPID.exit_condition(left_motors[0]);
+      right_exit = right_exit != RUNNING ? right_exit : rightPID.exit_condition(right_motors[0]);
+      pros::delay(util::DELAY_TIME);
+    }
+    if (print_toggle) std::cout << "  Left: " << exit_to_string(left_exit) << " Exit, error: " << leftPID.error << "   Right: " << exit_to_string(right_exit) << " Exit, error: " << rightPID.error << "\n";
+
+    if (left_exit == mA_EXIT || left_exit == VELOCITY_EXIT || right_exit == mA_EXIT || right_exit == VELOCITY_EXIT) {
+      interfered = true;
+    }
+  }
+
+  // Odom Exits
+  else if (mode == POINT_TO_POINT || mode == PURE_PURSUIT) {
+    exit_output xy_exit = RUNNING;
+    exit_output a_exit = RUNNING;
+
+    // Wait until pure pursuit is on the last point, then continue as normal
+    if (mode == PURE_PURSUIT) {
+      while (pp_index != pp_movements.size() - 1) {
+        xyPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+        current_a_odomPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+        xy_exit = xy_exit != RUNNING ? xy_exit : xyPID.exit_condition({left_motors[0], right_motors[0]});
+        a_exit = a_exit != RUNNING ? a_exit : current_a_odomPID.exit_condition({left_motors[0], right_motors[0]});
+
+        if ((xy_exit == mA_EXIT || xy_exit == VELOCITY_EXIT) && (a_exit == mA_EXIT || a_exit == VELOCITY_EXIT)) {
+          if (print_toggle) std::cout << "  XY: " << exit_to_string(xy_exit) << " Exited early, error: " << xyPID.error << ".   Angle: " << exit_to_string(a_exit) << " Exited early, error: " << current_a_odomPID.error << ".\n";
+          break;
+        }
+
+        pros::delay(util::DELAY_TIME);
+      }
+    }
+
+    // When we're at the last point in PP / we're just going to point
+    while (xy_exit == RUNNING || a_exit == RUNNING) {
+      xyPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+      current_a_odomPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+      xy_exit = xy_exit != RUNNING ? xy_exit : xyPID.exit_condition({left_motors[0], right_motors[0]});
+      a_exit = a_exit != RUNNING ? a_exit : current_a_odomPID.exit_condition({left_motors[0], right_motors[0]});
+      pros::delay(util::DELAY_TIME);
+    }
+    if (print_toggle) std::cout << "  XY: " << exit_to_string(xy_exit) << " Exit, error: " << xyPID.error << ".   Angle: " << exit_to_string(a_exit) << " Exit, error: " << current_a_odomPID.error << ".\n";
+
+    if (xy_exit == mA_EXIT || xy_exit == VELOCITY_EXIT || a_exit == mA_EXIT || a_exit == VELOCITY_EXIT) {
+      interfered = true;
+    }
+  }
+
+  // Turn Exit
+  else if (mode == TURN || mode == TURN_TO_POINT) {
+    exit_output turn_exit = RUNNING;
+    while (turn_exit == RUNNING) {
+      turnPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+      turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]});
+      pros::delay(util::DELAY_TIME);
+    }
+    if (print_toggle) std::cout << "  Turn: " << exit_to_string(turn_exit) << " Exit, error: " << turnPID.error << "\n";
+
+    if (turn_exit == mA_EXIT || turn_exit == VELOCITY_EXIT) {
+      interfered = true;
+    }
+  }
+
+  // Swing Exit
+  else if (mode == SWING) {
+    exit_output swing_exit = RUNNING;
+    pros::Motor& sensor = current_swing == ez::LEFT_SWING ? left_motors[0] : right_motors[0];
+    while (swing_exit == RUNNING) {
+      swingPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+      swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor);
+      pros::delay(util::DELAY_TIME);
+    }
+    if (print_toggle) std::cout << "  Swing: " << exit_to_string(swing_exit) << " Exit, error: " << swingPID.error << "\n";
+
+    if (swing_exit == mA_EXIT || swing_exit == VELOCITY_EXIT) {
+      interfered = true;
+    }
+  }
+}
+
+void Drive::wait_until_drive(double target) {
+  pros::delay(10);
+
+  // Make sure mode is correct
+  if (!(mode == DRIVE || mode == POINT_TO_POINT || mode == PURE_PURSUIT)) {
+    printf("Mode needs to be drive!\n");
+    return;
+  }
+
+  // Calculate error between current and target (target needs to be an in between position)
+  double l_tar = l_start + target;
+  double r_tar = r_start + target;
+  double l_error = l_tar - drive_sensor_left();
+  double r_error = r_tar - drive_sensor_right();
+  int l_sgn = util::sgn(l_error);
+  int r_sgn = util::sgn(r_error);
+
+  exit_output left_exit = RUNNING;
+  exit_output right_exit = RUNNING;
+
+  while (true) {
+    l_error = l_tar - drive_sensor_left();
+    r_error = r_tar - drive_sensor_right();
+
+    // Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop
+    if (util::sgn(l_error) == l_sgn || util::sgn(r_error) == r_sgn) {
+      if (left_exit == RUNNING || right_exit == RUNNING) {
+        leftPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+        rightPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+        left_exit = left_exit != RUNNING ? left_exit : leftPID.exit_condition(left_motors[0]);
+        right_exit = right_exit != RUNNING ? right_exit : rightPID.exit_condition(right_motors[0]);
+        pros::delay(util::DELAY_TIME);
+      } else {
+        if (print_toggle) {
+          std::cout << "  Left: " << exit_to_string(left_exit) << " Wait Until Exit Failsafe, triggered at " << drive_sensor_left() - l_start << " instead of " << l_tar << "\n";
+          std::cout << "  Right: " << exit_to_string(right_exit) << " Wait Until Exit Failsafe, triggered at " << drive_sensor_right() - r_start << " instead of " << r_tar << "\n";
+        }
+        if (left_exit == mA_EXIT || left_exit == VELOCITY_EXIT || right_exit == mA_EXIT || right_exit == VELOCITY_EXIT) {
+          interfered = true;
+        }
+        return;
+      }
+    }
+    // Once we've past target, return
+    else if (util::sgn(l_error) != l_sgn || util::sgn(r_error) != r_sgn) {
+      if (print_toggle) printf("  Drive Wait Until Exit Success. Triggered at: L,R(%.2f, %.2f)  Target: L,R(%.2f, %.2f)\n", drive_sensor_left() - l_start, drive_sensor_right() - r_start, l_tar, r_tar);
+      leftPID.timers_reset();
+      rightPID.timers_reset();
+      return;
+    }
+
+    pros::delay(util::DELAY_TIME);
+  }
+}
+
+// Function to wait until a certain position is reached.  Wrapper for exit condition.
+void Drive::wait_until_turn_swing(double target) {
+  // Make sure mode is correct
+  if (!(mode == TURN || mode == SWING || mode == TURN_TO_POINT)) {
+    printf("Mode needs to be swing or turn!\n");
+    return;
+  }
+
+  // Create new target that is the shortest from current
+  target = new_turn_target_compute(target, drive_imu_get(), shortest);
+
+  // Calculate error between current and target (target needs to be an in between position)
+  double g_error = target - drive_imu_get();
+  int g_sgn = util::sgn(g_error);
+
+  exit_output turn_exit = RUNNING;
+  exit_output swing_exit = RUNNING;
+
+  pros::Motor& sensor = current_swing == ez::LEFT_SWING ? left_motors[0] : right_motors[0];
+
+  while (true) {
+    g_error = target - drive_imu_get();
+
+    // If turning...
+    if (mode == TURN || mode == TURN_TO_POINT) {
+      // Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop
+      if (util::sgn(g_error) == g_sgn) {
+        if (turn_exit == RUNNING) {
+          turnPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+          turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]});
+          pros::delay(util::DELAY_TIME);
+        } else {
+          if (print_toggle) std::cout << "  Turn: " << exit_to_string(turn_exit) << " Wait Until Exit Failsafe, triggered at " << drive_imu_get() << " instead of " << target << "\n";
+
+          if (turn_exit == mA_EXIT || turn_exit == VELOCITY_EXIT) {
+            interfered = true;
+          }
+          return;
+        }
+      }
+      // Once we've past target, return
+      else if (util::sgn(g_error) != g_sgn) {
+        if (print_toggle) printf("  Turn Wait Until Exit Success, triggered at %.2f.  Target: %.2f\n", drive_imu_get(), target);
+        turnPID.timers_reset();
+        return;
+      }
+    }
+
+    // If swinging...
+    else {
+      // Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop
+      if (util::sgn(g_error) == g_sgn) {
+        if (swing_exit == RUNNING) {
+          swingPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+          swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor);
+          pros::delay(util::DELAY_TIME);
+        } else {
+          if (print_toggle) std::cout << "  Swing: " << exit_to_string(swing_exit) << " Wait Until Exit Failsafe, triggered at " << drive_imu_get() << " instead of " << target << "\n";
+
+          if (swing_exit == mA_EXIT || swing_exit == VELOCITY_EXIT) {
+            interfered = true;
+          }
+          return;
+        }
+      }
+      // Once we've past target, return
+      else if (util::sgn(g_error) != g_sgn) {
+        if (print_toggle) printf("  Swing Wait Until Exit Success, triggered at %.2f. Target: %.2f\n", drive_imu_get(), target);
+        swingPID.timers_reset();
+        return;
+      }
+    }
+
+    pros::delay(util::DELAY_TIME);
+  }
+}
+
+void Drive::pid_wait_until(okapi::QLength target) {
+  // If robot is driving...
+  if (mode == DRIVE || mode == POINT_TO_POINT || mode == PURE_PURSUIT) {
+    wait_until_drive(target.convert(okapi::inch));
+  } else {
+    printf("QLength not supported for turn or swing!\n");
+  }
+}
+
+void Drive::pid_wait_until(okapi::QAngle target) {
+  // If robot is driving...
+  if (mode == TURN || mode == SWING || mode == TURN_TO_POINT) {
+    wait_until_turn_swing(target.convert(okapi::degree));
+  } else {
+    printf("QAngle not supported for drive!\n");
+  }
+}
+
+void Drive::pid_wait_until(double target) {
+  // If driving...
+  if (mode == DRIVE || mode == POINT_TO_POINT || mode == PURE_PURSUIT) {
+    wait_until_drive(target);
+  }
+  // If turning or swinging...
+  else if (mode == TURN || mode == SWING || mode == TURN_TO_POINT) {
+    wait_until_turn_swing(target);
+  } else {
+    printf("Not in a valid drive mode!\n");
+  }
+}
+
+void Drive::pid_wait_until_point(pose target) {
+  pros::delay(10);
+
+  int xy_sgn = util::sgn(is_past_target(target, odom_pose_get()));
+
+  exit_output xy_exit = RUNNING;
+  exit_output a_exit = RUNNING;
+
+  while (true) {
+    xyPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+    current_a_odomPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+    xy_exit = xy_exit != RUNNING ? xy_exit : xyPID.exit_condition({left_motors[0], right_motors[0]});
+    a_exit = a_exit != RUNNING ? a_exit : current_a_odomPID.exit_condition({left_motors[0], right_motors[0]});
+
+    if (xy_exit != RUNNING && a_exit != RUNNING) {
+      if (print_toggle) {
+        std::cout << "  XY: " << exit_to_string(xy_exit) << " Wait Until Exit Failsafe, triggered at (" << odom_x_get() << ", " << odom_y_get() << ") instead of (" << target.x << ", " << target.y << ")\n";
+        xyPID.timers_reset();
+        current_a_odomPID.timers_reset();
+      }
+      return;
+    }
+
+    if (util::sgn((is_past_target(target, odom_pose_get()))) != xy_sgn) {
+      if (print_toggle) printf("  XY Wait Until Exit Success, triggered at (%.2f, %.2f).  Target: (%.2f, %.2f)\n", odom_x_get(), odom_y_get(), target.x, target.y);
+      xyPID.timers_reset();
+      current_a_odomPID.timers_reset();
+      return;
+    }
+
+    pros::delay(util::DELAY_TIME);
+  }
+}
+
+void Drive::pid_wait_until_point(united_pose target) { pid_wait_until_point(util::united_pose_to_pose(target)); }
+void Drive::pid_wait_until(pose target) { pid_wait_until_point(target); }
+void Drive::pid_wait_until(united_pose target) { pid_wait_until_point(target); }
+
+// wait for pp
+void Drive::pid_wait_until_index_started(int index) {
+  // Let the PID run at least 1 iteration
+  pros::delay(util::DELAY_TIME);
+
+  if (index > injected_pp_index.size() - 2 || index < 0)
+    printf("  Wait Until PP Error!  Index %i is not within range!  %i is max!\n", index, injected_pp_index.size() - 2);
+  index += 1;
+
+  exit_output xy_exit = RUNNING;
+  exit_output a_exit = RUNNING;
+  while (pp_index < injected_pp_index[index]) {
+    xyPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+    current_a_odomPID.velocity_sensor_secondary_set(drive_imu_accel_get());
+    xy_exit = xy_exit != RUNNING ? xy_exit : xyPID.exit_condition({left_motors[0], right_motors[0]});
+    a_exit = a_exit != RUNNING ? a_exit : current_a_odomPID.exit_condition({left_motors[0], right_motors[0]});
+
+    if (xy_exit != RUNNING && a_exit != RUNNING) {
+      if (print_toggle) {
+        std::cout << "  XY: " << exit_to_string(xy_exit) << " Wait Until Exit Failsafe, triggered at (" << odom_x_get() << ", " << odom_y_get() << ") instead of (" << pp_movements[index].target.x << ", " << pp_movements[index].target.y << ")\n";
+        xyPID.timers_reset();
+        current_a_odomPID.timers_reset();
+      }
+      break;
+    }
+
+    pros::delay(util::DELAY_TIME);
+  }
+}
+
+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 :)
+void Drive::pid_wait_quick() {
+  if (mode == PURE_PURSUIT) {
+    pid_wait_until_index(injected_pp_index.size() - 2);
+    return;
+  } else if (mode == POINT_TO_POINT) {
+    pid_wait_until_point(odom_target_start);
+    return;
+  } else if (!(mode == DRIVE || mode == TURN || mode == SWING || mode == TURN_TO_POINT)) {
+    printf("Not in a valid drive mode!\n");
+    return;
+  }
+
+  // This is the target the user set, not the modified chained target
+  pid_wait_until(chain_target_start);
+}
+
+// Set drive motion chain constants
+void Drive::pid_drive_chain_constant_set(double input) {
+  pid_drive_chain_forward_constant_set(input);
+  pid_drive_chain_backward_constant_set(input);
+}
+void Drive::pid_drive_chain_forward_constant_set(double input) { drive_forward_motion_chain_scale = fabs(input); }
+void Drive::pid_drive_chain_backward_constant_set(double input) { drive_backward_motion_chain_scale = fabs(input); }
+void Drive::pid_drive_chain_constant_set(okapi::QLength input) { pid_drive_chain_constant_set(input.convert(okapi::inch)); }
+void Drive::pid_drive_chain_forward_constant_set(okapi::QLength input) { pid_drive_chain_forward_constant_set(input.convert(okapi::inch)); }
+void Drive::pid_drive_chain_backward_constant_set(okapi::QLength input) { pid_drive_chain_backward_constant_set(input.convert(okapi::inch)); }
+
+// Set turn motion chain constants
+void Drive::pid_turn_chain_constant_set(double input) { turn_motion_chain_scale = fabs(input); }
+void Drive::pid_turn_chain_constant_set(okapi::QAngle input) { pid_turn_chain_constant_set(input.convert(okapi::degree)); }
+
+// Set swing motion chain constants
+void Drive::pid_swing_chain_constant_set(double input) {
+  pid_swing_chain_forward_constant_set(input);
+  pid_swing_chain_backward_constant_set(input);
+}
+void Drive::pid_swing_chain_forward_constant_set(double input) { swing_forward_motion_chain_scale = fabs(input); }
+void Drive::pid_swing_chain_backward_constant_set(double input) { swing_backward_motion_chain_scale = fabs(input); }
+void Drive::pid_swing_chain_constant_set(okapi::QAngle input) { pid_swing_chain_constant_set(input.convert(okapi::degree)); }
+void Drive::pid_swing_chain_forward_constant_set(okapi::QAngle input) { pid_swing_chain_forward_constant_set(input.convert(okapi::degree)); }
+void Drive::pid_swing_chain_backward_constant_set(okapi::QAngle input) { pid_swing_chain_backward_constant_set(input.convert(okapi::degree)); }
+
+// Get motion chain constants
+double Drive::pid_drive_chain_forward_constant_get() { return drive_forward_motion_chain_scale; }
+double Drive::pid_drive_chain_backward_constant_get() { return drive_backward_motion_chain_scale; }
+double Drive::pid_turn_chain_constant_get() { return turn_motion_chain_scale; }
+double Drive::pid_swing_chain_forward_constant_get() { return swing_forward_motion_chain_scale; }
+double Drive::pid_swing_chain_backward_constant_get() { return swing_backward_motion_chain_scale; }
+
+// Pid wait that hold momentum into the next motion
+void Drive::pid_wait_quick_chain() {
+  // If driving, add drive_motion_chain_scale to target
+  if (mode == DRIVE) {
+    double chain_scale = motion_chain_backward ? drive_backward_motion_chain_scale : drive_forward_motion_chain_scale;
+    used_motion_chain_scale = chain_scale * util::sgn(chain_target_start);
+    leftPID.target_set(leftPID.target_get() + used_motion_chain_scale);
+    rightPID.target_set(rightPID.target_get() + used_motion_chain_scale);
+  }
+
+  // If turning, add turn_motion_chain_scale to target
+  else if (mode == TURN) {
+    used_motion_chain_scale = turn_motion_chain_scale * util::sgn(chain_target_start - chain_sensor_start);
+    turnPID.target_set(turnPID.target_get() + used_motion_chain_scale);
+  }
+
+  // If swinging, add swing_motion_chain_scale to target
+  else if (mode == SWING) {
+    double chain_scale = motion_chain_backward ? swing_backward_motion_chain_scale : swing_forward_motion_chain_scale;
+    used_motion_chain_scale = chain_scale * util::sgn(chain_target_start - chain_sensor_start);
+    swingPID.target_set(swingPID.target_get() + used_motion_chain_scale);
+  }
+
+  // If odometrying, add drive_motion_chain_scale to the final target point
+  // It'll be at the angle between the second to last point and the last point
+  else if (mode == POINT_TO_POINT || mode == PURE_PURSUIT) {
+    double chain_scale = current_drive_direction == REV ? drive_backward_motion_chain_scale : drive_forward_motion_chain_scale;
+    used_motion_chain_scale = chain_scale;
+
+    // Figure out what angle to use.
+    // this will either by the angle between second to last point and last point,
+    // or it'll be the boomerang end angle
+    double angle = util::absolute_angle_to_point(odom_target_start, odom_second_to_last);
+    if (odom_target_start.theta != ANGLE_NOT_SET) angle = odom_target_start.theta;
+
+    // Create new point
+    pose target = util::vector_off_point(used_motion_chain_scale, {odom_target_start.x, odom_target_start.y, angle});
+    target.theta = odom_target_start.theta;
+
+    // Replace target in ptp, add new final point if pp
+    if (mode == POINT_TO_POINT)
+      odom_target = target;
+    else
+      pp_movements.push_back({target,
+                              pp_movements[pp_movements.size() - 1].drive_direction,
+                              pp_movements[pp_movements.size() - 1].max_xy_speed});
+
+  } else {
+    printf("Not in a supported drive mode!\n");
+    return;
+  }
+
+  // Exit at the real target
+  pid_wait_quick();
+}

src/EZ-Template/drive/pid_tasks.cpp

@@ -0,0 +1,271 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/drive/drive.hpp"
+#include "EZ-Template/util.hpp"
+#include "pros/misc.hpp"
+
+using namespace ez;
+
+void Drive::ez_auto_task() {
+  while (true) {
+    // Run odom
+    ez_tracking_task();
+
+    // Autonomous PID
+    switch (drive_mode_get()) {
+      case DRIVE:
+        drive_pid_task();
+        break;
+      case TURN ... TURN_TO_POINT:
+        turn_pid_task();
+        break;
+      case SWING:
+        swing_pid_task();
+        break;
+      case POINT_TO_POINT:
+        ptp_task();
+        break;
+      case PURE_PURSUIT:
+        pp_task();
+        break;
+      case DISABLE:
+        break;
+      default:
+        break;
+    }
+
+    // This is used to reset sensors for active braking
+    util::AUTON_RAN = drive_mode_get() != DISABLE ? true : false;
+
+    pros::delay(ez::util::DELAY_TIME);
+  }
+}
+
+// Drive PID task
+void Drive::drive_pid_task() {
+  // Compute PID
+  leftPID.compute(drive_sensor_left());
+  rightPID.compute(drive_sensor_right());
+
+  headingPID.compute(drive_imu_get());
+
+  // Compute slew
+  slew_left.iterate(drive_sensor_left());
+  slew_right.iterate(drive_sensor_right());
+
+  // Left and Right outputs
+  double l_drive_out = leftPID.output;
+  double r_drive_out = rightPID.output;
+
+  // Scale leftPID and rightPID to slew (if slew is disabled, it returns max_speed)
+  double max_slew_out = fmax(slew_left.output(), slew_right.output());
+  double faster_side = fmax(fabs(l_drive_out), fabs(r_drive_out));
+  if (faster_side > max_slew_out) {
+    l_drive_out *= (max_slew_out / faster_side);
+    r_drive_out *= (max_slew_out / faster_side);
+  }
+
+  // Toggle heading
+  double imu_out = heading_on ? headingPID.output : 0;
+
+  // Combine heading and drive
+  double l_out = l_drive_out + imu_out;
+  double r_out = r_drive_out - imu_out;
+
+  // Vector scaling when combining drive and imu
+  max_slew_out = fmax(slew_left.output(), slew_right.output());
+  faster_side = fmax(fabs(l_out), fabs(r_out));
+  if (faster_side > max_slew_out) {
+    l_out *= (max_slew_out / faster_side);
+    r_out *= (max_slew_out / faster_side);
+  }
+
+  // Set motors
+  if (drive_toggle)
+    private_drive_set(l_out, r_out);
+}
+
+// Turn PID task
+void Drive::turn_pid_task() {
+  // Compute PID if it's a normal turn
+  if (mode == TURN) {
+    turnPID.compute(drive_imu_get());
+  }
+  // Compute PID if we're turning to point
+  else {
+    double a_target = util::absolute_angle_to_point(point_to_face[!ptf1_running], odom_pose_get());  // Calculate the point for angle to face
+    a_target = new_turn_target_compute(a_target, odom_imu_start, current_angle_behavior);
+    double error = a_target - odom_theta_get();
+    turnPID.compute_error(error, odom_theta_get());
+  }
+
+  // Compute slew
+  slew_turn.iterate(drive_imu_get());
+
+  // Clip gyroPID to max speed
+  double gyro_out = util::clamp(turnPID.output, slew_turn.output(), -slew_turn.output());
+
+  // Clip the speed of the turn when the robot is within StartI, only do this when target is larger then StartI
+  if (turnPID.constants.ki != 0 && (fabs(turnPID.target_get()) > turnPID.constants.start_i && fabs(turnPID.error) < turnPID.constants.start_i)) {
+    if (pid_turn_min_get() != 0)
+      gyro_out = util::clamp(gyro_out, pid_turn_min_get(), -pid_turn_min_get());
+  }
+
+  // Set motors
+  if (drive_toggle)
+    private_drive_set(gyro_out, -gyro_out);
+}
+
+// Swing PID task
+void Drive::swing_pid_task() {
+  // Compute PID
+  swingPID.compute(drive_imu_get());
+  leftPID.compute(drive_sensor_left());
+  rightPID.compute(drive_sensor_right());
+
+  // Compute slew
+  double current = slew_swing_using_angle ? drive_imu_get() : (current_swing == LEFT_SWING ? drive_sensor_left() : drive_sensor_right());
+  slew_swing.iterate(current);
+
+  // Clip swingPID to max speed
+  double swing_out = util::clamp(swingPID.output, slew_swing.output(), -slew_swing.output());
+
+  // Clip the speed of the turn when the robot is within StartI, only do this when target is larger then StartI
+  if (swingPID.constants.ki != 0 && (fabs(swingPID.target_get()) > swingPID.constants.start_i && fabs(swingPID.error) < swingPID.constants.start_i)) {
+    if (pid_swing_min_get() != 0)
+      swing_out = util::clamp(swing_out, pid_swing_min_get(), -pid_swing_min_get());
+  }
+
+  // Set the motors powers, and decide what to do with the "still" side of the drive
+  double opposite_output = 0;
+  double scale = swing_out / max_speed;
+  if (drive_toggle) {
+    // Check if left or right swing, then set motors accordingly
+    if (current_swing == LEFT_SWING) {
+      opposite_output = swing_opposite_speed == 0 ? rightPID.output : (swing_opposite_speed * scale);
+      private_drive_set(swing_out, opposite_output);
+    } else if (current_swing == RIGHT_SWING) {
+      opposite_output = swing_opposite_speed == 0 ? leftPID.output : -(swing_opposite_speed * scale);
+      private_drive_set(opposite_output, -swing_out);
+    }
+  }
+}
+
+// Odom To Point Task
+void Drive::ptp_task() {
+  // Compute slew
+  slew_left.iterate(drive_sensor_left());
+  slew_right.iterate(drive_sensor_right());
+  double max_slew_out = fmax(slew_left.output(), slew_right.output());
+
+  // Decide if we've past the target or not
+  double temp_target = is_past_target(odom_target, odom_pose_get());        // Use this instead of distance formula to fix impossible movements
+  int dir = (current_drive_direction == REV ? -1 : 1);                      // If we're going backwards, add a -1
+  int flipped = util::sgn(temp_target) != util::sgn(past_target) ? -1 : 1;  // Check if we've flipped directions to what we started
+
+  // Compute xy PID
+  new_current_fake += xy_delta_fake * ((dir * flipped));  // Create a "current sensor value" for the PID to calculate off of
+  xyPID.compute_error(fabs(temp_target) * dir * flipped, new_current_fake);
+
+  // Compute angle
+  pose ptf = point_to_face[!ptf1_running];
+  double a_target = util::absolute_angle_to_point(ptf, odom_pose_get());  // Calculate the point for angle to face
+  a_target = new_turn_target_compute(a_target, odom_imu_start, current_angle_behavior);
+  double wrapped_a_target = a_target - odom_theta_get();
+  current_a_odomPID.compute_error(wrapped_a_target, odom_theta_get());
+  // printf("shortest_a_target: %.2f      error: %.2f\n", a_target, wrapped_a_target);
+
+  // Prioritize turning by scaling xy_out down
+  double xy_out = xyPID.output;
+  xy_out = util::clamp(xy_out, max_slew_out);
+  // double scale = cos(util::to_rad(current_a_odomPID.error)) / odom_turn_bias_amount;
+  double scale = 1.0 - ((1.0 - cos(util::to_rad(current_a_odomPID.error))) / odom_turn_bias_amount);  // 1 - ((1-0.7)/0.75)
+  if (odom_turn_bias_enabled())
+    xy_out *= scale;
+  double a_out = current_a_odomPID.output;
+  // a_out = util::clamp(a_out, max_slew_out);
+
+  // Scale xy_out and a_out to max speed
+  // this ensures no data is lost that would otherwise by lost in clamping
+  double faster_side = fmax(fabs(xy_out), fabs(a_out));
+  if (faster_side > max_slew_out) {
+    xy_out *= (max_slew_out / faster_side);
+    a_out *= (max_slew_out / faster_side);
+  }
+
+  // Combine heading and drive
+  double l_out = xy_out + a_out;
+  double r_out = xy_out - a_out;
+
+  // Vector scaling when combining drive and imu
+  // this ensures no data is lost that would otherwise by lost in clamping
+  faster_side = fmax(fabs(l_out), fabs(r_out));
+  if (faster_side > max_slew_out) {
+    l_out *= (max_slew_out / faster_side);
+    r_out *= (max_slew_out / faster_side);
+  }
+
+  // printf("lr out (%.2f, %.2f)   xy/a(%.2f, %.2f)   lr slew (%.2f, %.2f)\n", l_out, r_out, xy_out, a_out, slew_left.output(), slew_right.output());
+  // printf("max_slew_out %.2f      headingerr: %.2f\n", max_slew_out, aPID.error);
+  // printf("lr(%.2f, %.2f)   xy_raw: %.2f   xy_out: %.2f   heading_out: %.2f      max_slew_out: %.2f\n", l_out, r_out, xyPID.output, xy_out, current_a_odomPID.output, max_slew_out);
+  // printf("xy(%.2f, %.2f, %.2f)   xyPID: %.2f   aPID: %.2f     dir: %i   sgn: %i   past_target: %i    is_past_target: %i   is_past_using_xy: %i      fake_xy(%.2f, %.2f, %.2f)\n", odom_x_get(), odom_y_get(), odom_theta_get(), xyPID.target_get(), current_a_odomPID.target_get(), dir, flipped, past_target, (int)is_past_target(odom_target, odom_pose_get()), is_past_target_using_xy, fake_x, fake_y, util::to_deg(fake_angle));
+  // printf("xy(%.2f, %.2f, %.2f)   xyPID: %.2f   aPID: %.2f   ptf:(%.2f, %.2f)   xy/a(%.2f, %.2f)   lr(%.2f, %.2f)   fake xy: %.2f\n", odom_x_get(), odom_y_get(), odom_theta_get(), xyPID.error, current_a_odomPID.error, ptf.x, ptf.y, xy_out, a_out, l_out, r_out, new_current_fake);
+
+  // Set motors
+  if (drive_toggle)
+    private_drive_set(l_out, r_out);
+
+  // This is for wait_until
+  leftPID.compute(drive_sensor_left());
+  rightPID.compute(drive_sensor_right());
+}
+
+void Drive::boomerang_task() {
+  int target_index = pp_index;
+  pose target = pp_movements[target_index].target;
+
+  // target.theta += current_drive_direction == REV ? 180 : 0;  // Decide if going fwd or rev
+  int dir = current_drive_direction == REV ? -1 : 1;
+
+  double h = util::distance_to_point(target, odom_pose_get()) * odom_boomerang_dlead_get();
+  double max = max_boomerang_distance;
+  h = h > max ? max : h;
+  h *= dir;
+
+  pose temp = util::vector_off_point(-h, pp_movements[target_index].target);
+  temp.theta = target.theta;
+
+  if (util::distance_to_point(target, odom_pose_get()) < odom_look_ahead_get() / 2.0) {
+    temp = target;
+  }
+
+  if (odom_target.x != temp.x || odom_target.y != temp.y) {
+    bool slew_on = slew_left.enabled() || slew_right.enabled() ? true : false;
+    raw_pid_odom_ptp_set({temp, pp_movements[target_index].drive_direction, pp_movements[target_index].max_xy_speed}, slew_on);
+  }
+
+  // printf("cur(%.2f, %.2f, %.2f)   tar(%.2f, %.2f, %.2f)   h %.2f  \n", odom_x_get(), odom_y_get(), odom_theta_get(), temp.x, temp.y, temp.theta, h);
+
+  ptp_task();
+}
+
+void Drive::pp_task() {
+  if (fabs(util::distance_to_point(pp_movements[pp_index].target, odom_pose_get())) < odom_look_ahead_get()) {
+    if (pp_index < pp_movements.size() - 1) {
+      pp_index = pp_index >= pp_movements.size() - 1 ? pp_index : pp_index + 1;
+      bool slew_on = slew_left.enabled() || slew_right.enabled() ? true : false;
+      if (!current_slew_on) slew_on = false;
+      raw_pid_odom_ptp_set(pp_movements[pp_index], slew_on);
+    }
+  }
+
+  if (pp_movements[pp_index].target.theta != ANGLE_NOT_SET) {
+    boomerang_task();
+  } else {
+    ptp_task();
+  }
+}

src/EZ-Template/drive/pid_tuner.cpp

@@ -0,0 +1,212 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/drive/drive.hpp"
+#include "EZ-Template/sdcard.hpp"
+#include "EZ-Template/util.hpp"
+#include "liblvgl/llemu.hpp"
+#include "pros/llemu.hpp"
+#include "pros/misc.h"
+
+// Is the PID Tuner enabled?
+bool Drive::pid_tuner_enabled() { return pid_tuner_on; }
+
+// Enable/disabling the full PID tuner
+void Drive::pid_tuner_full_enable(bool enable) { is_full_pid_tuner_enabled = enable; }
+bool Drive::pid_tuner_full_enabled() { return is_full_pid_tuner_enabled; }
+
+// Toggle printing to terminal
+void Drive::pid_tuner_print_terminal_set(bool input) { pid_tuner_terminal_b = input; }
+bool Drive::pid_tuner_print_terminal_enabled() { return pid_tuner_terminal_b; }
+
+// Initialize the brain screen
+void Drive::pid_tuner_brain_init() {
+  last_auton_selector_state = ez::as::enabled();
+  // Shut off auton selector
+  ez::as::shutdown();
+  pros::lcd::initialize();
+}
+
+// Toggle printing to brain
+void Drive::pid_tuner_print_brain_set(bool input) {
+  if (pid_tuner_on && input != pid_tuner_lcd_b) {
+    if (!pid_tuner_lcd_b) {
+      pid_tuner_lcd_b = input;
+      pid_tuner_brain_init();
+      pid_tuner_print_brain();
+    } else if (pid_tuner_lcd_b) {
+      pid_tuner_lcd_b = input;
+      if (last_auton_selector_state) {
+        ez::as::initialize();
+      } else {
+        pros::lcd::shutdown();
+      }
+    }
+  }
+}
+bool Drive::pid_tuner_print_brain_enabled() { return pid_tuner_lcd_b; }
+
+// Enable PID Tuner
+void Drive::pid_tuner_enable() {
+  pid_tuner_brain_init();
+
+  if (pid_tuner_full_enabled())
+    used_pid_tuner_pids = &pid_tuner_full_pids;
+  else
+    used_pid_tuner_pids = &pid_tuner_pids;
+
+  // Keep track of the last state of this so we can set it back once PID Tuner is disables
+  last_controller_curve_state = opcontrol_curve_buttons_toggle_get();
+  opcontrol_curve_buttons_toggle(false);
+  pid_tuner_on = true;
+
+  pid_tuner_print();
+}
+
+// Disable PID Tuner
+void Drive::pid_tuner_disable() {
+  pid_tuner_on = false;
+  opcontrol_curve_buttons_toggle(last_controller_curve_state);
+  if (last_auton_selector_state) {
+    ez::as::initialize();
+  } else {
+    pros::lcd::shutdown();
+  }
+}
+
+// Toggle PID Tuner
+void Drive::pid_tuner_toggle() {
+  pid_tuner_on = !pid_tuner_on;
+  if (pid_tuner_on)
+    pid_tuner_enable();
+  else
+    pid_tuner_disable();
+}
+
+// Print PID Tuner
+void Drive::pid_tuner_print() {
+  if (!pid_tuner_on) return;
+
+  double kp = used_pid_tuner_pids->at(column).consts->kp;
+  double ki = used_pid_tuner_pids->at(column).consts->ki;
+  double kd = used_pid_tuner_pids->at(column).consts->kd;
+  double starti = used_pid_tuner_pids->at(column).consts->start_i;
+
+  std::string sname = used_pid_tuner_pids->at(column).name + "\n";
+  std::string skp = "kp: " + util::to_string_with_precision(kp, util::places_after_decimal(kp, 2));
+  std::string ski = "ki: " + util::to_string_with_precision(ki, util::places_after_decimal(ki, 2));
+  std::string skd = "kd: " + util::to_string_with_precision(kd, util::places_after_decimal(kd, 2));
+  std::string sstarti = "start i: " + util::to_string_with_precision(starti, util::places_after_decimal(starti, 2));
+
+  skp = row == 0 ? skp + arrow : skp + "\n";
+  ski = row == 1 ? ski + arrow : ski + "\n";
+  skd = row == 2 ? skd + arrow : skd + "\n";
+  sstarti = row == 3 ? sstarti + arrow : sstarti + "\n";
+
+  complete_pid_tuner_output = sname + "\n" + skp + ski + skd + sstarti + "\n";
+
+  pid_tuner_print_brain();
+  pid_tuner_print_terminal();
+}
+
+// Print tuner to brain if it's enabled
+void Drive::pid_tuner_print_brain() {
+  if (pid_tuner_lcd_b) ez::screen_print(complete_pid_tuner_output);
+}
+
+// Print tuner to terminal if it's enabled
+void Drive::pid_tuner_print_terminal() {
+  if (pid_tuner_terminal_b) std::cout << complete_pid_tuner_output;
+}
+
+// Modify constants
+void Drive::pid_tuner_value_modify(float p, float i, float d, float start) {
+  if (!pid_tuner_on) return;
+
+  switch (row) {
+    case 0:
+      used_pid_tuner_pids->at(column).consts->kp += p;
+      if (used_pid_tuner_pids->at(column).consts->kp < 0.0)
+        used_pid_tuner_pids->at(column).consts->kp = 0.0;
+      break;
+    case 1:
+      used_pid_tuner_pids->at(column).consts->ki += i;
+      if (used_pid_tuner_pids->at(column).consts->ki < 0.0)
+        used_pid_tuner_pids->at(column).consts->ki = 0.0;
+      break;
+    case 2:
+      used_pid_tuner_pids->at(column).consts->kd += d;
+      if (used_pid_tuner_pids->at(column).consts->kd < 0.0)
+        used_pid_tuner_pids->at(column).consts->kd = 0.0;
+      break;
+    case 3:
+      used_pid_tuner_pids->at(column).consts->start_i += start;
+      if (used_pid_tuner_pids->at(column).consts->start_i < 0.0)
+        used_pid_tuner_pids->at(column).consts->start_i = 0.0;
+      break;
+    default:
+      break;
+  }
+}
+void Drive::pid_tuner_value_increase() { pid_tuner_value_modify(p_increment, i_increment, d_increment, start_i_increment); }
+void Drive::pid_tuner_value_decrease() { pid_tuner_value_modify(-p_increment, -i_increment, -d_increment, -start_i_increment); }
+
+// Set and Get increments for modifying increments
+void Drive::pid_tuner_increment_p_set(double p) { p_increment = fabs(p); }
+void Drive::pid_tuner_increment_i_set(double i) { i_increment = fabs(i); }
+void Drive::pid_tuner_increment_d_set(double d) { d_increment = fabs(d); }
+void Drive::pid_tuner_increment_start_i_set(double start_i) { start_i_increment = fabs(start_i); }
+double Drive::pid_tuner_increment_p_get() { return p_increment; }
+double Drive::pid_tuner_increment_i_get() { return i_increment; }
+double Drive::pid_tuner_increment_d_get() { return d_increment; }
+double Drive::pid_tuner_increment_start_i_get() { return start_i_increment; }
+
+// Iterate
+void Drive::pid_tuner_iterate() {
+  if (!pid_tuner_on) return;  // Exit if it's disabled
+
+  // Exit if printing to terminal or brain are disabled
+  if (!pid_tuner_terminal_b && !pid_tuner_lcd_b) {
+    pid_tuner_disable();
+    printf("Cannot run PID Tuner without printing to Brain or Terminal!\n");
+    return;
+  }
+
+  // Up / Down for Rows
+  if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_RIGHT)) {
+    column++;
+    if (column > used_pid_tuner_pids->size() - 1)
+      column = 0;
+    pid_tuner_print();
+  } else if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_LEFT)) {
+    column--;
+    if (column < 0)
+      column = used_pid_tuner_pids->size() - 1;
+    pid_tuner_print();
+  }
+
+  // Left / Right for Columns
+  if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_DOWN)) {
+    row++;
+    if (row > 3)
+      row = 0;
+    pid_tuner_print();
+  } else if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_UP)) {
+    row--;
+    if (row < 0)
+      row = 3;
+    pid_tuner_print();
+  }
+
+  // Increase / Decrease constant
+  if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_A)) {
+    pid_tuner_value_increase();
+    pid_tuner_print();
+  } else if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_Y)) {
+    pid_tuner_value_decrease();
+    pid_tuner_print();
+  }
+}

src/EZ-Template/drive/pto.cpp

@@ -0,0 +1,53 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include <algorithm>
+#include <vector>
+
+#include "EZ-Template/drive/drive.hpp"
+
+bool Drive::pto_check(pros::Motor check_if_pto) {
+  auto does_exist = std::find(pto_active.begin(), pto_active.end(), check_if_pto.get_port());
+  if (does_exist != pto_active.end())
+    return true;  // Motor is in the list
+  return false;   // Motor isn't in the list
+}
+
+void Drive::pto_add(std::vector<pros::Motor> pto_list) {
+  for (auto i : pto_list) {
+    // Return if the motor is already in the list
+    if (pto_check(i)) return;
+
+    // Return if the first index was used (this motor is used for velocity)
+    if (i.get_port() == left_motors[0].get_port() || i.get_port() == right_motors[0].get_port()) {
+      printf("You cannot PTO the first index!\n");
+      return;
+    }
+
+    pto_active.push_back(i.get_port());
+  }
+}
+
+void Drive::pto_remove(std::vector<pros::Motor> pto_list) {
+  for (auto i : pto_list) {
+    auto does_exist = std::find(pto_active.begin(), pto_active.end(), i.get_port());
+    // Return if the motor isn't in the list
+    if (does_exist == pto_active.end()) return;
+
+    // Find index of motor
+    int index = std::distance(pto_active.begin(), does_exist);
+    pto_active.erase(pto_active.begin() + index);
+    i.set_brake_mode(CURRENT_BRAKE);  // Set the motor to the brake type of the drive
+    i.set_current_limit(CURRENT_MA);  // Set the motor to the mA of the drive
+  }
+}
+
+void Drive::pto_toggle(std::vector<pros::Motor> pto_list, bool toggle) {
+  if (toggle)
+    pto_add(pto_list);
+  else
+    pto_remove(pto_list);
+}

src/EZ-Template/drive/purepursuit_math.cpp

@@ -0,0 +1,359 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/api.hpp"
+#include "EZ-Template/util.hpp"
+
+// Returns a distance that the robot is away from target, but this keeps sign.
+double Drive::is_past_target(pose target, pose current) {
+  // Translated current x, y translated around origin
+  double fakek_y = (current.y - target.y);
+  double fakek_x = (current.x - target.x);
+
+  // Angle to face translated around origin
+  pose ptf;
+  ptf.y = point_to_face[!ptf1_running].y - target.y;
+  ptf.x = point_to_face[!ptf1_running].x - target.x;
+  int add = current_drive_direction == REV ? 180 : 0;
+  double fake_angle = util::to_rad((util::absolute_angle_to_point(ptf, {fakek_x, fakek_y})) + add);
+
+  // Rotate around origin
+  double fake_x = (fakek_x * cos(fake_angle)) - (fakek_y * sin(fake_angle));
+  double fake_y = (fakek_y * cos(fake_angle)) + (fakek_x * sin(fake_angle));
+
+  return fake_y;
+}
+
+// Find the angle to face during movements
+std::vector<pose> Drive::find_point_to_face(pose current, pose target, drive_directions dir, bool set_global) {
+  // rotate target around current 180deg if the robot wants to be reversed
+  if (dir == rev) {
+    pose new_target = target;
+    // Translate to origin
+    new_target.x -= current.x;
+    new_target.y -= current.y;
+
+    // Rotate 180
+    new_target.x *= -1;
+    new_target.y *= -1;
+
+    // Translate back to current
+    new_target.x += current.x;
+    new_target.y += current.y;
+
+    target = new_target;
+  }
+
+  double tx_cx = target.x - current.x;
+  double m = 0.0;
+  double angle = 0.0;
+  if (tx_cx != 0) {
+    m = (target.y - current.y) / tx_cx;
+    angle = 90.0 - util::to_deg(atan(m));
+  }
+  pose ptf1 = util::vector_off_point(odom_look_ahead_get(), {target.x, target.y, angle});
+  pose ptf2 = util::vector_off_point(-odom_look_ahead_get(), {target.x, target.y, angle});
+
+  if (set_global) {
+    double ptf1_dist = util::distance_to_point(ptf1, current);
+    double ptf2_dist = util::distance_to_point(ptf2, current);
+    if (ptf1_dist > ptf2_dist) {
+      ptf1_running = true;
+    } else {
+      ptf1_running = false;
+    }
+  }
+  // printf("\n");
+  point_to_face = {ptf1, ptf2};
+
+  // printf("pft1(%.2f, %.2f, %.2f)   ptf2(%.2f, %.2f, %.2f)      angle: %.2f   y2-y1: %.2f   x2-x1: %.2f\n", point_to_face[0].x, point_to_face[0].y, point_to_face[0].theta, point_to_face[1].x, point_to_face[1].y, point_to_face[1].theta, angle, (target.y - current.y), tx_cx);
+
+  return {ptf1, ptf2};
+}
+
+// Inject point based on https://www.chiefdelphi.com/t/paper-implementation-of-the-adaptive-pure-pursuit-controller/166552
+std::vector<odom> Drive::inject_points(std::vector<ez::odom> imovements) {
+  injected_pp_index.clear();
+  bool first_point_added = false;
+
+  // Create new vector that includes the starting point
+  std::vector<odom> input = imovements;
+  input.insert(input.begin(), {{{odom_x_get(), odom_y_get(), ANGLE_NOT_SET}, imovements[0].drive_direction, imovements[0].max_xy_speed}});
+
+  // Inject new parent points for boomerang
+  int t = 0;
+  for (int i = 0; i < input.size() - 1; i++) {
+    int j = i + t;
+    j = i;
+    if (input[j].target.theta != ANGLE_NOT_SET) {
+      // Calculate the new point with known information: hypot and angle
+      double angle_to_point = input[j].target.theta;
+      int dir = input[j].drive_direction == REV ? -1 : 1;
+      pose new_point = util::vector_off_point(odom_look_ahead_get() * dir, {input[j].target.x, input[j].target.y, angle_to_point});
+      new_point.theta = ANGLE_NOT_SET;
+
+      input.insert(input.cbegin() + j + 1, {new_point, input[j].drive_direction, input[j].max_xy_speed});
+
+      t++;
+    }
+  }
+
+  // Shift all the turn behaviors 1 parent point down
+  for (int i = 0; i < input.size() - 1; i++) {
+    input[i].turn_behavior = input[i + 1].turn_behavior;
+  }
+  input.back().turn_behavior = raw;
+
+  std::vector<odom> output;  // Output vector
+  int output_index = -1;     // Keeps track of current index
+  injected_pp_index.push_back(0);
+
+  bool allow_injecting = false;  // Flag to disable injecting for the first few points
+
+  // This for loop runs for how many points there are minus one because there is one less vector then points
+  for (int i = 0; i < input.size() - 1; i++) {
+    // Figure out how many points fit in the vector
+    int num_of_points_that_fit = (util::distance_to_point(input[i + 1].target, input[i].target)) / SPACING;
+
+    // Add parent point
+    // Make sure the robot is looking at next point
+    output.push_back({{input[i].target.x, input[i].target.y, input[i].target.theta},
+                      input[i].drive_direction,
+                      input[i].max_xy_speed,
+                      input[i].turn_behavior});
+    output_index++;
+
+    // don't let the injected point after boomerang in
+    if (i != 0 && input[i - 1].target.theta == ANGLE_NOT_SET) {
+      injected_pp_index.push_back(output_index);
+    }
+
+    // Add the injected points
+    for (int j = 0; j < num_of_points_that_fit; j++) {
+      if (input[i + 1].target.theta == ANGLE_NOT_SET) {
+        // Calculate the new point with known information: hypot and angle
+        double angle_to_point = util::absolute_angle_to_point(input[i + 1].target, input[i].target);
+        pose new_point = util::vector_off_point(SPACING * (j + 1), {input[i].target.x, input[i].target.y, angle_to_point});
+
+        // A one time flag to stop points from being injected for LOOK_AHEAD from current
+        // https://github.com/EZ-Robotics/EZ-Template/issues/152
+        if (util::distance_to_point(new_point, input[0].target) >= odom_look_ahead_get())
+          allow_injecting = true;
+
+        // If the new point is basically the same as the parent point, remove it to save 10ms delay
+        if (util::distance_to_point(new_point, input[i + 1].target) >= SPACING && allow_injecting) {
+          // Update the first target point with the real desired turn behavior
+          e_angle_behavior new_turn_behavior = raw;
+          if (!first_point_added) {
+            new_turn_behavior = input[0].turn_behavior;
+            first_point_added = true;
+          }
+
+          // Push new point to vector
+          output.push_back({{new_point.x, new_point.y, ANGLE_NOT_SET},
+                            input[i + 1].drive_direction,
+                            input[i + 1].max_xy_speed,
+                            new_turn_behavior});  // Setting this to raw will maintain the parent points turn behavior
+          output_index++;
+        }
+      } else {
+        j = num_of_points_that_fit;
+      }
+    }
+  }
+  output.push_back(input.back());
+  output_index++;
+
+  injected_pp_index.push_back(output_index);
+
+  // Return final vector
+  return output;
+}
+
+// Path smoothing based on https://medium.com/@jaems33/understanding-robot-motion-path-smoothing-5970c8363bc4
+std::vector<odom> Drive::smooth_path(std::vector<odom> ipath, double weight_smooth, double weight_data, double tolerance) {
+  double path[500][3];
+  double new_path[500][3];
+  std::vector<bool> dont_touch;
+  int t = 0;
+  bool allow_injecting = false;
+  bool boomerang_after_not_done = false;
+
+  // Convert odom to array
+  for (int i = 0; i < ipath.size(); i++) {
+    path[i][0] = new_path[i][0] = ipath[i].target.x;
+    path[i][1] = new_path[i][1] = ipath[i].target.y;
+    path[i][2] = new_path[i][2] = ipath[i].target.theta;
+
+    bool dont_touch_this_point = false;
+
+    // A one time flag to stop points from being smoothed for LOOK_AHEAD from current
+    // https://github.com/EZ-Robotics/EZ-Template/issues/152
+    if (util::distance_to_point(ipath[i].target, ipath[0].target) > odom_look_ahead_get() && !allow_injecting)
+      allow_injecting = true;
+
+    // if (t <= odom_look_ahead_get() / SPACING && (prev_point_angle_not_set || t != 0)))
+    if (boomerang_after_not_done) {
+      t++;
+      dont_touch_this_point = true;
+      if (t >= odom_look_ahead_get() / SPACING) {
+        t = 0;
+        boomerang_after_not_done = false;
+      }
+    }
+
+    // Don't touch that extends after boomerang, or that are super close to start
+    if (ipath[i].target.theta != ANGLE_NOT_SET || !allow_injecting || i == 1) {
+      dont_touch_this_point = true;
+    }
+
+    dont_touch.push_back(dont_touch_this_point);
+
+    if (ipath[i].target.theta != ANGLE_NOT_SET) {
+      boomerang_after_not_done = true;
+      t++;
+    }
+  }
+
+  double change = tolerance;
+
+  while (change >= tolerance) {
+    change = 0.0;
+    for (int i = 1; i < ipath.size() - 2; i++) {
+      // if (path[i][2] == ANGLE_NOT_SET) {
+      if (!dont_touch[i]) {
+        for (int j = 0; j < 2; j++) {
+          double x_i = path[i][j];
+          double y_i = new_path[i][j];
+          double y_prev = new_path[i - 1][j];
+          double y_next = new_path[i + 1][j];
+
+          double y_i_saved = y_i;
+          y_i += weight_data * (x_i - y_i) + weight_smooth * (y_next + y_prev - (2.0 * y_i));
+          new_path[i][j] = y_i;
+
+          change += abs(y_i - y_i_saved);
+        }
+      }
+    }
+  }
+
+  // Convert array to odom
+  std::vector<odom> output = ipath;  // Set output to input so target angles, turn types and speed hold
+  // Overwrite x and y
+  for (int i = 0; i < ipath.size(); i++) {
+    output[i].target.x = new_path[i][0];
+    output[i].target.y = new_path[i][1];
+  }
+
+  return output;
+}
+
+// Outputs the shortest angle to where you want to go, but tolerances it
+// to bias one way
+double Drive::turn_short(double target, double current, bool print) {
+  if (print) printf("SHORTEST   Target: %.2f   Current: %.2f      New Target: ", target, current);
+  double shortest = util::turn_shortest(target, current, false);
+  double longest = util::turn_longest(target, current, false);
+  double output = turn_is_toleranced(target, current, shortest, longest, shortest);
+  if (print) printf("%.2f\n", output);
+  return output;
+}
+
+// Outputs the shortest angle to where you want to go, but tolerances it
+// to bias one way
+double Drive::turn_long(double target, double current, bool print) {
+  if (print) printf("LONGEST   Target: %.2f   Current: %.2f      New Target: ", target, current);
+  double longest = util::turn_longest(target, current, false);
+  double shortest = util::turn_shortest(target, current, false);
+  double output = turn_is_toleranced(target, current, longest, longest, shortest);
+  if (print) printf("%.2f\n", output);
+  return output;
+}
+
+// This figures out the tolerances
+double Drive::turn_is_toleranced(double target, double current, double input, double longest, double shortest) {
+  double output = input;
+
+  double long_error = longest - current;
+  double short_error = shortest - current;
+
+  if (fabs(long_error) - fabs(short_error) >= turn_tolerance * 2.0)
+    return output;
+
+  int long_error_sgn = util::sgn(long_error);
+  int short_error_sgn = util::sgn(short_error);
+
+  if (turn_biased_left)
+    output = long_error_sgn == -1 ? longest : shortest;
+  else
+    output = long_error_sgn == 1 ? longest : shortest;
+
+  return output;
+}
+
+// Always turn left
+double Drive::turn_left(double target, double current, bool print) {
+  if (print) printf("LEFT   Target: %.2f   Current: %.2f      New Target: ", target, current);
+  double shortest = util::turn_shortest(target, current, false);
+  double output = shortest;
+  if (util::sgn(shortest - current) == -1) {
+    output = shortest;
+
+    return output;
+  }
+
+  double longest = util::turn_longest(target, current, false);
+  output = longest;
+
+  if (print) printf("%.2f\n", output);
+  return output;
+}
+
+// Always turn right
+double Drive::turn_right(double target, double current, bool print) {
+  if (print) printf("LEFT   Target: %.2f   Current: %.2f      New Target: ", target, current);
+  double shortest = util::turn_shortest(target, current, false);
+  double output = shortest;
+  if (util::sgn(shortest - current) == 1) {
+    output = shortest;
+
+    return output;
+  }
+
+  double longest = util::turn_longest(target, current, false);
+  output = longest;
+
+  if (print) printf("%.2f\n", output);
+  return output;
+}
+
+// This outputs a new target depending on the turn behavior.  this is used throughout ez-template
+// as a "one stop shop" for new turn targets
+double Drive::new_turn_target_compute(double target, double current, ez::e_angle_behavior behavior) {
+  double new_target = 0.0;
+  switch (behavior) {
+    case raw:
+      new_target = target;
+      break;
+    case cw:
+      new_target = turn_right(target, current);
+      break;
+    case ccw:
+      new_target = turn_left(target, current);
+      break;
+    case shortest:
+      new_target = turn_short(target, current);
+      break;
+    case longest:
+      new_target = turn_long(target, current);
+      break;
+    default:
+      new_target = target;
+      break;
+  }
+  return new_target;
+}

src/EZ-Template/drive/set_pid/set_drive_pid.cpp

@@ -0,0 +1,163 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/api.hpp"
+#include "okapi/api/units/QAngle.hpp"
+
+/////
+// Sets swing constants
+/////
+
+// Slew constants
+void Drive::slew_drive_constants_forward_set(okapi::QLength distance, int min_speed) {
+  double dist = distance.convert(okapi::inch);
+  slew_forward.constants_set(dist, min_speed);
+}
+void Drive::slew_drive_constants_backward_set(okapi::QLength distance, int min_speed) {
+  double dist = distance.convert(okapi::inch);
+  slew_backward.constants_set(dist, min_speed);
+}
+void Drive::slew_drive_constants_set(okapi::QLength distance, int min_speed) {
+  slew_drive_constants_backward_set(distance, min_speed);
+  slew_drive_constants_forward_set(distance, min_speed);
+}
+// Global enables for drive slew
+void Drive::slew_drive_set(bool slew_on) {
+  global_forward_drive_slew_enabled = slew_on;
+  global_backward_drive_slew_enabled = slew_on;
+}
+void Drive::slew_drive_forward_set(bool slew_on) { global_forward_drive_slew_enabled = slew_on; }
+bool Drive::slew_drive_forward_get() { return global_forward_drive_slew_enabled; }
+void Drive::slew_drive_backward_set(bool slew_on) { global_backward_drive_slew_enabled = slew_on; }
+bool Drive::slew_drive_backward_get() { return global_backward_drive_slew_enabled; }
+
+// PID Constants
+void Drive::pid_drive_constants_set(double p, double i, double d, double p_start_i) {
+  pid_drive_constants_forward_set(0.0, 0.0, 0.0, 0.0);
+  pid_drive_constants_backward_set(0.0, 0.0, 0.0, 0.0);
+  fwd_rev_drivePID.constants_set(p, i, d, p_start_i);
+}
+void Drive::pid_drive_constants_forward_set(double p, double i, double d, double p_start_i) {
+  forward_drivePID.constants_set(p, i, d, p_start_i);
+}
+void Drive::pid_drive_constants_backward_set(double p, double i, double d, double p_start_i) {
+  backward_drivePID.constants_set(p, i, d, p_start_i);
+}
+void Drive::pid_heading_constants_set(double p, double i, double d, double p_start_i) {
+  headingPID.constants_set(p, i, d, p_start_i);
+}
+void Drive::drive_angle_set(double angle) {
+  headingPID.target_set(angle);
+  drive_imu_reset(angle);
+  central_pose.theta = angle;
+  l_pose.theta = angle;
+  r_pose.theta = angle;
+  was_odom_just_set = true;
+}
+void Drive::drive_angle_set(okapi::QAngle p_angle) {
+  double angle = p_angle.convert(okapi::degree);  // Convert okapi unit to degree
+  drive_angle_set(angle);
+}
+PID::Constants Drive::pid_heading_constants_get() { return headingPID.constants_get(); }
+PID::Constants Drive::pid_drive_constants_backward_get() { return backward_drivePID.constants_get(); }
+PID::Constants Drive::pid_drive_constants_forward_get() { return forward_drivePID.constants_get(); }
+PID::Constants Drive::pid_drive_constants_get() {
+  auto fwd_const = pid_drive_constants_forward_get();
+  auto rev_const = pid_drive_constants_backward_get();
+  if (!(fwd_const.kp == rev_const.kp && fwd_const.ki == rev_const.ki && fwd_const.kd == rev_const.kd && fwd_const.start_i == rev_const.start_i)) {
+    printf("\nForward and Reverse constants are not the same!");
+    return {-1, -1, -1, -1};
+  }
+  return fwd_const;
+}
+
+/////
+// Set drive PID
+/////
+
+// Set pid using global slew
+void Drive::pid_drive_set(double target, int speed) {
+  bool slew_on = util::sgn(target) >= 0 ? slew_drive_forward_get() : slew_drive_backward_get();
+  pid_drive_set(target, speed, slew_on);
+}
+
+// Set drive PID
+void Drive::pid_drive_set(okapi::QLength p_target, int speed, bool slew_on, bool toggle_heading) {
+  double target = p_target.convert(okapi::inch);  // Convert okapi unit to inches
+  pid_drive_set(target, speed, slew_on, toggle_heading);
+}
+
+// Set drive PID with global slew and okapi units
+void Drive::pid_drive_set(okapi::QLength p_target, int speed) {
+  double target = p_target.convert(okapi::inch);  // Convert okapi unit to inches
+  pid_drive_set(target, speed);
+}
+
+// Set drive PID raw
+void Drive::pid_drive_set(double target, int speed, bool slew_on, bool toggle_heading) {
+  leftPID.timers_reset();
+  rightPID.timers_reset();
+
+  // Print targets
+  if (print_toggle) printf("Drive Started... Target Value: %.2f", target);
+  if (slew_on && print_toggle) printf(" with slew");
+  if (print_toggle) printf("\n");
+  chain_target_start = target;
+  chain_sensor_start = drive_sensor_left();
+  used_motion_chain_scale = 0.0;
+
+  // Global setup
+  pid_speed_max_set(speed);
+  heading_on = toggle_heading;
+  l_start = drive_sensor_left();
+  r_start = drive_sensor_right();
+
+  double l_target_encoder, r_target_encoder;
+
+  // Figure actual target value
+  l_target_encoder = l_start + target;
+  r_target_encoder = r_start + target;
+
+  PID *new_drive_pid;
+  slew::Constants slew_consts;
+
+  // Figure out if going forward or backward and set constants accordingly
+  if (l_target_encoder < l_start && r_target_encoder < r_start) {
+    new_drive_pid = &backward_drivePID;
+    slew_consts = slew_backward.constants_get();
+    motion_chain_backward = true;
+  } else {
+    new_drive_pid = &forward_drivePID;
+    slew_consts = slew_forward.constants_get();
+    motion_chain_backward = false;
+  }
+
+  // Prioritize custom fwd/rev constants.  Otherwise, use the same for fwd and rev
+  if (fwd_rev_drivePID.constants_set_check() && !new_drive_pid->constants_set_check())
+    new_drive_pid = &fwd_rev_drivePID;
+
+  PID::Constants pid_drive_consts = new_drive_pid->constants_get();
+  leftPID.constants_set(pid_drive_consts.kp, pid_drive_consts.ki, pid_drive_consts.kd, pid_drive_consts.start_i);
+  rightPID.constants_set(pid_drive_consts.kp, pid_drive_consts.ki, pid_drive_consts.kd, pid_drive_consts.start_i);
+  slew_left.constants_set(slew_consts.distance_to_travel, slew_consts.min_speed);
+  slew_right.constants_set(slew_consts.distance_to_travel, slew_consts.min_speed);
+
+  // Set PID targets
+  leftPID.target_set(l_target_encoder);
+  rightPID.target_set(r_target_encoder);
+
+  // Initialize slew
+  slew_left.initialize(slew_on, max_speed, l_target_encoder, drive_sensor_left());
+  slew_right.initialize(slew_on, max_speed, r_target_encoder, drive_sensor_right());
+  current_slew_on = slew_on;
+
+  // Make sure we're using normal PID
+  leftPID.exit = internal_leftPID.exit;
+  rightPID.exit = internal_rightPID.exit;
+
+  // Run task
+  drive_mode_set(DRIVE);
+}

src/EZ-Template/drive/set_pid/set_odom_pid.cpp

@@ -0,0 +1,494 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/drive/drive.hpp"
+#include "okapi/api/units/QAngle.hpp"
+
+/////
+// Set constants
+/////
+void Drive::odom_path_print() {
+  for (int i = 0; i < pp_movements.size(); i++) {
+    printf("Point %i: (%.2f, %.2f, %.2f)\n", i, pp_movements[i].target.x, pp_movements[i].target.y, pp_movements[i].target.theta);
+  }
+}
+void Drive::pid_odom_behavior_set(ez::e_angle_behavior behavior) { default_odom_type = behavior; }
+ez::e_angle_behavior Drive::pid_odom_behavior_get() { return default_odom_type; }
+// Flip all inputs so it works internally
+pose Drive::flip_pose(pose input) {
+  int flip_x = x_flipped ? -1 : 1;
+  int flip_y = y_flipped ? -1 : 1;
+  int flip_a = theta_flipped ? -1 : 1;
+
+  pose new_pose = input;
+  new_pose.x *= flip_x;
+  new_pose.y *= flip_y;
+  if (new_pose.theta != ANGLE_NOT_SET)
+    new_pose.theta = flip_angle_target(new_pose.theta);
+
+  return new_pose;
+}
+std::vector<odom> Drive::set_odoms_direction(std::vector<odom> inputs) {
+  std::vector<odom> output;
+
+  for (int i = 0; i < inputs.size(); i++) {
+    pose new_pose = flip_pose(inputs[i].target);
+    output.push_back({new_pose,
+                      inputs[i].drive_direction,
+                      inputs[i].max_xy_speed,
+                      inputs[i].turn_behavior});
+  }
+
+  return output;
+}
+odom Drive::set_odom_direction(odom input) {
+  return set_odoms_direction({input})[0];
+}
+void Drive::pid_odom_angular_constants_set(double p, double i, double d, double p_start_i) {
+  odom_angularPID.constants_set(p, i, d, p_start_i);
+}
+void Drive::pid_odom_boomerang_constants_set(double p, double i, double d, double p_start_i) {
+  boomerangPID.constants_set(p, i, d, p_start_i);
+}
+
+void Drive::odom_path_smooth_constants_set(double weight_smooth, double weight_data, double tolerance) {
+  odom_smooth_weight_smooth = weight_smooth;
+  odom_smooth_weight_data = weight_data;
+  odom_smooth_tolerance = tolerance;
+}
+std::vector<double> Drive::odom_path_smooth_constants_get() {
+  return {odom_smooth_weight_smooth, odom_smooth_weight_data, odom_smooth_tolerance};
+}
+
+void Drive::odom_x_flip(bool flip) { x_flipped = flip; }
+bool Drive::odom_x_direction_get() { return x_flipped; }
+void Drive::odom_y_flip(bool flip) { y_flipped = flip; }
+bool Drive::odom_y_direction_get() { return y_flipped; }
+void Drive::odom_theta_flip(bool flip) { theta_flipped = flip; }
+bool Drive::odom_theta_direction_get() { return theta_flipped; }
+void Drive::odom_boomerang_dlead_set(double input) { dlead = input; }
+double Drive::odom_boomerang_dlead_get() { return dlead; }
+void Drive::odom_boomerang_distance_set(double distance) { max_boomerang_distance = distance; }
+void Drive::odom_boomerang_distance_set(okapi::QLength p_distance) { odom_boomerang_distance_set(p_distance.convert(okapi::inch)); }
+double Drive::odom_boomerang_distance_get() { return max_boomerang_distance; }
+void Drive::odom_turn_bias_set(double bias) { odom_turn_bias_amount = bias; }
+double Drive::odom_turn_bias_get() { return odom_turn_bias_amount; }
+void Drive::odom_path_spacing_set(double spacing) { SPACING = spacing; }
+void Drive::odom_path_spacing_set(okapi::QLength p_spacing) { odom_path_spacing_set(p_spacing.convert(okapi::inch)); }
+double Drive::odom_path_spacing_get() { return SPACING; }
+void Drive::odom_look_ahead_set(double distance) { LOOK_AHEAD = distance; }
+void Drive::odom_look_ahead_set(okapi::QLength p_distance) { odom_look_ahead_set(p_distance.convert(okapi::inch)); }
+double Drive::odom_look_ahead_get() { return LOOK_AHEAD; }
+bool Drive::odom_turn_bias_enabled() { return is_odom_turn_bias_enabled; }
+void Drive::odom_turn_bias_enable(bool set) { is_odom_turn_bias_enabled = set; }
+void Drive::slew_odom_reenable(bool reenable) { slew_reenables_when_max_speed_changes = reenable; }
+bool Drive::slew_odom_reenabled() { return slew_reenables_when_max_speed_changes; }
+
+/////
+// pid_odom_set but it looks like pid_drive_set
+/////
+void Drive::pid_odom_set(okapi::QLength p_target, int speed, bool slew_on) {
+  double target = p_target.convert(okapi::inch);
+  pid_odom_set(target, speed, slew_on);
+}
+void Drive::pid_odom_set(okapi::QLength p_target, int speed) {
+  double target = p_target.convert(okapi::inch);
+  pid_odom_set(target, speed);
+}
+void Drive::pid_odom_set(double target, int speed) {
+  bool slew_on = util::sgn(target) >= 0 ? slew_drive_forward_get() : slew_drive_backward_get();
+  pid_odom_set(target, speed, slew_on);
+}
+void Drive::pid_odom_set(double target, int speed, bool slew_on) {
+  drive_directions fwd_or_rev = util::sgn(target) >= 0 ? fwd : rev;
+  pose target_pose = util::vector_off_point(target, {odom_x_get(), odom_y_get(), headingPID.target_get()});
+  odom path = {{target_pose.x, target_pose.y}, fwd_or_rev, speed};
+
+  xyPID.timers_reset();
+  current_a_odomPID.timers_reset();
+
+  if (print_toggle) printf("Injected ");
+  std::vector<odom> input_path = inject_points({path});
+  odom_turn_bias_enable(false);
+  current_slew_on = slew_on;
+  slew_min_when_it_enabled = 0;
+  slew_will_enable_later = false;
+  raw_pid_odom_pp_set(input_path, slew_on);
+}
+
+/////
+// pid_odom_set
+/////
+// No units
+void Drive::pid_odom_set(odom imovement) {
+  bool slew_on = imovement.drive_direction == fwd ? slew_drive_forward_get() : slew_drive_backward_get();
+  pid_odom_set(imovement, slew_on);
+}
+void Drive::pid_odom_set(odom imovement, bool slew_on) {
+  if (imovement.target.theta != ANGLE_NOT_SET)
+    pid_odom_boomerang_set(imovement, slew_on);
+  else
+    pid_odom_injected_pp_set({imovement}, slew_on);
+}
+void Drive::pid_odom_set(std::vector<odom> imovements) {
+  bool slew_on = imovements[0].drive_direction == fwd ? slew_drive_forward_get() : slew_drive_backward_get();
+  pid_odom_set(imovements, slew_on);
+}
+void Drive::pid_odom_set(std::vector<odom> imovements, bool slew_on) {
+  pid_odom_smooth_pp_set(imovements, slew_on);
+}
+// Units
+void Drive::pid_odom_set(united_odom p_imovement) {
+  odom imovement = util::united_odom_to_odom(p_imovement);
+  pid_odom_set(imovement);
+}
+void Drive::pid_odom_set(united_odom p_imovement, bool slew_on) {
+  odom imovement = util::united_odom_to_odom(p_imovement);
+  pid_odom_set(imovement, slew_on);
+}
+void Drive::pid_odom_set(std::vector<united_odom> p_imovements) {
+  std::vector<odom> imovements = util::united_odoms_to_odoms(p_imovements);
+  pid_odom_set(imovements);
+}
+void Drive::pid_odom_set(std::vector<united_odom> p_imovements, bool slew_on) {
+  std::vector<odom> imovements = util::united_odoms_to_odoms(p_imovements);
+  pid_odom_set(imovements, slew_on);
+}
+
+/////
+// ptp
+/////
+// No units
+void Drive::pid_odom_ptp_set(odom imovement) {
+  bool slew_on = imovement.drive_direction == fwd ? slew_drive_forward_get() : slew_drive_backward_get();
+  pid_odom_ptp_set(imovement, slew_on);
+}
+// Units
+void Drive::pid_odom_ptp_set(united_odom p_imovement) {
+  odom imovement = util::united_odom_to_odom(p_imovement);
+  pid_odom_ptp_set(imovement);
+}
+void Drive::pid_odom_ptp_set(united_odom p_imovement, bool slew_on) {
+  odom imovement = util::united_odom_to_odom(p_imovement);
+  pid_odom_ptp_set(imovement, slew_on);
+}
+
+/////
+// pp
+/////
+// No units
+void Drive::pid_odom_pp_set(std::vector<odom> imovements) {
+  bool slew_on = imovements[0].drive_direction == fwd ? slew_drive_forward_get() : slew_drive_backward_get();
+  pid_odom_pp_set(imovements, slew_on);
+}
+// Units
+void Drive::pid_odom_pp_set(std::vector<united_odom> p_imovements) {
+  std::vector<odom> imovements = util::united_odoms_to_odoms(p_imovements);
+  pid_odom_pp_set(imovements);
+}
+void Drive::pid_odom_pp_set(std::vector<united_odom> p_imovements, bool slew_on) {
+  std::vector<odom> imovements = util::united_odoms_to_odoms(p_imovements);
+  pid_odom_pp_set(imovements, slew_on);
+}
+
+/////
+// injected pp
+/////
+// No units
+void Drive::pid_odom_injected_pp_set(std::vector<ez::odom> imovements) {
+  bool slew_on = imovements[0].drive_direction == fwd ? slew_drive_forward_get() : slew_drive_backward_get();
+  pid_odom_injected_pp_set(imovements, slew_on);
+}
+void Drive::pid_odom_injected_pp_set(std::vector<ez::odom> imovements, bool slew_on) {
+  xyPID.timers_reset();
+  current_a_odomPID.timers_reset();
+
+  if (print_toggle) printf("Injected ");
+  std::vector<odom> input_path = inject_points(set_odoms_direction(imovements));
+  odom_turn_bias_enable(true);
+  current_slew_on = slew_on;
+  slew_min_when_it_enabled = 0;
+  slew_will_enable_later = false;
+  raw_pid_odom_pp_set(input_path, slew_on);
+}
+// Units
+void Drive::pid_odom_injected_pp_set(std::vector<ez::united_odom> p_imovements) {
+  std::vector<odom> imovements = util::united_odoms_to_odoms(p_imovements);
+  pid_odom_injected_pp_set(imovements);
+}
+void Drive::pid_odom_injected_pp_set(std::vector<ez::united_odom> p_imovements, bool slew_on) {
+  std::vector<odom> imovements = util::united_odoms_to_odoms(p_imovements);
+  pid_odom_injected_pp_set(imovements, slew_on);
+}
+
+/////
+// smooth injected pp
+/////
+// No units
+void Drive::pid_odom_smooth_pp_set(std::vector<odom> imovements) {
+  bool slew_on = imovements[0].drive_direction == fwd ? slew_drive_forward_get() : slew_drive_backward_get();
+  pid_odom_smooth_pp_set(imovements, slew_on);
+}
+void Drive::pid_odom_smooth_pp_set(std::vector<odom> imovements, bool slew_on) {
+  xyPID.timers_reset();
+  current_a_odomPID.timers_reset();
+
+  if (print_toggle) printf("Smooth Injected ");
+  std::vector<odom> input_path = smooth_path(inject_points(set_odoms_direction(imovements)), odom_smooth_weight_smooth, odom_smooth_weight_data, odom_smooth_tolerance);
+  odom_turn_bias_enable(true);
+  current_slew_on = slew_on;
+  slew_min_when_it_enabled = 0;
+  slew_will_enable_later = false;
+  raw_pid_odom_pp_set(input_path, slew_on);
+}
+// Units
+void Drive::pid_odom_smooth_pp_set(std::vector<united_odom> p_imovements) {
+  std::vector<odom> imovements = util::united_odoms_to_odoms(p_imovements);
+  pid_odom_smooth_pp_set(imovements);
+}
+void Drive::pid_odom_smooth_pp_set(std::vector<united_odom> p_imovements, bool slew_on) {
+  std::vector<odom> imovements = util::united_odoms_to_odoms(p_imovements);
+  pid_odom_smooth_pp_set(imovements, slew_on);
+}
+
+/////
+// boomerang
+/////
+// No units
+void Drive::pid_odom_boomerang_set(odom imovement) {
+  bool slew_on = imovement.drive_direction == fwd ? slew_drive_forward_get() : slew_drive_backward_get();
+  pid_odom_boomerang_set(imovement, slew_on);
+}
+void Drive::pid_odom_boomerang_set(odom imovement, bool slew_on) {
+  if (print_toggle) printf("Boomerang ");
+  pid_odom_pp_set({imovement}, slew_on);
+}
+// Units
+void Drive::pid_odom_boomerang_set(united_odom p_imovement) {
+  odom imovement = util::united_odom_to_odom(p_imovement);
+  pid_odom_boomerang_set(imovement);
+}
+void Drive::pid_odom_boomerang_set(united_odom p_imovement, bool slew_on) {
+  odom imovement = util::united_odom_to_odom(p_imovement);
+  pid_odom_boomerang_set(imovement, slew_on);
+}
+
+/////
+// External base pure pursuit
+/////
+void Drive::pid_odom_pp_set(std::vector<odom> imovements, bool slew_on) {
+  xyPID.timers_reset();
+  current_a_odomPID.timers_reset();
+
+  std::vector<odom> input = set_odoms_direction(imovements);
+  input.insert(input.begin(), {{{odom_x_get(), odom_y_get(), ANGLE_NOT_SET}, imovements[0].drive_direction, imovements[0].max_xy_speed}});
+
+  int t = 0;
+  for (int i = 0; i < input.size() - 1; i++) {
+    // Inject new parent points for boomerang
+    int j = i + t;
+    j = i;
+    if (input[j].target.theta != ANGLE_NOT_SET) {
+      // Calculate the new point with known information: hypot and angle
+      double angle_to_point = input[j].target.theta;
+      int dir = input[j].drive_direction == REV ? -1 : 1;
+      pose new_point = util::vector_off_point(odom_look_ahead_get() * dir, {input[j].target.x, input[j].target.y, angle_to_point});
+      new_point.theta = ANGLE_NOT_SET;
+
+      input.insert(input.cbegin() + j + 1, {new_point, input[j].drive_direction, input[j].max_xy_speed});
+
+      t++;
+    }
+  }
+
+  // Shift all the turn behaviors 1 parent point down
+  for (int i = 0; i < input.size() - 1; i++) {
+    input[i].turn_behavior = input[i + 1].turn_behavior;
+  }
+  input.back().turn_behavior = raw;
+
+  // This is used for pid_wait_until_pp()
+  injected_pp_index.clear();
+  injected_pp_index.push_back(0);
+  for (int i = 0; i < input.size(); i++) {
+    if (i != 0 && input[i - 1].target.theta == ANGLE_NOT_SET)
+      injected_pp_index.push_back(i);
+  }
+
+  odom_turn_bias_enable(true);
+  current_slew_on = slew_on;
+  slew_min_when_it_enabled = 0;
+  slew_will_enable_later = false;
+
+  if (print_toggle) printf("Pure Pursuit ");
+  raw_pid_odom_pp_set(input, slew_on);
+}
+
+//////
+// External base ptp
+/////
+void Drive::pid_odom_ptp_set(odom imovement, bool slew_on) {
+  imovement = set_odom_direction(imovement);
+
+  odom_second_to_last = odom_pose_get();
+  odom_target_start = imovement.target;
+  odom_start = odom_pose_get();
+
+  xyPID.timers_reset();
+  current_a_odomPID.timers_reset();
+
+  // This is used for wait_until and slew
+  l_start = drive_sensor_left();
+  r_start = drive_sensor_right();
+
+  odom_turn_bias_enable(true);
+  current_slew_on = slew_on;
+  slew_min_when_it_enabled = 0;
+  slew_will_enable_later = false;
+  raw_pid_odom_ptp_set(imovement, slew_on);
+
+  // Initialize slew
+  int dir = current_drive_direction == REV ? -1 : 1;  // If we're going backwards, add a -1
+  double dist_to_target = util::distance_to_point(odom_target, odom_pose_get()) * dir;
+  slew_left.initialize(slew_on, max_speed, dist_to_target + l_start, l_start);
+  slew_right.initialize(slew_on, max_speed, dist_to_target + r_start, r_start);
+
+  drive_mode_set(POINT_TO_POINT);
+}
+
+/////
+// Base pure pursuit
+/////
+void Drive::raw_pid_odom_pp_set(std::vector<odom> imovements, bool slew_on) {
+  odom_second_to_last = imovements[imovements.size() - 2].target;
+  odom_target_start = imovements[imovements.size() - 1].target;
+  odom_start = odom_pose_get();
+
+  was_last_pp_mode_boomerang = false;
+
+  // Clear current list of targets
+  pp_movements.clear();
+  pp_index = 0;
+
+  // Set new target
+  pp_movements = imovements;
+
+  raw_pid_odom_ptp_set(pp_movements[pp_index], slew_on);
+
+  // This is used for wait_until and slew
+  l_start = drive_sensor_left();
+  r_start = drive_sensor_right();
+
+  // Initialize slew
+  int dir = current_drive_direction == REV ? -1 : 1;  // If we're going backwards, add a -1
+  double dist_to_target = util::distance_to_point(pp_movements.end()->target, odom_pose_get()) * dir;
+  slew_left.initialize(slew_on, max_speed, dist_to_target + l_start, l_start);
+  slew_right.initialize(slew_on, max_speed, dist_to_target + r_start, r_start);
+
+  drive_mode_set(PURE_PURSUIT);
+}
+
+/////
+// Base point to point
+/////
+void Drive::raw_pid_odom_ptp_set(odom imovement, bool slew_on) {
+  // Update current drive/turn behavior
+  current_drive_direction = imovement.drive_direction;
+
+  // Calculate the point to look at
+  point_to_face = find_point_to_face(odom_pose_get(), {imovement.target.x, imovement.target.y}, current_drive_direction, true);
+  double target = util::absolute_angle_to_point(point_to_face[!ptf1_running], odom_pose_get());  // Calculate the point for angle to face
+  if (imovement.turn_behavior != raw) {
+    odom_imu_start = drive_imu_get();
+    current_angle_behavior = imovement.turn_behavior;
+  }
+
+  if (current_slew_on && imovement.max_xy_speed > pid_speed_max_get() && slew_odom_reenabled()) {
+    slew_will_enable_later = true;
+  }
+  target = new_turn_target_compute(target, odom_imu_start, current_angle_behavior);
+  headingPID.target_set(target);
+
+  // Set targets
+  odom_target.x = imovement.target.x;
+  odom_target.y = imovement.target.y;
+
+  // Change constants if we're going fwd or rev
+  PID *new_drive_pid;
+  slew::Constants slew_consts;
+  if (current_drive_direction == REV) {
+    new_drive_pid = &backward_drivePID;
+    slew_consts = slew_backward.constants_get();
+
+  } else {
+    new_drive_pid = &forward_drivePID;
+    slew_consts = slew_forward.constants_get();
+  }
+
+  // Prioritize custom fwd/rev constants.  Otherwise, use the same for fwd and rev
+  if (fwd_rev_drivePID.constants_set_check() && !new_drive_pid->constants_set_check())
+    new_drive_pid = &fwd_rev_drivePID;
+
+  // Set constants
+  PID::Constants pid_drive_consts = new_drive_pid->constants_get();
+  xyPID.constants_set(pid_drive_consts.kp, pid_drive_consts.ki, pid_drive_consts.kd, pid_drive_consts.start_i);
+
+  // Set max speed
+  pid_speed_max_set(imovement.max_xy_speed);
+
+  int slew_min = slew_consts.min_speed;
+  if (current_slew_on && slew_will_enable_later && !slew_on && slew_odom_reenabled()) {
+    slew_on = true;
+    slew_will_enable_later = false;
+    if (slew_min_when_it_enabled > slew_consts.min_speed)
+      slew_min = slew_min_when_it_enabled;
+
+    slew_left.constants_set(slew_consts.distance_to_travel, slew_min);
+    slew_right.constants_set(slew_consts.distance_to_travel, slew_min);
+
+    // Initialize slew
+    int dir = current_drive_direction == REV ? -1 : 1;  // If we're going backwards, add a -1
+    double dist_to_target = 100.0 * dir;
+    slew_left.initialize(slew_on, max_speed, dist_to_target + drive_sensor_left(), drive_sensor_left());
+    slew_right.initialize(slew_on, max_speed, dist_to_target + drive_sensor_right(), drive_sensor_right());
+  }
+
+  if (slew_min_when_it_enabled == 0) {
+    slew_left.constants_set(slew_consts.distance_to_travel, slew_min);
+    slew_right.constants_set(slew_consts.distance_to_travel, slew_min);
+  }
+
+  bool is_current_boomerang = false;
+  if (mode == PURE_PURSUIT)
+    is_current_boomerang = pp_movements[pp_index].target.theta != ANGLE_NOT_SET ? true : false;
+  if (print_toggle && !was_last_pp_mode_boomerang) {
+    if (mode == PURE_PURSUIT)
+      printf(" ");
+    printf("Odom Motion Started... Target Coordinates: (%.2f, %.2f, %.2f) \n", imovement.target.x, imovement.target.y, imovement.target.theta);
+  }
+  if (mode == PURE_PURSUIT)
+    was_last_pp_mode_boomerang = is_current_boomerang;
+
+  // Change angle constants if it's boomerang vs normal odom move
+  PID::Constants angle_const;
+  if (is_current_boomerang)
+    angle_const = boomerangPID.constants_get();
+  else
+    angle_const = odom_angularPID.constants_get();
+  current_a_odomPID.constants_set(angle_const.kp, angle_const.ki, angle_const.kd, angle_const.start_i);
+
+  // Get the starting point for if we're positive or negative.  This is used to find if we've past target
+  past_target = util::sgn(is_past_target(odom_target, odom_pose_get()));
+
+  slew_min_when_it_enabled = pid_speed_max_get();
+
+  // This is used for wait_until
+  int dir = current_drive_direction == REV ? -1 : 1;  // If we're going backwards, add a -1
+  leftPID.target_set(l_start + (odom_look_ahead_get() * dir));
+  rightPID.target_set(l_start + (odom_look_ahead_get() * dir));
+  leftPID.exit = xyPID.exit;  // Switch over to xy pid exits
+  rightPID.exit = xyPID.exit;
+}

src/EZ-Template/drive/set_pid/set_pid.cpp

@@ -0,0 +1,76 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/api.hpp"
+#include "okapi/api/units/QAngle.hpp"
+
+// Updates max speed
+void Drive::pid_speed_max_set(int speed) {
+  max_speed = abs(util::clamp(speed, 127, -127));
+  slew_left.speed_max_set(max_speed);
+  slew_right.speed_max_set(max_speed);
+  slew_turn.speed_max_set(max_speed);
+  slew_swing.speed_max_set(max_speed);
+}
+int Drive::pid_speed_max_get() { return max_speed; }
+
+// "turn bias" will bias either left or right, the user can decide
+// the shortest path from 0.1 to 180 would be to go 179.9 degrees, but
+// PID has some level of variance.  this allows the user to set a tolerance
+// that will make the robot go left or right when it's within that tolerance
+void Drive::pid_angle_behavior_bias_set(e_angle_behavior behavior) {
+  if (behavior == ez::LEFT_TURN)
+    turn_biased_left = true;
+  else if (behavior == ez::RIGHT_TURN)
+    turn_biased_left = false;
+  else
+    printf("Must input 'left' or 'right' for angle behavior bias!\n");
+}
+e_angle_behavior Drive::pid_angle_behavior_bias_get() { return turn_biased_left ? ez::LEFT_TURN : ez::RIGHT_TURN; }
+void Drive::pid_angle_behavior_tolerance_set(double tolerance) { turn_tolerance = tolerance; }
+void Drive::pid_angle_behavior_tolerance_set(okapi::QAngle p_tolerance) { pid_angle_behavior_tolerance_set(p_tolerance.convert(okapi::degree)); }
+double Drive::pid_angle_behavior_tolerance_get() { return turn_tolerance; }
+
+// Changes global default turn behavior to either:
+//  - cw
+//  - ccw
+//  - shortest
+//  - longest
+//  - raw
+void Drive::pid_angle_behavior_set(ez::e_angle_behavior behavior) {
+  default_swing_type = behavior;
+  default_turn_type = behavior;
+  default_odom_type = behavior;
+}
+
+void Drive::pid_targets_reset() {
+  headingPID.target_set(0);
+  leftPID.target_set(0);
+  rightPID.target_set(0);
+  xyPID.target_set(0);
+  current_a_odomPID.target_set(0);
+  forward_drivePID.target_set(0);
+  backward_drivePID.target_set(0);
+  turnPID.target_set(0);
+  swingPID.target_set(0);
+  forward_swingPID.target_set(0);
+  backward_swingPID.target_set(0);
+}
+
+void Drive::drive_mode_set(e_mode p_mode, bool stop_drive) {
+  mode = p_mode;
+  if (mode == DISABLE && stop_drive)
+    private_drive_set(0, 0);
+}
+e_mode Drive::drive_mode_get() { return mode; }
+
+// Toggle drive motors but still allow PID to run
+void Drive::pid_drive_toggle(bool toggle) { drive_toggle = toggle; }
+bool Drive::pid_drive_toggle_get() { return drive_toggle; }
+
+// Don't print stuff
+void Drive::pid_print_toggle(bool toggle) { print_toggle = toggle; }
+bool Drive::pid_print_toggle_get() { return print_toggle; }

src/EZ-Template/drive/set_pid/set_swing_pid.cpp

@@ -0,0 +1,354 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/api.hpp"
+#include "okapi/api/units/QAngle.hpp"
+
+/////
+// Sets swing constants
+/////
+// Max speed when i is enabled for large turns
+void Drive::pid_swing_min_set(int min) { swing_min = abs(min); }
+int Drive::pid_swing_min_get() { return swing_min; }
+
+// PID Constants
+void Drive::pid_swing_constants_set(double p, double i, double d, double p_start_i) {
+  pid_swing_constants_forward_set(0.0, 0.0, 0.0, 0.0);
+  pid_swing_constants_backward_set(0.0, 0.0, 0.0, 0.0);
+  fwd_rev_swingPID.constants_set(p, i, d, p_start_i);
+}
+void Drive::pid_swing_constants_forward_set(double p, double i, double d, double p_start_i) {
+  forward_swingPID.constants_set(p, i, d, p_start_i);
+}
+void Drive::pid_swing_constants_backward_set(double p, double i, double d, double p_start_i) {
+  backward_swingPID.constants_set(p, i, d, p_start_i);
+}
+PID::Constants Drive::pid_swing_constants_forward_get() { return forward_swingPID.constants_get(); }
+PID::Constants Drive::pid_swing_constants_backward_get() { return backward_swingPID.constants_get(); }
+PID::Constants Drive::pid_swing_constants_get() {
+  auto fwd_const = pid_swing_constants_forward_get();
+  auto rev_const = pid_swing_constants_backward_get();
+  if (!(fwd_const.kp == rev_const.kp && fwd_const.ki == rev_const.ki && fwd_const.kd == rev_const.kd && fwd_const.start_i == rev_const.start_i)) {
+    printf("\nForward and Reverse constants are not the same!");
+    return {-1, -1, -1, -1};
+  }
+  return fwd_const;
+}
+
+// Slew Constants
+void Drive::slew_swing_constants_backward_set(okapi::QLength distance, int min_speed) {
+  slew_swing_rev_using_angle = false;
+  double dist = distance.convert(okapi::inch);
+  slew_swing_backward.constants_set(dist, min_speed);
+}
+void Drive::slew_swing_constants_forward_set(okapi::QLength distance, int min_speed) {
+  slew_swing_fwd_using_angle = false;
+  double dist = distance.convert(okapi::inch);
+  slew_swing_forward.constants_set(dist, min_speed);
+}
+void Drive::slew_swing_constants_set(okapi::QLength distance, int min_speed) {
+  slew_swing_constants_forward_set(distance, min_speed);
+  slew_swing_constants_backward_set(distance, min_speed);
+}
+void Drive::slew_swing_constants_backward_set(okapi::QAngle distance, int min_speed) {
+  slew_swing_rev_using_angle = true;
+  double dist = distance.convert(okapi::degree);
+  slew_swing_backward.constants_set(dist, min_speed);
+}
+void Drive::slew_swing_constants_forward_set(okapi::QAngle distance, int min_speed) {
+  slew_swing_fwd_using_angle = true;
+  double dist = distance.convert(okapi::degree);
+  slew_swing_forward.constants_set(dist, min_speed);
+}
+void Drive::slew_swing_constants_set(okapi::QAngle distance, int min_speed) {
+  slew_swing_constants_forward_set(distance, min_speed);
+  slew_swing_constants_backward_set(distance, min_speed);
+}
+
+// Global enables for swing slew
+void Drive::slew_swing_set(bool slew_on) {
+  global_forward_swing_slew_enabled = slew_on;
+  global_backward_swing_slew_enabled = slew_on;
+}
+void Drive::slew_swing_forward_set(bool slew_on) { global_forward_swing_slew_enabled = slew_on; }
+bool Drive::slew_swing_forward_get() { return global_forward_swing_slew_enabled; }
+void Drive::slew_swing_backward_set(bool slew_on) { global_backward_swing_slew_enabled = slew_on; }
+bool Drive::slew_swing_backward_get() { return global_backward_swing_slew_enabled; }
+// Checks if slew is globally enabled or not
+bool Drive::is_swing_slew_enabled(e_swing type, double target, double current) {
+  int side = type == ez::LEFT_SWING ? 1 : -1;
+  int direction = util::sgn((target - current) * side);
+  return direction == 1 ? slew_swing_forward_get() : slew_swing_backward_get();
+}
+
+// Swing default behavior set
+void Drive::pid_swing_behavior_set(ez::e_angle_behavior behavior) { default_swing_type = behavior; }
+ez::e_angle_behavior Drive::pid_swing_behavior_get() { return default_swing_type; }
+
+/////
+// Set swing PID basic wrappers
+/////
+// Absolute
+void Drive::pid_swing_set(e_swing type, double target, int speed) {
+  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  pid_swing_set(type, target, speed, 0, pid_swing_behavior_get(), slew_on);
+}
+void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_set(type, target, speed);
+}
+// Relative
+void Drive::pid_swing_relative_set(e_swing type, double target, int speed) {
+  // Figure out if going forward or backward
+  double absolute_heading = target + headingPID.target_get();
+  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_imu_get());
+  pid_swing_relative_set(type, target, speed, 0, pid_swing_behavior_get(), slew_on);
+}
+void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_relative_set(type, target, speed);
+}
+
+/////
+// Set turn PID with only swing behavior
+/////
+// Absolute
+void Drive::pid_swing_set(e_swing type, double target, int speed, e_angle_behavior behavior) {
+  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  pid_swing_set(type, target, speed, 0, behavior, slew_on);
+}
+void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_set(type, target, speed, behavior);
+}
+// Relative
+void Drive::pid_swing_relative_set(e_swing type, double target, int speed, e_angle_behavior behavior) {
+  // Figure out if going forward or backward
+  double absolute_heading = target + headingPID.target_get();
+  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_imu_get());
+  pid_swing_relative_set(type, target, speed, 0, behavior, slew_on);
+}
+void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_relative_set(type, target, speed, behavior);
+}
+
+/////
+// Set turn PID with only opposite speed
+/////
+// Absolute
+void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_speed) {
+  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  pid_swing_set(type, target, speed, opposite_speed, pid_swing_behavior_get(), slew_on);
+}
+void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  pid_swing_set(type, target, speed, opposite_speed, slew_on);
+}
+// Relative
+void Drive::pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed) {
+  double absolute_heading = target + headingPID.target_get();
+  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_imu_get());
+  pid_swing_relative_set(type, target, speed, opposite_speed, pid_swing_behavior_get(), slew_on);
+}
+void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_relative_set(type, target, speed, opposite_speed);
+}
+
+/////
+// Set turn PID with only slew
+/////
+// Absolute
+void Drive::pid_swing_set(e_swing type, double target, int speed, bool slew_on) {
+  pid_swing_set(type, target, speed, 0, pid_swing_behavior_get(), slew_on);
+}
+void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, bool slew_on) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_set(type, target, speed, slew_on);
+}
+// Relative
+void Drive::pid_swing_relative_set(e_swing type, double target, int speed, bool slew_on) {
+  pid_swing_relative_set(type, target, speed, 0, pid_swing_behavior_get(), slew_on);
+}
+void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, bool slew_on) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_relative_set(type, target, speed, slew_on);
+}
+
+/////
+// Set turn PID with only opposite speed and swing behavior
+/////
+// Absolute
+void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior) {
+  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  pid_swing_set(type, target, speed, opposite_speed, behavior, slew_on);
+}
+void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  pid_swing_set(type, target, speed, opposite_speed, behavior);
+}
+// Relative
+void Drive::pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior) {
+  double absolute_heading = target + headingPID.target_get();
+  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_imu_get());
+  pid_swing_relative_set(type, target, speed, opposite_speed, behavior, slew_on);
+}
+void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_relative_set(type, target, speed, opposite_speed, behavior);
+}
+
+/////
+// Set turn PID with opposite speed and slew
+/////
+// Absolute
+void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on) {
+  pid_swing_set(type, target, speed, opposite_speed, pid_swing_behavior_get(), slew_on);
+}
+void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, bool slew_on) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_set(type, target, speed, opposite_speed, slew_on);
+}
+// Relative
+void Drive::pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on) {
+  // Compute absolute target by adding to current heading
+  double absolute_target = headingPID.target_get() + target;
+  if (print_toggle) printf("Relative ");
+  pid_swing_set(type, absolute_target, speed, opposite_speed, pid_swing_behavior_get(), slew_on);
+}
+void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, bool slew_on) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_relative_set(type, target, speed, opposite_speed, slew_on);
+}
+
+/////
+// Set turn PID with swing behavior and slew
+/////
+// Absolute
+void Drive::pid_swing_set(e_swing type, double target, int speed, e_angle_behavior behavior, bool slew_on) {
+  pid_swing_set(type, target, speed, 0, pid_swing_behavior_get(), slew_on);
+}
+void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_set(type, target, speed, behavior, slew_on);
+}
+// Relative
+void Drive::pid_swing_relative_set(e_swing type, double target, int speed, e_angle_behavior behavior, bool slew_on) {
+  // Compute absolute target by adding to current heading
+  double absolute_target = headingPID.target_get() + target;
+  if (print_toggle) printf("Relative ");
+  pid_swing_set(type, absolute_target, speed, 0, pid_swing_behavior_get(), slew_on);
+}
+void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_relative_set(type, target, speed, behavior, slew_on);
+}
+
+/////
+// Set turn PID with opposite speed, swing behavior, and slew
+/////
+// Absolute
+void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_set(type, target, speed, opposite_speed, behavior, slew_on);
+}
+// Relative
+void Drive::pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on) {
+  // Compute absolute target by adding to current heading
+  double absolute_target = headingPID.target_get() + target;
+  if (print_toggle) printf("Relative ");
+  pid_swing_set(type, absolute_target, speed, opposite_speed, behavior, slew_on);
+}
+void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_swing_relative_set(type, target, speed, opposite_speed, behavior, slew_on);
+}
+
+/////
+// Swing set base
+/////
+void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on) {
+  swingPID.timers_reset();
+
+  // Set turn behavior
+  current_angle_behavior = behavior;
+
+  // Compute new turn target based on new angle
+  target = flip_angle_target(target);
+  target = new_turn_target_compute(target, drive_imu_get(), current_angle_behavior);
+
+  // Print targets
+  if (print_toggle) printf("Swing Started... Target Value: %.2f\n", target);
+
+  chain_sensor_start = drive_imu_get();
+  chain_target_start = target;
+  used_motion_chain_scale = 0.0;
+
+  // Flip the swing from left-right if rotation axis is flipped
+  current_swing = type;
+  if (odom_theta_direction_get())
+    current_swing = current_swing == ez::LEFT_SWING ? ez::RIGHT_SWING : ez::LEFT_SWING;
+
+  // Figure out if going forward or backward
+  int side = type == ez::LEFT_SWING ? 1 : -1;
+  int direction = util::sgn((target - chain_sensor_start) * side);
+
+  // Set constants according to the robots direction
+
+  PID *new_drive_pid;
+  PID *new_swing_pid;
+  slew::Constants slew_consts;
+
+  if (direction == -1) {
+    new_drive_pid = &backward_drivePID;
+    new_swing_pid = &backward_swingPID;
+    slew_consts = slew_swing_backward.constants_get();
+    slew_swing_using_angle = slew_swing_rev_using_angle;
+    motion_chain_backward = true;
+  } else {
+    new_drive_pid = &forward_drivePID;
+    new_swing_pid = &forward_swingPID;
+    slew_consts = slew_swing_forward.constants_get();
+    slew_swing_using_angle = slew_swing_fwd_using_angle;
+    motion_chain_backward = false;
+  }
+
+  // Prioritize custom fwd/rev constants.  Otherwise, use the same for fwd and rev
+  if (fwd_rev_drivePID.constants_set_check() && (!new_drive_pid->constants_set_check()))
+    new_drive_pid = &fwd_rev_drivePID;
+  if (fwd_rev_swingPID.constants_set_check() && !new_swing_pid->constants_set_check())
+    new_swing_pid = &fwd_rev_swingPID;
+
+  PID::Constants pid_drive_consts = new_drive_pid->constants_get();
+  PID::Constants pid_swing_consts = new_swing_pid->constants_get();
+  swingPID.constants_set(pid_swing_consts.kp, pid_swing_consts.ki, pid_swing_consts.kd, pid_swing_consts.start_i);
+  leftPID.constants_set(pid_drive_consts.kp, pid_drive_consts.ki, pid_drive_consts.kd, pid_drive_consts.start_i);
+  rightPID.constants_set(pid_drive_consts.kp, pid_drive_consts.ki, pid_drive_consts.kd, pid_drive_consts.start_i);
+  slew_swing.constants_set(slew_consts.distance_to_travel, slew_consts.min_speed);
+
+  // Set targets for the side that isn't moving
+  leftPID.target_set(drive_sensor_left());
+  rightPID.target_set(drive_sensor_right());
+
+  // Set PID targets
+  swingPID.target_set(target);
+  headingPID.target_set(target);  // Update heading target for next drive motion
+  pid_speed_max_set(speed);
+  swing_opposite_speed = opposite_speed;
+
+  // Initialize slew
+  double current = slew_swing_using_angle ? chain_sensor_start : (current_swing == LEFT_SWING ? drive_sensor_left() : drive_sensor_right());
+  double slew_tar = slew_swing_using_angle ? target : direction * 100;
+  if (!slew_swing_using_angle) slew_tar += current;
+  slew_swing.initialize(slew_on, max_speed, slew_tar, current);
+  current_slew_on = slew_on;
+
+  // Run task
+  drive_mode_set(SWING);
+}

src/EZ-Template/drive/set_pid/set_turn_pid.cpp

@@ -0,0 +1,197 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/api.hpp"
+#include "okapi/api/units/QAngle.hpp"
+
+/////
+// Sets turn constants
+/////
+void Drive::slew_turn_constants_set(okapi::QAngle distance, int min_speed) {
+  double dist = distance.convert(okapi::degree);
+  slew_turn.constants_set(dist, min_speed);
+}
+void Drive::pid_turn_constants_set(double p, double i, double d, double p_start_i) {
+  turnPID.constants_set(p, i, d, p_start_i);
+}
+PID::Constants Drive::pid_turn_constants_get() { return turnPID.constants_get(); }
+void Drive::pid_turn_min_set(int min) { turn_min = abs(min); }
+int Drive::pid_turn_min_get() { return turn_min; }
+
+// Sets the behavior of turning
+void Drive::pid_turn_behavior_set(ez::e_angle_behavior behavior) { default_turn_type = behavior; }
+ez::e_angle_behavior Drive::pid_turn_behavior_get() { return default_turn_type; }
+
+// Global enables for turn slew
+void Drive::slew_turn_set(bool slew_on) { global_turn_slew_enabled = slew_on; }
+bool Drive::slew_turn_get() { return global_turn_slew_enabled; }
+
+double Drive::flip_angle_target(double target) {
+  int flip_theta = theta_flipped ? -1 : 1;
+  double new_target = target;
+  new_target *= flip_theta;
+  return new_target;
+}
+
+/////
+// Set turn PID basic wrappers
+/////
+// Absolute
+void Drive::pid_turn_set(double target, int speed) {
+  pid_turn_set(target, speed, pid_turn_behavior_get(), slew_turn_get());
+}
+void Drive::pid_turn_set(okapi::QAngle p_target, int speed) {
+  pid_turn_set(p_target, speed, pid_turn_behavior_get(), slew_turn_get());
+}
+// Relative
+void Drive::pid_turn_relative_set(double target, int speed) {
+  pid_turn_relative_set(target, speed, pid_turn_behavior_get(), slew_turn_get());
+}
+void Drive::pid_turn_relative_set(okapi::QAngle p_target, int speed) {
+  pid_turn_relative_set(p_target, speed, pid_turn_behavior_get(), slew_turn_get());
+}
+
+/////
+// Set turn PID with only turn behavior
+/////
+// Absolute
+void Drive::pid_turn_set(double target, int speed, e_angle_behavior behavior) {
+  pid_turn_set(target, speed, behavior, slew_turn_get());
+}
+void Drive::pid_turn_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior) {
+  pid_turn_set(p_target, speed, behavior, slew_turn_get());
+}
+// Relative
+void Drive::pid_turn_relative_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior) {
+  pid_turn_relative_set(p_target, speed, behavior, slew_turn_get());
+}
+void Drive::pid_turn_relative_set(double target, int speed, e_angle_behavior behavior) {
+  pid_turn_relative_set(target, speed, behavior, slew_turn_get());
+}
+
+/////
+// Set turn PID with only slew
+/////
+// Absolute
+void Drive::pid_turn_set(double target, int speed, bool slew_on) {
+  pid_turn_set(target, speed, pid_turn_behavior_get(), slew_on);
+}
+void Drive::pid_turn_set(okapi::QAngle p_target, int speed, bool slew_on) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_turn_set(target, speed, pid_turn_behavior_get(), slew_on);
+}
+// Relative
+void Drive::pid_turn_relative_set(double target, int speed, bool slew_on) {
+  pid_turn_relative_set(target, speed, pid_turn_behavior_get(), slew_on);
+}
+void Drive::pid_turn_relative_set(okapi::QAngle p_target, int speed, bool slew_on) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_turn_relative_set(target, speed, pid_turn_behavior_get(), slew_on);
+}
+
+/////
+// Set turn PID with turn behavior and slew
+/////
+// Absolute
+void Drive::pid_turn_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_turn_set(target, speed, behavior, slew_on);
+}
+// Relative
+void Drive::pid_turn_relative_set(double target, int speed, e_angle_behavior behavior, bool slew_on) {
+  // Compute absolute target by adding to current heading
+  double absolute_target = headingPID.target_get() + target;
+  if (print_toggle) printf("Relative ");
+  pid_turn_set(absolute_target, speed, behavior, slew_on);
+}
+void Drive::pid_turn_relative_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on) {
+  double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
+  pid_turn_relative_set(target, speed, behavior, slew_on);
+}
+
+/////
+// Turn to angle base
+/////
+void Drive::pid_turn_set(double target, int speed, e_angle_behavior behavior, bool slew_on) {
+  turnPID.timers_reset();
+
+  // Set turn behavior
+  current_angle_behavior = behavior;
+
+  // Compute new turn target based on new angle
+  target = flip_angle_target(target);
+  target = new_turn_target_compute(target, drive_imu_get(), current_angle_behavior);
+
+  // Print targets
+  if (print_toggle) printf("Turn Started... Target Value: %.2f\n", target);
+  chain_sensor_start = drive_imu_get();
+  chain_target_start = target;
+  used_motion_chain_scale = 0.0;
+
+  // Set PID targets
+  turnPID.target_set(target);
+  headingPID.target_set(target);  // Update heading target for next drive motion
+  pid_speed_max_set(speed);
+
+  // Initialize slew
+  slew_turn.initialize(slew_on, max_speed, target, chain_sensor_start);
+  current_slew_on = slew_on;
+
+  // Run task
+  drive_mode_set(TURN);
+}
+
+/////
+// Turn to point wrappers
+/////
+// No units
+void Drive::pid_turn_set(pose itarget, drive_directions dir, int speed) {
+  pid_turn_set(itarget, dir, speed, default_turn_type, slew_turn_get());
+}
+void Drive::pid_turn_set(pose itarget, drive_directions dir, int speed, bool slew_on) {
+  pid_turn_set(itarget, dir, speed, default_turn_type, slew_on);
+}
+void Drive::pid_turn_set(pose itarget, drive_directions dir, int speed, e_angle_behavior behavior) {
+  pid_turn_set(itarget, dir, speed, behavior, slew_turn_get());
+}
+// Units
+void Drive::pid_turn_set(united_pose p_itarget, drive_directions dir, int speed) {
+  pid_turn_set(util::united_pose_to_pose(p_itarget), dir, speed);
+}
+void Drive::pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, bool slew_on) {
+  pid_turn_set(util::united_pose_to_pose(p_itarget), dir, speed, slew_on);
+}
+void Drive::pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, e_angle_behavior behavior) {
+  pid_turn_set(util::united_pose_to_pose(p_itarget), dir, speed, behavior);
+}
+void Drive::pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, e_angle_behavior behavior, bool slew_on) {
+  pid_turn_set(util::united_pose_to_pose(p_itarget), dir, speed, behavior, slew_on);
+}
+
+/////
+// Turn to point base
+/////
+void Drive::pid_turn_set(pose itarget, drive_directions dir, int speed, e_angle_behavior behavior, bool slew_on) {
+  itarget = flip_pose(itarget);
+  odom_imu_start = drive_imu_get();
+
+  current_drive_direction = dir;
+  current_angle_behavior = behavior;
+
+  // Calculate the point to look at
+  point_to_face = find_point_to_face(odom_pose_get(), {itarget.x, itarget.y}, current_drive_direction, true);
+
+  double target = util::absolute_angle_to_point(point_to_face[!ptf1_running], odom_pose_get());  // Calculate the point for angle to face
+
+  // Compute new turn target based on new angle
+  // angle_adder = (new_turn_target_compute(target, odom_imu_start, current_angle_behavior)) - target;
+  // ANGLE_ADDER_WAS_RESET = false;
+
+  if (print_toggle) printf("Turn to Point PID Started... Target Point: (%.2f, %.2f) \n", itarget.x, itarget.y);
+  pid_turn_set(target, speed, behavior, slew_on);
+
+  drive_mode_set(TURN_TO_POINT);
+}

src/EZ-Template/drive/tracking.cpp

@@ -0,0 +1,243 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/drive/drive.hpp"
+#include "EZ-Template/tracking_wheel.hpp"
+#include "EZ-Template/util.hpp"
+
+using namespace ez;
+
+// Sets and gets
+void Drive::odom_x_set(double x) {
+  odom_current.x = x;
+  l_pose.x = x;
+  r_pose.x = x;
+  central_pose.x = x;
+  was_odom_just_set = true;
+}
+void Drive::odom_x_set(okapi::QLength p_x) { odom_x_set(p_x.convert(okapi::inch)); }
+void Drive::odom_y_set(double y) {
+  odom_current.y = y;
+  l_pose.y = y;
+  r_pose.y = y;
+  central_pose.y = y;
+  was_odom_just_set = true;
+}
+void Drive::odom_y_set(okapi::QLength p_y) { odom_y_set(p_y.convert(okapi::inch)); }
+void Drive::odom_theta_set(double a) { drive_angle_set(a); }
+void Drive::odom_theta_set(okapi::QAngle p_a) { odom_theta_set(p_a.convert(okapi::degree)); }
+void Drive::drive_width_set(double input) {
+  global_track_width = fabs(input);
+  if (input != 0.0) {
+    odom_ime_track_width_left = -(global_track_width / 2.0);
+    odom_ime_track_width_right = (global_track_width / 2.0);
+    return;
+  }
+  odom_ime_track_width_left = 0.0;
+  odom_ime_track_width_right = 0.0;
+}
+void Drive::drive_width_set(okapi::QLength p_input) { drive_width_set(p_input.convert(okapi::inch)); }
+void Drive::odom_xy_set(double x, double y) {
+  odom_x_set(x);
+  odom_y_set(y);
+}
+void Drive::odom_xy_set(okapi::QLength p_x, okapi::QLength p_y) { odom_xy_set(p_x.convert(okapi::inch), p_y.convert(okapi::inch)); }
+void Drive::odom_xyt_set(double x, double y, double t) {
+  odom_x_set(x);
+  odom_y_set(y);
+  odom_theta_set(t);
+}
+void Drive::odom_xyt_set(okapi::QLength p_x, okapi::QLength p_y, okapi::QAngle p_t) { odom_xyt_set(p_x.convert(okapi::inch), p_y.convert(okapi::inch), p_t.convert(okapi::degree)); }
+void Drive::odom_pose_set(pose itarget) {
+  odom_x_set(itarget.x);
+  odom_y_set(itarget.y);
+  odom_theta_set(itarget.theta);
+}
+void Drive::odom_pose_set(united_pose itarget) { odom_pose_set(util::united_pose_to_pose(itarget)); }
+void Drive::odom_reset() { odom_pose_set({0.0, 0.0, 0.0}); }
+void Drive::odom_enable(bool input) { odometry_enabled = input; }
+bool Drive::odom_enabled() { return odometry_enabled; }
+
+double Drive::odom_x_get() { return odom_current.x; }
+double Drive::odom_y_get() { return odom_current.y; }
+double Drive::odom_theta_get() { return odom_current.theta; }
+pose Drive::odom_pose_get() { return odom_current; }
+double Drive::drive_width_get() { return global_track_width; }
+
+std::pair<float, float> Drive::decide_vert_sensor(ez::tracking_wheel* tracker, bool is_tracker_enabled, float ime, float ime_track) {
+  float current = ime;
+  float track_width = ime_track;
+  if (is_tracker_enabled) {
+    current = tracker->get();
+    track_width = tracker->distance_to_center_get();
+  }
+
+  return {current, track_width};
+}
+
+ez::pose Drive::solve_xy_vert(float p_track_width, float current_t, float delta_vert, float delta_t) {
+  pose output = {0.0, 0.0, 0.0};
+
+  // Figure out how far we've actually moved
+  float local_x = delta_vert;
+  float half_delta_t = 0.0;
+  if (delta_t != 0) {
+    half_delta_t = delta_t / 2.0;
+    float i = sin(half_delta_t) * 2.0;
+    local_x = (delta_vert / delta_t - p_track_width) * i;
+  }
+
+  float alpha = current_t - half_delta_t;
+  float x = cos(alpha) * local_x;
+  float y = sin(alpha) * local_x;
+
+  // xy is calculated internally using math standard but translated to what's intuitive
+  // where going forward from 0 degrees increases Y
+  output.x = -y;
+  output.y = x;
+
+  return output;
+}
+
+ez::pose Drive::solve_xy_horiz(float p_track_width, float current_t, float delta_horiz, float delta_t) {
+  pose output = {0.0, 0.0, 0.0};
+
+  // Figure out how far we've actually moved
+  float local_y = delta_horiz;
+  float half_delta_t = 0.0;
+  if (delta_t != 0) {
+    half_delta_t = delta_t / 2.0;
+    float i = sin(half_delta_t) * 2.0;
+    local_y = (delta_horiz / delta_t + p_track_width) * i;
+  }
+
+  float alpha = current_t - half_delta_t;
+  float x = -sin(alpha) * local_y;
+  float y = cos(alpha) * local_y;
+
+  // xy is calculated internally using math standard but translated to what's intuitive
+  // where going forward from 0 degrees increases Y
+  output.x = -y;
+  output.y = x;
+
+  return output;
+}
+// pose central_pose;
+// Tracking based on https://wiki.purduesigbots.com/software/odometry
+void Drive::ez_tracking_task() {
+  // Don't let this function run if odom is disabled
+  // and make sure all the "lasts" are 0
+  if (!imu_calibration_complete || !odometry_enabled) {
+    h_last = 0.0;
+    t_last = 0.0;
+    l_last = 0.0;
+    r_last = 0.0;
+    return;
+  }
+
+  // Decide on using a horiz tracker vs not
+
+  ez::tracking_wheel* h_sensor = odom_tracker_back != nullptr ? odom_tracker_back : odom_tracker_front;
+  bool h_tracker_enabled = h_sensor == odom_tracker_back ? odom_tracker_back_enabled : odom_tracker_front_enabled;
+  std::pair<float, float> h_cur_and_track = decide_vert_sensor(h_sensor, h_tracker_enabled);
+  float h_current = h_cur_and_track.first;
+  float h_track_width = h_cur_and_track.second;
+  // Calculate velocity based on horiz value
+  float h_ = h_current - h_last;
+  h_last = h_current;
+
+  // Decide on left ime vs left tracker
+  std::pair<float, float> l_cur_and_track = decide_vert_sensor(odom_tracker_left, odom_tracker_left_enabled, drive_sensor_left(), odom_ime_track_width_left);
+  float l_current = l_cur_and_track.first;
+  float l_track_width = l_cur_and_track.second;
+  // Calculate velocity based on left value
+  float l_ = l_current - l_last;
+  l_last = l_current;
+
+  // Decide on right ime vs right tracker
+  std::pair<float, float> r_cur_and_track = decide_vert_sensor(odom_tracker_right, odom_tracker_right_enabled, drive_sensor_right(), odom_ime_track_width_right);
+  float r_current = r_cur_and_track.first;
+  float r_track_width = r_cur_and_track.second;
+  // Calculate velocity based on left value
+  float r_ = r_current - r_last;
+  r_last = r_current;
+
+  // Angle and velocity
+  float t_current = -ez::util::to_rad(drive_imu_get());  // negative for math standard
+  float t_ = t_current - t_last;
+  t_last = t_current;
+
+  pose h_pose_ = solve_xy_horiz(h_track_width, t_current, h_, t_);
+  pose l_pose_ = solve_xy_vert(l_track_width, t_current, l_, t_);
+  pose r_pose_ = solve_xy_vert(r_track_width, t_current, r_, t_);
+
+  r_pose.x += r_pose_.x;
+  r_pose.y += r_pose_.y;
+  r_pose.x += h_pose_.x;
+  r_pose.y += h_pose_.y;
+
+  l_pose.x += l_pose_.x;
+  l_pose.y += l_pose_.y;
+  l_pose.x += h_pose_.x;
+  l_pose.y += h_pose_.y;
+
+  // Track width of 0, but the delta is avg of l+r
+  double avg = l_ + r_;
+  if (avg != 0.0)
+    avg /= 2.0;
+  pose central_pose_ = solve_xy_vert(0.0, t_current, avg, t_);
+  central_pose.x += central_pose_.x;
+  central_pose.y += central_pose_.y;
+  central_pose.x += h_pose_.x;
+  central_pose.y += h_pose_.y;
+
+  odom_current.x = central_pose.x;
+  odom_current.y = central_pose.y;
+
+  // If there is a single vert tracker, use it
+  if (odom_tracker_left_enabled != odom_tracker_right_enabled) {
+    if (odom_tracker_left_enabled) {
+      odom_current.x = l_pose.x;
+      odom_current.y = l_pose.y;
+    } else if (odom_tracker_right_enabled) {
+      odom_current.x = r_pose.x;
+      odom_current.y = r_pose.y;
+    }
+  }
+  // If both sides have a sensor (2 vert trackers or 0 trackers), let the user pick what side
+  //  defaults to left tracker and central ime
+  else {
+    // If using IMEs, use central pose
+    if (is_tracker == DRIVE_INTEGRATED) {
+      odom_current.x = central_pose.x;
+      odom_current.y = central_pose.y;
+    } else if (odom_use_left) {
+      odom_current.x = l_pose.x;
+      odom_current.y = l_pose.y;
+    } else {
+      odom_current.x = r_pose.x;
+      odom_current.y = r_pose.y;
+    }
+  }
+
+  odom_current.theta = drive_imu_get();
+
+  // This is used for PID as a "current" sensor value
+  // what this value actually is doesn't matter, it just needs to move with the correct sign
+  xy_current_fake = fabs(is_past_target({0.0, 0.0}, odom_pose_get()));
+  if (!was_odom_just_set)
+    xy_delta_fake = fabs(xy_current_fake - xy_last_fake);
+  else
+    was_odom_just_set = false;
+  xy_last_fake = xy_current_fake;
+
+  // printf("odom_ime_track_width_left %f   l_ %f   r_ %f   t_current %f\n", odom_ime_track_width_left, r_, t_, t_current);
+
+  // printf("left (%.2f, %.2f)", l_pose.x, l_pose.y);
+  // printf("   right (%.2f, %.2f)", r_pose.x, r_pose.y);
+  // printf("   current used (%.2f, %.2f, %.2f)      l delta: %.2f   r delta: %.2f", odom_current.x, odom_current.y, odom_current.theta, l_, r_);
+  // printf("        htw: %f\n", h_track_width);
+}

src/EZ-Template/drive/user_input.cpp

@@ -0,0 +1,357 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/PID.hpp"
+#include "EZ-Template/drive/drive.hpp"
+#include "pros/misc.h"
+
+void Drive::opcontrol_arcade_scaling(bool enable) { arcade_vector_scaling = enable; }
+bool Drive::opcontrol_arcade_scaling_enabled() { return arcade_vector_scaling; }
+
+// Set curve defaults
+void Drive::opcontrol_curve_default_set(double left, double right) {
+  left_curve_scale = left;
+  right_curve_scale = right;
+
+  save_l_curve_sd();
+  save_r_curve_sd();
+}
+
+std::vector<double> Drive::opcontrol_curve_default_get() {
+  return {left_curve_scale, right_curve_scale};
+}
+
+// Initialize curve SD card
+void Drive::opcontrol_curve_sd_initialize() {
+  // If no SD card, return
+  if (!ez::util::SD_CARD_ACTIVE) return;
+
+  FILE* l_usd_file_read;
+  // If file exists...
+  if ((l_usd_file_read = fopen("/usd/left_curve.txt", "r"))) {
+    char l_buf[5];
+    fread(l_buf, 1, 5, l_usd_file_read);
+    left_curve_scale = std::stof(l_buf);
+    fclose(l_usd_file_read);
+  }
+  // If file doesn't exist, create file
+  else {
+    save_l_curve_sd();  // Writing to a file that doesn't exist creates the file
+    printf("Created left_curve.txt\n");
+  }
+
+  FILE* r_usd_file_read;
+  // If file exists...
+  if ((r_usd_file_read = fopen("/usd/right_curve.txt", "r"))) {
+    char l_buf[5];
+    fread(l_buf, 1, 5, r_usd_file_read);
+    right_curve_scale = std::stof(l_buf);
+    fclose(r_usd_file_read);
+  }
+  // If file doesn't exist, create file
+  else {
+    save_r_curve_sd();  // Writing to a file that doesn't exist creates the file
+    printf("Created right_curve.txt\n");
+  }
+}
+
+// Save new left curve to SD card
+void Drive::save_l_curve_sd() {
+  // If no SD card, return
+  if (!ez::util::SD_CARD_ACTIVE) return;
+
+  FILE* usd_file_write = fopen("/usd/left_curve.txt", "w");
+  std::string in_str = std::to_string(left_curve_scale);
+  char const* in_c = in_str.c_str();
+  fputs(in_c, usd_file_write);
+  fclose(usd_file_write);
+}
+
+// Save new right curve to SD card
+void Drive::save_r_curve_sd() {
+  // If no SD card, return
+  if (!ez::util::SD_CARD_ACTIVE) return;
+
+  FILE* usd_file_write = fopen("/usd/right_curve.txt", "w");
+  std::string in_str = std::to_string(right_curve_scale);
+  char const* in_c = in_str.c_str();
+  fputs(in_c, usd_file_write);
+  fclose(usd_file_write);
+}
+
+void Drive::opcontrol_curve_buttons_left_set(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase) {
+  l_increase_.button = increase;
+  l_decrease_.button = decrease;
+}
+void Drive::opcontrol_curve_buttons_right_set(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase) {
+  r_increase_.button = increase;
+  r_decrease_.button = decrease;
+}
+
+std::vector<pros::controller_digital_e_t> Drive::opcontrol_curve_buttons_left_get() {
+  return {l_decrease_.button, r_decrease_.button};
+}
+
+std::vector<pros::controller_digital_e_t> Drive::opcontrol_curve_buttons_right_get() {
+  return {r_decrease_.button, r_decrease_.button};
+}
+
+// Increase / decrease left and right curves
+void Drive::l_increase() { left_curve_scale += 0.1; }
+void Drive::l_decrease() {
+  left_curve_scale -= 0.1;
+  left_curve_scale = left_curve_scale < 0 ? 0 : left_curve_scale;
+}
+void Drive::r_increase() { right_curve_scale += 0.1; }
+void Drive::r_decrease() {
+  right_curve_scale -= 0.1;
+  right_curve_scale = right_curve_scale < 0 ? 0 : right_curve_scale;
+}
+
+// Button press logic for increase/decrease curves
+void Drive::button_press(button_* input_name, int button, std::function<void()> change_curve, std::function<void()> save) {
+  // If button is pressed, increase the curve and set toggles.
+  if (button && !input_name->lock) {
+    change_curve();
+    input_name->lock = true;
+    input_name->release_reset = true;
+  }
+
+  // If the button is still held, check if it's held for 500ms.
+  // Then, increase the curve every 100ms by 0.1
+  else if (button && input_name->lock) {
+    input_name->hold_timer += ez::util::DELAY_TIME;
+    if (input_name->hold_timer > 500.0) {
+      input_name->increase_timer += ez::util::DELAY_TIME;
+      if (input_name->increase_timer > 100.0) {
+        change_curve();
+        input_name->increase_timer = 0;
+      }
+    }
+  }
+
+  // When button is released for 250ms, save the new curve value to the SD card
+  else if (!button) {
+    input_name->lock = false;
+    input_name->hold_timer = 0;
+
+    if (input_name->release_reset) {
+      input_name->release_timer += ez::util::DELAY_TIME;
+      if (input_name->release_timer > 250.0) {
+        save();
+        input_name->release_timer = 0;
+        input_name->release_reset = false;
+      }
+    }
+  }
+}
+
+// Toggle modifying curves with controller
+void Drive::opcontrol_curve_buttons_toggle(bool toggle) {
+  if (pid_tuner_on && toggle) {
+    printf("Cannot modify curve while PID Tuner is active!\n");
+    return;
+  }
+  disable_controller = toggle;
+  if (!disable_controller)
+    master.set_text(2, 0, "            ");
+}
+bool Drive::opcontrol_curve_buttons_toggle_get() { return disable_controller; }
+
+// Modify curves with button presses and display them to controller
+void Drive::opcontrol_curve_buttons_iterate() {
+  if (!disable_controller) return;  // True enables, false disables.
+
+  button_press(&l_increase_, master.get_digital(l_increase_.button), ([this] { this->l_increase(); }), ([this] { this->save_l_curve_sd(); }));
+  button_press(&l_decrease_, master.get_digital(l_decrease_.button), ([this] { this->l_decrease(); }), ([this] { this->save_l_curve_sd(); }));
+  if (!is_tank) {
+    button_press(&r_increase_, master.get_digital(r_increase_.button), ([this] { this->r_increase(); }), ([this] { this->save_r_curve_sd(); }));
+    button_press(&r_decrease_, master.get_digital(r_decrease_.button), ([this] { this->r_decrease(); }), ([this] { this->save_r_curve_sd(); }));
+  }
+
+  auto sl = util::to_string_with_precision(left_curve_scale, 1);
+  auto sr = util::to_string_with_precision(right_curve_scale, 1);
+  if (!is_tank)
+    master.set_text(2, 0, sl + "         " + sr);
+  else
+    master.set_text(2, 0, sl);
+}
+
+// Left curve function
+double Drive::opcontrol_curve_left(double x) {
+  if (left_curve_scale != 0) {
+    // if (CURVE_TYPE)
+    return (powf(2.718, -(left_curve_scale / 10)) + powf(2.718, (fabs(x) - 127) / 10) * (1 - powf(2.718, -(left_curve_scale / 10)))) * x;
+    // else
+    // return powf(2.718, ((abs(x)-127)*RIGHT_CURVE_SCALE)/100)*x;
+  }
+  return x;
+}
+
+// Right curve function
+double Drive::opcontrol_curve_right(double x) {
+  if (right_curve_scale != 0) {
+    // if (CURVE_TYPE)
+    return (powf(2.718, -(right_curve_scale / 10)) + powf(2.718, (fabs(x) - 127) / 10) * (1 - powf(2.718, -(right_curve_scale / 10)))) * x;
+    // else
+    // return powf(2.718, ((abs(x)-127)*RIGHT_CURVE_SCALE)/100)*x;
+  }
+  return x;
+}
+
+// Set active brake constant
+void Drive::opcontrol_drive_activebrake_set(double kp, double ki, double kd, double start_i) {
+  left_activebrakePID.constants_set(kp, ki, kd, start_i);
+  right_activebrakePID.constants_set(kp, ki, kd, start_i);
+  opcontrol_drive_activebrake_targets_set();
+}
+
+// Get active brake kp constant
+double Drive::opcontrol_drive_activebrake_get() { return left_activebrakePID.constants.kp; }
+
+// Get active brake kp constant
+PID::Constants Drive::opcontrol_drive_activebrake_constants_get() { return left_activebrakePID.constants; }
+
+// Set joystick threshold
+void Drive::opcontrol_joystick_threshold_set(int threshold) { JOYSTICK_THRESHOLD = abs(threshold); }
+int Drive::opcontrol_joystick_threshold_get() { return JOYSTICK_THRESHOLD; }
+
+void Drive::opcontrol_drive_activebrake_targets_set() {
+  left_activebrakePID.target_set(drive_sensor_left());
+  right_activebrakePID.target_set(drive_sensor_right());
+}
+
+void Drive::opcontrol_drive_sensors_reset() {
+  if (util::AUTON_RAN) {
+    opcontrol_drive_activebrake_targets_set();
+    util::AUTON_RAN = false;
+  }
+}
+
+void Drive::opcontrol_joystick_practicemode_toggle(bool toggle) { practice_mode_is_on = toggle; }
+bool Drive::opcontrol_joystick_practicemode_toggle_get() { return practice_mode_is_on; }
+
+void Drive::opcontrol_drive_reverse_set(bool toggle) { is_reversed = toggle; }
+bool Drive::opcontrol_drive_reverse_get() { return is_reversed; }
+
+void Drive::opcontrol_joystick_threshold_iterate(int l_stick, int r_stick) {
+  double l_out = 0.0, r_out = 0.0;
+
+  // Check the motors are being set to power
+  if (abs(l_stick) > 0 || abs(r_stick) > 0) {
+    if (left_activebrakePID.constants_set_check()) opcontrol_drive_activebrake_targets_set();  // Update active brake PID targets
+
+    if (practice_mode_is_on && (abs(l_stick) > 120 || abs(r_stick) > 120)) {
+      l_out = 0.0;
+      r_out = 0.0;
+    } else if (!is_reversed) {
+      l_out = l_stick;
+      r_out = r_stick;
+    } else {
+      l_out = -r_stick;
+      r_out = -l_stick;
+    }
+
+  }
+  // When joys are released, run active brake (P) on drive
+  else {
+    l_out = left_activebrakePID.compute(drive_sensor_left());
+    r_out = right_activebrakePID.compute(drive_sensor_right());
+  }
+
+  // Constrain output between 127 and -127
+  if (arcade_vector_scaling) {
+    double faster_side = fmax(fabs(l_out), fabs(r_out));
+    if (faster_side > 127.0) {
+      l_out *= (127.0 / faster_side);
+      r_out *= (127.0 / faster_side);
+    }
+  }
+
+  // Constrain left and right outputs to the user set max speed
+  // the user set max speed is defaulted to 127
+  l_out *= (opcontrol_speed_max / 127.0);
+  r_out *= (opcontrol_speed_max / 127.0);
+
+  // Ensure output is within speed limit
+  l_out = l_out > opcontrol_speed_max ? opcontrol_speed_max : l_out;
+  r_out = r_out > opcontrol_speed_max ? opcontrol_speed_max : r_out;
+
+  drive_set(l_out, r_out);
+}
+
+void Drive::opcontrol_speed_max_set(int speed) { opcontrol_speed_max = (double)speed; }
+int Drive::opcontrol_speed_max_get() { return (int)opcontrol_speed_max; }
+
+// Clip joysticks based on joystick threshold
+int Drive::clipped_joystick(int joystick) { return abs(joystick) < JOYSTICK_THRESHOLD ? 0 : joystick; }
+
+// Tank control
+void Drive::opcontrol_tank() {
+  is_tank = true;
+  opcontrol_drive_sensors_reset();
+
+  // Toggle for controller curve
+  opcontrol_curve_buttons_iterate();
+
+  auto analog_left_value = master.get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y);
+  auto analog_right_value = master.get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_Y);
+
+  // Put the joysticks through the curve function
+  int l_stick = opcontrol_curve_left(clipped_joystick(master.get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y)));
+  int r_stick = opcontrol_curve_left(clipped_joystick(master.get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_Y)));
+
+  // Set robot to l_stick and r_stick, check joystick threshold, set active brake
+  opcontrol_joystick_threshold_iterate(l_stick, r_stick);
+}
+
+// Arcade standard
+void Drive::opcontrol_arcade_standard(e_type stick_type) {
+  is_tank = false;
+  opcontrol_drive_sensors_reset();
+
+  // Toggle for controller curve
+  opcontrol_curve_buttons_iterate();
+
+  int fwd_stick, turn_stick;
+  // Check arcade type (split vs single, normal vs flipped)
+  if (stick_type == SPLIT) {
+    // Put the joysticks through the curve function
+    fwd_stick = opcontrol_curve_left(clipped_joystick(master.get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y)));
+    turn_stick = opcontrol_curve_right(clipped_joystick(master.get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X)));
+  } else if (stick_type == SINGLE) {
+    // Put the joysticks through the curve function
+    fwd_stick = opcontrol_curve_left(clipped_joystick(master.get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y)));
+    turn_stick = opcontrol_curve_right(clipped_joystick(master.get_analog(pros::E_CONTROLLER_ANALOG_LEFT_X)));
+  }
+
+  // Set robot to l_stick and r_stick, check joystick threshold, set active brake
+  opcontrol_joystick_threshold_iterate(fwd_stick + turn_stick, fwd_stick - turn_stick);
+}
+
+// Arcade control flipped
+void Drive::opcontrol_arcade_flipped(e_type stick_type) {
+  is_tank = false;
+  opcontrol_drive_sensors_reset();
+
+  // Toggle for controller curve
+  opcontrol_curve_buttons_iterate();
+
+  int turn_stick, fwd_stick;
+  // Check arcade type (split vs single, normal vs flipped)
+  if (stick_type == SPLIT) {
+    // Put the joysticks through the curve function
+    fwd_stick = opcontrol_curve_right(clipped_joystick(master.get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_Y)));
+    turn_stick = opcontrol_curve_left(clipped_joystick(master.get_analog(pros::E_CONTROLLER_ANALOG_LEFT_X)));
+  } else if (stick_type == SINGLE) {
+    // Put the joysticks through the curve function
+    fwd_stick = opcontrol_curve_right(clipped_joystick(master.get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_Y)));
+    turn_stick = opcontrol_curve_left(clipped_joystick(master.get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X)));
+  }
+
+  // Set robot to l_stick and r_stick, check joystick threshold, set active brake
+  opcontrol_joystick_threshold_iterate(fwd_stick + turn_stick, fwd_stick - turn_stick);
+}

src/EZ-Template/piston.cpp

@@ -0,0 +1,46 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/piston.hpp"
+
+using namespace ez;
+
+// Constructor for one piston
+Piston::Piston(int input_port, bool default_state)
+    : piston(input_port, default_state) {
+  reversed = default_state;
+}
+
+// Constructor for one piston plugged into expander
+Piston::Piston(int input_port, int expander_smart_port, bool default_state)
+    : piston({expander_smart_port, input_port}, default_state) {
+  reversed = default_state;
+}
+
+// Set piston
+void Piston::set(bool input) {
+  piston.set_value(reversed ? !input : input);
+  current = input;
+}
+
+// Get the current state
+bool Piston::get() { return current; }
+
+// Toggle for user control
+void Piston::button_toggle(int toggle) {
+  if (toggle && !last_press) {
+    set(!get());
+  }
+  last_press = toggle;
+}
+
+// Two button control for piston
+void Piston::buttons(int active, int deactive) {
+  if (active && !get())
+    set(true);
+  else if (deactive && get())
+    set(false);
+}

src/EZ-Template/sdcard.cpp

@@ -0,0 +1,175 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "sdcard.hpp"
+
+#include <filesystem>
+
+#include "auton_selector.hpp"
+#include "liblvgl/llemu.hpp"
+#include "pros/llemu.hpp"
+#include "util.hpp"
+
+namespace ez::as {
+AutonSelector auton_selector{};
+
+void auto_sd_update() {
+  // If no SD card, return
+  if (!ez::util::SD_CARD_ACTIVE) return;
+
+  FILE* usd_file_write = fopen("/usd/auto.txt", "w");
+  std::string cp_str = std::to_string(auton_selector.auton_page_current);
+  char const* cp_c = cp_str.c_str();
+  fputs(cp_c, usd_file_write);
+  fclose(usd_file_write);
+}
+
+void auton_selector_initialize() {
+  // If no SD card, return
+  if (!ez::util::SD_CARD_ACTIVE) return;
+
+  FILE* as_usd_file_read;
+  // If file exists...
+  if ((as_usd_file_read = fopen("/usd/auto.txt", "r"))) {
+    char a_buf[10];
+    fread(a_buf, 1, 10, as_usd_file_read);
+    ez::as::auton_selector.auton_page_current = std::stof(a_buf);
+    fclose(as_usd_file_read);
+  }
+  // If file doesn't exist, create file
+  else {
+    auto_sd_update();  // Writing to a file that doesn't exist creates the file
+    printf("Created auto.txt\n");
+  }
+
+  if (ez::as::auton_selector.auton_page_current > ez::as::auton_selector.auton_count - 1 || ez::as::auton_selector.auton_page_current < 0) {
+    ez::as::auton_selector.auton_page_current = 0;
+    ez::as::auto_sd_update();
+  }
+}
+
+void print_page() {
+  if (((auton_selector.auton_count - 1 - amount_of_blank_pages) - auton_selector.auton_page_current) >= 0) {
+    auto_sd_update();
+    auton_selector.selected_auton_print();
+  } else {
+    for (int i = 0; i < 8; i++)
+      pros::lcd::clear_line(i);
+    screen_print("Page " + std::to_string(auton_selector.auton_page_current + 1) + " - Blank page " + std::to_string(page_blank_current() + 1));
+  }
+  if (page_blank_current() < 0)
+    auton_selector.last_auton_page_current = auton_selector.auton_page_current;
+}
+
+void page_up() {
+  if (util::sgn(page_blank_current()) == -1) auton_selector.last_auton_page_current = auton_selector.auton_page_current;
+  if (auton_selector.auton_page_current == auton_selector.auton_count - 1)
+    auton_selector.auton_page_current = 0;
+  else
+    auton_selector.auton_page_current++;
+  print_page();
+}
+
+void page_down() {
+  if (util::sgn(page_blank_current()) == -1) auton_selector.last_auton_page_current = auton_selector.auton_page_current;
+  if (auton_selector.auton_page_current == 0)
+    auton_selector.auton_page_current = auton_selector.auton_count - 1;
+  else
+    auton_selector.auton_page_current--;
+  print_page();
+}
+
+int page_blank_current() {
+  return (auton_selector.auton_count - amount_of_blank_pages - auton_selector.auton_page_current) * -1;
+}
+
+int amount_of_blank_pages = 0;
+bool page_blank_is_on(int page) {
+  if (page + 1 > amount_of_blank_pages) {
+    auton_selector.auton_count -= amount_of_blank_pages;
+    amount_of_blank_pages = page + 1;
+    auton_selector.auton_count += amount_of_blank_pages;
+  }
+  if (page_blank_current() == page)
+    return true;
+  return false;
+}
+
+void page_blank_remove(int page) {
+  if (amount_of_blank_pages >= page + 1) {
+    auton_selector.auton_count -= amount_of_blank_pages;
+    amount_of_blank_pages -= page + 1;
+    auton_selector.auton_count += amount_of_blank_pages;
+  }
+  auton_selector.auton_page_current = auton_selector.last_auton_page_current;
+  print_page();
+}
+
+void page_blank_remove_all() {
+  page_blank_remove(amount_of_blank_pages - 1);
+}
+
+int page_blank_amount() {
+  return amount_of_blank_pages;
+}
+
+void initialize() {
+  // Initialize auto selector and LLEMU
+  pros::lcd::initialize();
+  ez::as::auton_selector_initialize();
+
+  // Callbacks for auto selector
+  print_page();
+  pros::lcd::register_btn0_cb(ez::as::page_down);
+  pros::lcd::register_btn2_cb(ez::as::page_up);
+
+  auton_selector_running = true;
+}
+
+void shutdown() {
+  pros::lcd::shutdown();
+  pros::lcd::register_btn0_cb(nullptr);
+  pros::lcd::register_btn2_cb(nullptr);
+
+  auton_selector_running = false;
+}
+
+bool enabled() { return auton_selector_running; }
+
+bool turn_off = false;
+
+// Using a button to control the lcd
+pros::adi::DigitalIn* limit_switch_left = nullptr;
+pros::adi::DigitalIn* limit_switch_right = nullptr;
+pros::Task limit_switch_task(ez::as::limitSwitchTask);
+void limit_switch_lcd_initialize(pros::adi::DigitalIn* right_limit, pros::adi::DigitalIn* left_limit) {
+  if (!left_limit && !right_limit) {
+    delete limit_switch_left;
+    delete limit_switch_right;
+    if (pros::millis() <= 100)
+      turn_off = true;
+    return;
+  }
+  turn_off = false;
+  limit_switch_right = right_limit;
+  limit_switch_left = left_limit;
+  limit_switch_task.resume();
+}
+
+void limitSwitchTask() {
+  while (true) {
+    if (limit_switch_right && limit_switch_right->get_new_press())
+      ez::as::page_up();
+    else if (limit_switch_left && limit_switch_left->get_new_press())
+      ez::as::page_down();
+
+    if (pros::millis() >= 500 && turn_off)
+      limit_switch_task.suspend();
+
+    pros::delay(50);
+  }
+}
+}  // namespace ez::as

src/EZ-Template/slew.cpp

@@ -0,0 +1,61 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/api.hpp"
+
+using namespace ez;
+
+// Constructor
+slew::slew() {}
+slew::slew(double distance, int minimum_speed) {
+  constants_set(distance, minimum_speed);
+}
+
+// Set constants
+void slew::constants_set(double distance, int minimum_speed) {
+  constants.min_speed = minimum_speed;
+  constants.distance_to_travel = distance;
+}
+
+void slew::speed_max_set(double speed) { max_speed = speed; }
+double slew::speed_max_get() { return max_speed; }
+
+bool slew::enabled() { return is_enabled; }                  // Is slew currently enabled?
+double slew::output() { return last_output; }                // Returns output
+slew::Constants slew::constants_get() { return constants; }  // Get constants
+
+// Initialize for the movement
+void slew::initialize(bool enabled, double maximum_speed, double target, double current) {
+  is_enabled = maximum_speed < constants.min_speed ? false : enabled;
+  max_speed = maximum_speed;
+
+  sign = util::sgn(target - current);
+  x_intercept = current + ((constants.distance_to_travel * sign));
+  y_intercept = max_speed * sign;
+  slope = ((sign * constants.min_speed) - y_intercept) / (x_intercept - 0 - current);  // y2-y1 / x2-x1
+}
+
+// Iterate the slew throughout the movement
+double slew::iterate(double current) {
+  // Is slew still on?
+  if (is_enabled) {
+    // Error is distance away from completed slew
+    error = x_intercept - current;
+
+    // When the sign of error flips, slew is completed
+    if (util::sgn(error) != sign)
+      is_enabled = false;
+
+    // Return y=mx+b
+    else if (util::sgn(error) == sign)
+      last_output = ((slope * error) + y_intercept) * sign;
+  } else {
+    // When slew is completed, return max speed
+    last_output = max_speed;
+  }
+
+  return last_output;
+}

src/EZ-Template/tracking_wheel.cpp

@@ -0,0 +1,95 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/tracking_wheel.hpp"
+
+#include "EZ-Template/util.hpp"
+
+using namespace ez;
+
+// ADI Encoder
+tracking_wheel::tracking_wheel(std::vector<int> ports, double wheel_diameter, double distance_to_center, double ratio)
+    : adi_encoder(abs(ports[0]), abs(ports[1]), util::reversed_active(ports[0])),
+      smart_encoder(-1) {
+  IS_TRACKER = DRIVE_ADI_ENCODER;
+
+  distance_to_center_set(distance_to_center);
+  wheel_diameter_set(wheel_diameter);
+  ratio_set(ratio);
+  ticks_per_rev_set(360.0);
+}
+
+// ADI Encoder in 3-wire expander
+tracking_wheel::tracking_wheel(int smart_port, std::vector<int> ports, double wheel_diameter, double distance_to_center, double ratio)
+    : adi_encoder({abs(smart_port), abs(ports[0]), abs(ports[1])}, util::reversed_active(ports[0])),
+      smart_encoder(-1) {
+  IS_TRACKER = DRIVE_ADI_ENCODER;
+
+  distance_to_center_set(distance_to_center);
+  wheel_diameter_set(wheel_diameter);
+  ratio_set(ratio);
+  ticks_per_rev_set(360.0);
+}
+
+// Rotation Sensor
+tracking_wheel::tracking_wheel(int port, double wheel_diameter, double distance_to_center, double ratio)
+    : adi_encoder(-1, -1, false),
+      smart_encoder(abs(port)) {
+  IS_TRACKER = DRIVE_ROTATION;
+  smart_encoder.set_reversed(util::reversed_active(port));
+
+  distance_to_center_set(distance_to_center);
+  wheel_diameter_set(wheel_diameter);
+  ratio_set(ratio);
+  ticks_per_rev_set(36000.0);
+}
+
+void tracking_wheel::ticks_per_rev_set(double input) { ENCODER_TICKS_PER_REV = fabs(input); }
+double tracking_wheel::ticks_per_rev_get() { return ENCODER_TICKS_PER_REV; }
+
+void tracking_wheel::ratio_set(double input) { RATIO = fabs(input); }
+double tracking_wheel::ratio_get() { return RATIO; }
+
+void tracking_wheel::distance_to_center_flip_set(bool input) { IS_FLIPPED = input; }
+bool tracking_wheel::distance_to_center_flip_get() { return IS_FLIPPED; }
+void tracking_wheel::distance_to_center_set(double input) { DISTANCE_TO_CENTER = fabs(input); }
+double tracking_wheel::distance_to_center_get() {
+  int flipped = IS_FLIPPED ? -1 : 1;
+  return DISTANCE_TO_CENTER * flipped;
+}
+
+void tracking_wheel::wheel_diameter_set(double input) { WHEEL_DIAMETER = fabs(input); }
+double tracking_wheel::wheel_diameter_get() { return WHEEL_DIAMETER; }
+
+double tracking_wheel::ticks_per_inch() {
+  double c = WHEEL_DIAMETER * M_PI;
+  WHEEL_TICK_PER_REV = ENCODER_TICKS_PER_REV * RATIO;
+  return WHEEL_TICK_PER_REV / c;
+}
+
+double tracking_wheel::get_raw() {
+  if (IS_TRACKER == DRIVE_ROTATION) {
+    return smart_encoder.get_position();
+  }
+  return adi_encoder.get_value();
+}
+double tracking_wheel::get() {
+  double tpi = ticks_per_inch();
+  double raw = get_raw();
+  if (tpi != 0)
+    return raw / tpi;
+  return raw;
+}
+
+void tracking_wheel::reset() {
+  if (IS_TRACKER == DRIVE_ADI_ENCODER) {
+    adi_encoder.reset();
+    return;
+  } else if (IS_TRACKER == DRIVE_ROTATION) {
+    smart_encoder.reset_position();
+    return;
+  }
+}

src/EZ-Template/util.cpp

@@ -0,0 +1,285 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/api.hpp"
+#include "liblvgl/llemu.hpp"
+#include "pros/llemu.hpp"
+#include "pros/misc.h"
+
+pros::Controller master(pros::E_CONTROLLER_MASTER);
+
+namespace ez {
+int mode = DISABLE;
+
+void ez_template_print() {
+  std::cout << R"(
+
+
+    _____ ______   _____                    _       _
+   |  ___|___  /  |_   _|                  | |     | |
+   | |__    / /_____| | ___ _ __ ___  _ __ | | __ _| |_ ___
+   |  __|  / /______| |/ _ \ '_ ` _ \| '_ \| |/ _` | __/ _ \
+   | |___./ /___    | |  __/ | | | | | |_) | | (_| | ||  __/
+   \____/\_____/    \_/\___|_| |_| |_| .__/|_|\__,_|\__\___|
+                                     | |
+                                     |_|
+)" << '\n';
+}
+std::string get_last_word(std::string text) {
+  std::string word = "";
+  for (int i = text.length() - 1; i >= 0; i--) {
+    if (text[i] != ' ') {
+      word += text[i];
+    } else {
+      std::reverse(word.begin(), word.end());
+      return word;
+    }
+  }
+  std::reverse(word.begin(), word.end());
+  return word;
+}
+std::string get_rest_of_the_word(std::string text, int position) {
+  std::string word = "";
+  for (int i = position; i < text.length(); i++) {
+    if (text[i] != ' ' && text[i] != '\n') {
+      word += text[i];
+    } else {
+      return word;
+    }
+  }
+  return word;
+}
+
+void screen_print(std::string text, int line) {
+  int CurrAutoLine = line;
+  std::vector<string> texts = {};
+  std::string temp = "";
+
+  for (int i = 0; i < text.length(); i++) {
+    if (text[i] != '\n' && temp.length() + 1 > 38) {
+      auto last_word = get_last_word(temp);
+      if (last_word == temp) {
+        texts.push_back(temp);
+        temp = text[i];
+      } else {
+        int size = last_word.length();
+
+        auto rest_of_word = get_rest_of_the_word(text, i);
+        temp.erase(temp.length() - size, size);
+        texts.push_back(temp);
+        last_word += rest_of_word;
+        i += rest_of_word.length();
+        temp = last_word;
+        if (i >= text.length() - 1) {
+          texts.push_back(temp);
+          break;
+        }
+      }
+    }
+    if (i >= text.length() - 1) {
+      temp += text[i];
+      texts.push_back(temp);
+      temp = "";
+      break;
+    } else if (text[i] == '\n') {
+      texts.push_back(temp);
+      temp = "";
+    } else {
+      temp += text[i];
+    }
+  }
+  for (auto i : texts) {
+    if (CurrAutoLine > 7) {
+      pros::lcd::clear();
+      pros::lcd::set_text(line, "Out of Bounds. Print Line is too far down");
+      return;
+    }
+    pros::lcd::clear_line(CurrAutoLine);
+    pros::lcd::set_text(CurrAutoLine, i);
+    CurrAutoLine++;
+  }
+}
+
+std::string exit_to_string(exit_output input) {
+  switch ((int)input) {
+    case RUNNING:
+      return "Running";
+    case SMALL_EXIT:
+      return "Small";
+    case BIG_EXIT:
+      return "Big";
+    case VELOCITY_EXIT:
+      return "Velocity";
+    case mA_EXIT:
+      return "mA";
+    case ERROR_NO_CONSTANTS:
+      return "Error: Exit condition constants not set!";
+    default:
+      return "Error: Out of bounds!";
+  }
+
+  return "Error: Out of bounds!";
+}
+namespace util {
+bool AUTON_RAN = true;
+
+int places_after_decimal(double input, int min) {
+  std::string in = std::to_string(input);
+  int places_after_decimal = 6;
+  for (int i = in.length() - 1; i > 0; i--) {
+    if (in[i] == '.')
+      break;
+
+    if (in[i] == '0')
+      places_after_decimal--;
+    else
+      break;
+  }
+  return places_after_decimal < min ? min : places_after_decimal;
+}
+
+std::string to_string_with_precision(double input, int n) {
+  std::ostringstream out;
+  out << std::fixed << std::setprecision(n) << input;
+  return out.str();
+}
+
+bool reversed_active(double input) {
+  if (input < 0) return true;
+  return false;
+}
+
+int sgn(double input) {
+  if (input > 0)
+    return 1;
+  else if (input < 0)
+    return -1;
+  return 0;
+}
+
+double clamp(double input, double max, double min) {
+  if (input > max)
+    return max;
+  else if (input < min)
+    return min;
+  return input;
+}
+
+double clamp(double input, double max) { return clamp(input, fabs(max), -fabs(max)); }
+
+// Conversions from deg to rad and rad to deg
+double to_deg(double input) { return input * (180 / M_PI); }
+double to_rad(double input) { return input * (M_PI / 180); }
+
+// Finds error in shortest angle to point
+double absolute_angle_to_point(pose itarget, pose icurrent) {
+  // Difference in target to current (legs of triangle)
+  double x_error = itarget.x - icurrent.x;
+  double y_error = itarget.y - icurrent.y;
+
+  // Displacement of error
+  double error = to_deg(atan2(x_error, y_error));
+  return error;
+}
+
+// Outputs a target that will get you there the fastest
+double turn_shortest(double target, double current, bool print) {
+  if (print) printf("SHORTEST   Target: %.2f   Current: %.2f      New Target: ", target, current);
+  double error = target - current;
+  if (fabs(error) < 180.0) {
+    if (print) printf("%.2f\n", target);
+    return target;
+  }
+  double new_target = target;
+
+  while (error > 180) {
+    new_target -= 360;
+    error = new_target - current;
+  }
+  while (error < -180) {
+    new_target += 360;
+    error = new_target - current;
+  }
+
+  if (new_target - current == 0.0) {
+    if (print) printf("%.2f\n", target);
+    return current;
+  }
+
+  if (print) printf("%.2f\n", new_target);
+  return new_target;
+}
+
+// Outputs a target that will get you there the slowest
+double turn_longest(double target, double current, bool print) {
+  if (print) printf("LONGEST   Target: %.2f   Current: %.2f      New Target: ", target, current);
+  double shortest_target = turn_shortest(target, current, false);
+  double new_target = target;
+
+  double error = shortest_target - current;
+  new_target = shortest_target - (360 * util::sgn(error));
+
+  if (print) printf("%.2f\n", new_target);
+  return new_target;
+}
+
+// Outputs angle within 180 t0 -180
+double wrap_angle(double theta) {
+  while (theta > 180) theta -= 360;
+  while (theta < -180) theta += 360;
+  return theta;
+}
+
+// Find shortest distance to point
+double distance_to_point(pose itarget, pose icurrent) {
+  // Difference in target to current (legs of triangle)
+  double x_error = (itarget.x - icurrent.x);
+  double y_error = (itarget.y - icurrent.y);
+
+  // Hypotenuse of triangle
+  double distance = hypot(x_error, y_error);
+
+  return distance;
+}
+
+// Uses input as hypot to find the new xy
+pose vector_off_point(double added, pose icurrent) {
+  double x_error = sin(to_rad(icurrent.theta)) * added;
+  double y_error = cos(to_rad(icurrent.theta)) * added;
+
+  pose output;
+  output.x = x_error + icurrent.x;
+  output.y = y_error + icurrent.y;
+  output.theta = icurrent.theta;
+  return output;
+}
+
+pose united_pose_to_pose(united_pose input) {
+  pose output = {0, 0, 0};
+  output.x = input.x.convert(okapi::inch);
+  output.y = input.y.convert(okapi::inch);
+  if (input.theta == p_ANGLE_NOT_SET)
+    output.theta = ANGLE_NOT_SET;
+  else
+    output.theta = input.theta.convert(okapi::degree);
+  return output;
+}
+
+std::vector<odom> united_odoms_to_odoms(std::vector<united_odom> inputs) {
+  std::vector<odom> output;
+  for (int i = 0; i < inputs.size(); i++) {
+    pose new_pose = united_pose_to_pose(inputs[i].target);
+    output.push_back({{new_pose}, inputs[i].drive_direction, inputs[i].max_xy_speed, inputs[i].turn_behavior});
+  }
+  return output;
+}
+
+odom united_odom_to_odom(united_odom input) {
+  return united_odoms_to_odoms({input})[0];
+}
+
+}  // namespace util
+}  // namespace ez