drive.hpp

Go to the documentation of this file.

/*
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/.
*/
 
#pragma once
 
#include <functional>
#include <iostream>
#include <tuple>
 
#include "LightLib/control/pid.hpp"
#include "LightLib/control/slew.hpp"
#include "LightLib/drive/tracking_wheel.hpp"
#include "LightLib/util/util.hpp"
#include "light/motor_group.hpp"
#include "light/motors.h"
#include "light/units.hpp"
 
using namespace light;
 
namespace light {
 
using PoseGetterFn = pose (*)();
using PoseSetterFn = void (*)(pose);
 
void register_pose_source(PoseGetterFn getter, PoseSetterFn setter);
 
extern int DRIVE_SPEED;
extern int TURN_SPEED;
extern int SWING_SPEED;
 
class Drive {
 public:
  int JOYSTICK_THRESHOLD;
 
  pros::motor_brake_mode_e_t CURRENT_BRAKE = pros::E_MOTOR_BRAKE_COAST;
 
  int CURRENT_MA = 2500;
 
  e_swing current_swing;
 
  std::vector<pros::Motor> left_motors;
 
  std::vector<pros::Motor> right_motors;
 
  std::vector<int> pto_active;
 
  pros::Imu imu;
 
  pros::adi::Encoder left_tracker;
 
  pros::adi::Encoder right_tracker;
 
  pros::Rotation left_rotation;
 
  pros::Rotation right_rotation;
 
  tracking_wheel* odom_tracker_left;
 
  tracking_wheel* odom_tracker_right;
 
  tracking_wheel* odom_tracker_front;
 
  tracking_wheel* odom_tracker_back;
 
  PID headingPID;
  PID turnPID;
  PID leftPID;
  PID rightPID;
  PID forward_drivePID;
  PID backward_drivePID;
  PID fwd_rev_drivePID;
  PID swingPID;
  PID forward_swingPID;
  PID backward_swingPID;
  PID fwd_rev_swingPID;
  PID xyPID;
  PID current_a_odomPID;
  PID boomerangPID;
  PID odom_angularPID;
  PID internal_leftPID;
  PID internal_rightPID;
  PID left_activebrakePID;
  PID right_activebrakePID;
 
  light::slew slew_left;
  light::slew slew_right;
  light::slew slew_forward;
  light::slew slew_backward;
  light::slew slew_turn;
  light::slew slew_swing_forward;
  light::slew slew_swing_backward;
  light::slew slew_swing;
 
  double friction_kS = 0.0;
  double friction_kV = 0.0;
  double friction_kQ = 0.0;
 
  void friction_constants_set(double kS, double kV = 0.0, double kQ = 0.0);
 
  std::vector<double> friction_constants_get();
 
  double friction_ff(double v_target);
 
  void slew_swing_constants_set(okapi::QLength distance, int min_speed);
 
  void slew_swing_constants_forward_set(okapi::QLength distance, int min_speed);
 
  void slew_swing_constants_backward_set(okapi::QLength distance, int min_speed);
 
  void slew_swing_constants_set(okapi::QAngle distance, int min_speed);
 
  void slew_swing_constants_forward_set(okapi::QAngle distance, int min_speed);
 
  void slew_swing_constants_backward_set(okapi::QAngle distance, int min_speed);
 
  void slew_turn_constants_set(okapi::QAngle distance, int min_speed);
 
  void slew_drive_constants_forward_set(okapi::QLength distance, int min_speed);
 
  void slew_drive_constants_backward_set(okapi::QLength distance, int min_speed);
 
  void slew_drive_constants_set(okapi::QLength distance, int min_speed);
 
  void slew_drive_set(bool slew_on);
 
  void slew_drive_forward_set(bool slew_on);
 
  bool slew_drive_forward_get();
 
  void slew_drive_backward_set(bool slew_on);
 
  bool slew_drive_backward_get();
 
  void slew_swing_set(bool slew_on);
 
  void slew_swing_forward_set(bool slew_on);
 
  bool slew_swing_forward_get();
 
  void slew_swing_backward_set(bool slew_on);
 
  bool slew_swing_backward_get();
 
  void slew_turn_set(bool slew_on);
 
  bool slew_turn_get();
 
  void slew_odom_reenable(bool reenable);
 
  bool slew_odom_reenabled();
 
  e_mode mode;
 
  void drive_mode_set(e_mode p_mode, bool stop_drive = true);
 
  e_mode drive_mode_get();
 
  void initialize();
 
  pros::Task ez_auto;
 
  Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio = 1.0);
 
  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) __attribute__((deprecated("Use the integrated encoder constructor with odom_tracker_left_set() and odom_tracker_right_set() instead!")));
 
  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) __attribute__((deprecated("Use the integrated encoder constructor with odom_tracker_left_set() and odom_tracker_right_set() instead!")));
 
  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) __attribute__((deprecated("Use the integrated encoder constructor with odom_tracker_left_set() and odom_tracker_right_set() instead!")));
 
  void drive_defaults_set();
 
  //
  // General Odometry
  //
 
  void ez_tracking_task();
 
  void odom_enable(bool input);
 
  bool odom_enabled();
 
  void drive_width_set(double input);
 
  void drive_width_set(okapi::QLength p_input);
 
  double drive_width_get();
 
  void odom_x_set(double x);
 
  void odom_x_set(okapi::QLength p_x);
 
  double odom_x_get();
 
  void odom_y_set(double y);
 
  void odom_y_set(okapi::QLength p_y);
 
  double odom_y_get();
 
  void odom_theta_set(double a);
 
  void odom_theta_set(okapi::QAngle p_a);
 
  double odom_theta_get();
 
  void odom_pose_set(pose itarget);
 
  void odom_pose_set(united_pose itarget);
 
  void odom_xy_set(double x, double y);
 
  void odom_xy_set(okapi::QLength p_x, okapi::QLength p_y);
 
  void odom_xyt_set(double x, double y, double t);
 
  void odom_xyt_set(okapi::QLength p_x, okapi::QLength p_y, okapi::QAngle p_t);
 
  pose odom_pose_get();
 
  void odom_reset();
 
  void odom_x_flip(bool flip = true);
 
  bool odom_x_direction_get();
 
  void odom_y_flip(bool flip = true);
 
  bool odom_y_direction_get();
 
  void odom_theta_flip(bool flip = true);
 
  bool odom_theta_direction_get();
 
  void odom_boomerang_dlead_set(double input);
 
  double odom_boomerang_dlead_get();
 
  void odom_boomerang_distance_set(double distance);
 
  void odom_boomerang_distance_set(okapi::QLength p_distance);
 
  double odom_boomerang_distance_get();
 
  void odom_turn_bias_set(double bias);
 
  double odom_turn_bias_get();
 
  void odom_path_spacing_set(double spacing);
 
  void odom_path_spacing_set(okapi::QLength p_spacing);
 
  double odom_path_spacing_get();
 
  void odom_path_smooth_constants_set(double weight_smooth, double weight_data, double tolerance);
 
  std::vector<double> odom_path_smooth_constants_get();
 
  void odom_path_print();
 
  void odom_look_ahead_set(double distance);
 
  void odom_look_ahead_set(okapi::QLength p_distance);
 
  double odom_look_ahead_get();
 
  void odom_tracker_left_set(tracking_wheel* input);
 
  void odom_tracker_right_set(tracking_wheel* input);
 
  void odom_tracker_front_set(tracking_wheel* input);
 
  void odom_tracker_back_set(tracking_wheel* input);
 
  void pid_angle_behavior_set(e_angle_behavior behavior);
 
  void pid_turn_behavior_set(e_angle_behavior behavior);
 
  void pid_swing_behavior_set(e_angle_behavior behavior);
 
  void pid_odom_behavior_set(e_angle_behavior behavior);
 
  e_angle_behavior pid_turn_behavior_get();
 
  e_angle_behavior pid_swing_behavior_get();
 
  e_angle_behavior pid_odom_behavior_get();
 
  void pid_angle_behavior_tolerance_set(okapi::QAngle p_tolerance);
 
  void pid_angle_behavior_tolerance_set(double tolerance);
 
  double pid_angle_behavior_tolerance_get();
 
  void pid_angle_behavior_bias_set(e_angle_behavior behavior);
 
  e_angle_behavior pid_angle_behavior_bias_get();
 
  //
  // User Control
  //
 
  void opcontrol_tank();
 
  void opcontrol_arcade_standard(e_type stick_type);
 
  void opcontrol_arcade_flipped(e_type stick_type);
 
  void opcontrol_curve_sd_initialize();
 
  void opcontrol_curve_default_set(double left, double right = 0);
 
  std::vector<double> opcontrol_curve_default_get();
 
  void opcontrol_drive_activebrake_set(double kp, double ki = 0.0, double kd = 0.0, double start_i = 0.0);
 
  double opcontrol_drive_activebrake_get();
 
  PID::Constants opcontrol_drive_activebrake_constants_get();
 
  void opcontrol_curve_buttons_toggle(bool toggle);
 
  bool opcontrol_curve_buttons_toggle_get();
 
  void opcontrol_curve_buttons_left_set(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase);
 
  std::vector<pros::controller_digital_e_t> opcontrol_curve_buttons_left_get();
 
  void opcontrol_curve_buttons_right_set(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase);
 
  std::vector<pros::controller_digital_e_t> opcontrol_curve_buttons_right_get();
 
  double opcontrol_curve_left(double x);
 
  double opcontrol_curve_right(double x);
 
  void opcontrol_joystick_threshold_set(int threshold);
 
  int opcontrol_joystick_threshold_get();
 
  void opcontrol_drive_sensors_reset();
 
  void opcontrol_joystick_threshold_iterate(int l_stick, int r_stick);
 
  //
  // PTO
  //
 
  bool pto_check(pros::Motor check_if_pto);
 
  void pto_add(std::vector<pros::Motor> pto_list);
 
  void pto_remove(std::vector<pros::Motor> pto_list);
 
  void pto_toggle(std::vector<pros::Motor> pto_list, bool toggle);
 
  //
  // PROS Wrappers
  //
 
  void drive_set(int left, int right);
 
  std::vector<int> drive_get();
 
  void drive_brake_set(pros::motor_brake_mode_e_t brake_type);
 
  pros::motor_brake_mode_e_t drive_brake_get();
 
  void drive_current_limit_set(int mA);
 
  int drive_current_limit_get();
 
  void pid_drive_toggle(bool toggle);
 
  bool pid_drive_toggle_get();
 
  void pid_print_toggle(bool toggle);
 
  bool pid_print_toggle_get();
 
  //
  // Telemetry
  //
 
  double drive_sensor_right();
 
  int drive_sensor_right_raw();
 
  int drive_velocity_right();
 
  double drive_mA_right();
 
  bool drive_current_right_over();
 
  double drive_sensor_left();
 
  int drive_sensor_left_raw();
 
  int drive_velocity_left();
 
  double drive_mA_left();
 
  bool drive_current_left_over();
 
  void drive_sensor_reset();
 
  void drive_imu_reset(double new_heading = 0);
 
  double drive_imu_get();
 
  double drive_imu_accel_get();
 
  void drive_imu_scaler_set(double scaler);
 
  double drive_imu_scaler_get();
 
  bool drive_imu_calibrate(bool run_loading_animation = true);
 
  bool drive_imu_calibrated();
 
  void drive_imu_display_loading(int iter);
 
  void opcontrol_joystick_practicemode_toggle(bool toggle);
 
  bool opcontrol_joystick_practicemode_toggle_get();
 
  void opcontrol_drive_reverse_set(bool toggle);
 
  bool opcontrol_drive_reverse_get();
 
  //
  // Autonomous Functions
  //
 
  void pid_odom_set(double target, int speed);
 
  void pid_odom_set(double target, int speed, bool slew_on);
 
  void pid_odom_set(okapi::QLength p_target, int speed);
 
  void pid_odom_set(okapi::QLength p_target, int speed, bool slew_on);
 
  void pid_odom_set(odom imovement);
 
  void pid_odom_set(odom imovement, bool slew_on);
 
  void pid_odom_ptp_set(odom imovement);
 
  void pid_odom_ptp_set(odom imovement, bool slew_on);
 
  void pid_odom_boomerang_set(odom imovement);
 
  void pid_odom_boomerang_set(odom imovement, bool slew_on);
 
  void pid_odom_boomerang_set(united_odom p_imovement);
 
  void pid_odom_boomerang_set(united_odom p_imovement, bool slew_on);
 
  void pid_odom_ptp_set(united_odom p_imovement);
 
  void pid_odom_ptp_set(united_odom p_imovement, bool slew_on);
 
  void pid_odom_set(united_odom p_imovement);
 
  void pid_odom_set(united_odom p_imovement, bool slew_on);
 
  void pid_odom_set(std::vector<odom> imovements);
 
  void pid_odom_set(std::vector<odom> imovements, bool slew_on);
 
  void pid_odom_pp_set(std::vector<odom> imovements);
 
  void pid_odom_pp_set(std::vector<odom> imovements, bool slew_on);
 
  void pid_odom_injected_pp_set(std::vector<odom> imovements);
 
  void pid_odom_injected_pp_set(std::vector<odom> imovements, bool slew_on);
 
  void pid_odom_smooth_pp_set(std::vector<odom> imovements);
 
  void pid_odom_smooth_pp_set(std::vector<odom> imovements, bool slew_on);
 
  void pid_odom_smooth_pp_set(std::vector<united_odom> p_imovements);
 
  void pid_odom_smooth_pp_set(std::vector<united_odom> p_imovements, bool slew_on);
 
  void pid_odom_injected_pp_set(std::vector<united_odom> p_imovements);
 
  void pid_odom_injected_pp_set(std::vector<united_odom> p_imovements, bool slew_on);
 
  void pid_odom_pp_set(std::vector<united_odom> p_imovements);
 
  void pid_odom_pp_set(std::vector<united_odom> p_imovements, bool slew_on);
 
  void pid_odom_set(std::vector<united_odom> p_imovements);
 
  void pid_odom_set(std::vector<united_odom> p_imovements, bool slew_on);
 
  void pid_drive_set(okapi::QLength p_target, int speed = DRIVE_SPEED);
 
  void pid_drive_set(okapi::QLength p_target, int speed, bool slew_on, bool toggle_heading = true);
 
  void pid_drive_set(double target, int speed = DRIVE_SPEED);
 
  void pid_drive_set(double target, int speed, bool slew_on, bool toggle_heading = true);
 
  void pid_turn_set(pose itarget, drive_directions dir, int speed = TURN_SPEED);
 
  void pid_turn_set(pose itarget, drive_directions dir, int speed, bool slew_on);
 
  void pid_turn_set(pose itarget, drive_directions dir, int speed, e_angle_behavior behavior);
 
  void pid_turn_set(pose itarget, drive_directions dir, int speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed = TURN_SPEED);
 
  void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, bool slew_on);
 
  void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, e_angle_behavior behavior);
 
  void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_turn_set(double target, int speed = TURN_SPEED);
 
  void pid_turn_set(double target, int speed, e_angle_behavior behavior);
 
  void pid_turn_set(double target, int speed, bool slew_on);
 
  void pid_turn_set(double target, int speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_turn_set(okapi::QAngle p_target, int speed = TURN_SPEED);
 
  void pid_turn_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior);
 
  void pid_turn_set(okapi::QAngle p_target, int speed, bool slew_on);
 
  void pid_turn_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_turn_relative_set(okapi::QAngle p_target, int speed);
 
  void pid_turn_relative_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior);
 
  void pid_turn_relative_set(okapi::QAngle p_target, int speed, bool slew_on);
 
  void pid_turn_relative_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_turn_relative_set(double target, int speed);
 
  void pid_turn_relative_set(double target, int speed, e_angle_behavior behavior);
 
  void pid_turn_relative_set(double target, int speed, bool slew_on);
 
  void pid_turn_relative_set(double target, int speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_swing_set(e_swing type, double target, int speed = SWING_SPEED);
 
  void pid_swing_set(e_swing type, double target, int speed, e_angle_behavior behavior);
 
  void pid_swing_set(e_swing type, double target, int speed, bool slew_on);
 
  void pid_swing_set(e_swing type, double target, int speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_swing_set(e_swing type, double target, int speed, int opposite_speed);
 
  void pid_swing_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior);
 
  void pid_swing_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on);
 
  void pid_swing_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed = SWING_SPEED);
 
  void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior);
 
  void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, bool slew_on);
 
  void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed);
 
  void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior);
 
  void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, bool slew_on);
 
  void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed);
 
  void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior);
 
  void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, bool slew_on);
 
  void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed);
 
  void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior);
 
  void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, bool slew_on);
 
  void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_swing_relative_set(e_swing type, double target, int speed);
 
  void pid_swing_relative_set(e_swing type, double target, int speed, e_angle_behavior behavior);
 
  void pid_swing_relative_set(e_swing type, double target, int speed, bool slew_on);
 
  void pid_swing_relative_set(e_swing type, double target, int speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed);
 
  void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior);
 
  void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on);
 
  void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on);
 
  void pid_targets_reset();
 
  void drive_angle_set(okapi::QAngle p_angle);
 
  void drive_angle_set(double angle);
 
  void pid_wait();
 
  void pid_wait_until(okapi::QAngle target);
 
  void pid_wait_until(okapi::QLength target);
 
  void pid_wait_until(double target);
 
  void pid_wait_quick();
 
  void pid_wait_quick_chain();
 
  void pid_wait_until_index(int index);
 
  void pid_wait_until_index_started(int index);
 
  void pid_wait_until_point(pose target);
 
  void pid_wait_until_point(united_pose target);
 
  void pid_wait_until(pose target);
 
  void pid_wait_until(united_pose target);
 
  bool interfered = false;
 
  void drive_ratio_set(double ratio);
 
  void drive_rpm_set(double rpm);
 
  double drive_ratio_get();
 
  double drive_rpm_get();
 
  void pid_speed_max_set(int speed);
 
  int pid_speed_max_get();
 
  void pid_turn_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);
 
  PID::Constants pid_turn_constants_get();
 
  void pid_turn_chain_constant_set(okapi::QAngle input);
 
  void pid_turn_chain_constant_set(double input);
 
  double pid_turn_chain_constant_get();
 
  void pid_swing_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);
 
  PID::Constants pid_swing_constants_get();
 
  void pid_swing_constants_forward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);
 
  PID::Constants pid_swing_constants_forward_get();
 
  void pid_swing_constants_backward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);
 
  PID::Constants pid_swing_constants_backward_get();
 
  void pid_swing_chain_constant_set(okapi::QAngle input);
 
  void pid_swing_chain_forward_constant_set(okapi::QAngle input);
 
  void pid_swing_chain_backward_constant_set(okapi::QAngle input);
 
  void pid_swing_chain_constant_set(double input);
 
  void pid_swing_chain_forward_constant_set(double input);
 
  double pid_swing_chain_forward_constant_get();
 
  void pid_swing_chain_backward_constant_set(double input);
 
  double pid_swing_chain_backward_constant_get();
 
  void pid_heading_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);
 
  PID::Constants pid_heading_constants_get();
 
  void pid_drive_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);
 
  PID::Constants pid_drive_constants_get();
 
  void pid_drive_constants_forward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);
 
  PID::Constants pid_drive_constants_forward_get();
 
  void pid_drive_constants_backward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);
 
  PID::Constants pid_drive_constants_backward_get();
 
  void pid_drive_chain_constant_set(okapi::QLength input);
 
  void pid_drive_chain_forward_constant_set(okapi::QLength input);
 
  void pid_drive_chain_backward_constant_set(okapi::QLength input);
 
  void pid_drive_chain_constant_set(double input);
 
  void pid_drive_chain_forward_constant_set(double input);
 
  double pid_drive_chain_forward_constant_get();
 
  void pid_drive_chain_backward_constant_set(double input);
 
  double pid_drive_chain_backward_constant_get();
 
  void pid_swing_min_set(int min);
 
  void pid_turn_min_set(int min);
 
  int pid_swing_min_get();
 
  int pid_turn_min_get();
 
  void pid_odom_angular_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);
 
  void pid_odom_boomerang_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);
 
  void 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 = true);
 
  void 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 = true);
 
  void 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 = true);
 
  void 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 = true);
 
  void 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 = true);
 
  void 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 = true);
 
  void 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 = true);
 
  void 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 = true);
 
  void 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 = true);
 
  void 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 = true);
 
  double drive_tick_per_inch();
 
  void opcontrol_curve_buttons_iterate();
 
  void pid_tuner_enable();
 
  void pid_tuner_disable();
 
  void pid_tuner_toggle();
 
  bool pid_tuner_enabled();
 
  void pid_tuner_refresh();
 
  void pid_tuner_column_set(int idx);
 
  void pid_tuner_iterate();
 
  void pid_tuner_print_brain_set(bool input);
 
  void pid_tuner_print_terminal_set(bool input);
 
  bool pid_tuner_print_terminal_enabled();
 
  bool pid_tuner_print_brain_enabled();
 
  void pid_tuner_increment_p_set(double p);
 
  void pid_tuner_increment_i_set(double i);
 
  void pid_tuner_increment_d_set(double d);
 
  void pid_tuner_increment_start_i_set(double start_i);
 
  double pid_tuner_increment_p_get();
 
  double pid_tuner_increment_i_get();
 
  double pid_tuner_increment_d_get();
 
  double pid_tuner_increment_start_i_get();
 
  void pid_tuner_full_enable(bool enable);
 
  bool pid_tuner_full_enabled();
 
  struct const_and_name {
    std::string name = "";
    PID::Constants* consts;
  };
 
  std::vector<const_and_name> pid_tuner_pids = {
      {"Drive PID Constants", &fwd_rev_drivePID.constants},
      {"Odom Angular PID Constants", &odom_angularPID.constants},
      {"Boomerang Angular PID Constants", &boomerangPID.constants},
      {"Heading PID Constants", &headingPID.constants},
      {"Turn PID Constants", &turnPID.constants},
      {"Swing PID Constants", &fwd_rev_swingPID.constants}};
 
  std::vector<const_and_name> pid_tuner_full_pids = {
      {"Drive Forward PID Constants", &forward_drivePID.constants},
      {"Drive Backward PID Constants", &backward_drivePID.constants},
      {"Odom Angular PID Constants", &odom_angularPID.constants},
      {"Boomerang Angular PID Constants", &boomerangPID.constants},
      {"Heading PID Constants", &headingPID.constants},
      {"Turn PID Constants", &turnPID.constants},
      {"Swing Forward PID Constants", &forward_swingPID.constants},
      {"Swing Backward PID Constants", &backward_swingPID.constants}};
 
  void opcontrol_speed_max_set(int speed);
 
  int opcontrol_speed_max_get();
 
  void opcontrol_arcade_scaling(bool enable);
 
  bool opcontrol_arcade_scaling_enabled();
 
 private:
  void opcontrol_drive_activebrake_targets_set();
  double odom_smooth_weight_smooth = 0.0;
  double odom_smooth_weight_data = 0.0;
  double odom_smooth_tolerance = 0.0;
  bool odom_use_left = true;
  double odom_ime_track_width_left = 0.0;
  double odom_ime_track_width_right = 0.0;
  bool imu_calibrate_took_too_long = false;
  bool is_full_pid_tuner_enabled = false;
  std::vector<const_and_name>* used_pid_tuner_pids;
  double opcontrol_speed_max = 127.0;
  bool arcade_vector_scaling = false;
  // odom privates
  std::vector<odom> pp_movements;
  std::vector<int> injected_pp_index;
  int pp_index = 0;
  std::vector<odom> smooth_path(std::vector<odom> ipath, double weight_smooth, double weight_data, double tolerance);
  double is_past_target(pose target, pose current);
  void raw_pid_odom_pp_set(std::vector<odom> imovements, bool slew_on);
  bool ptf1_running = false;
  std::vector<pose> find_point_to_face(pose current, pose target, drive_directions dir, bool set_global);
  void raw_pid_odom_ptp_set(odom imovement, bool slew_on);
  std::vector<odom> inject_points(std::vector<odom> imovements);
  std::vector<pose> point_to_face = {{0, 0, 0}, {0, 0, 0}};
  double turn_is_toleranced(double target, double current, double input, double longest, double shortest);
  double turn_short(double target, double current, bool print = false);
  double turn_long(double target, double current, bool print = false);
  double new_turn_target_compute(double target, double current, light::e_angle_behavior behavior);
  double turn_left(double target, double current, bool print = false);
  double turn_right(double target, double current, bool print = false);
  bool imu_calibration_complete = false;
  bool is_swing_slew_enabled(e_swing type, double target, double current);
  bool slew_reenables_when_max_speed_changes = true;
  int slew_min_when_it_enabled = 0;
  bool slew_will_enable_later = false;
  bool current_slew_on = false;
  bool is_odom_turn_bias_enabled = true;
  bool odom_turn_bias_enabled();
  void odom_turn_bias_enable(bool set);
  double angle_rad = 0.0;
  double global_track_width = 0.0;
  bool odometry_enabled = true;
  pose odom_target = {0.0, 0.0, 0.0};
  pose odom_current = {0.0, 0.0, 0.0};
  pose odom_second_to_last = {0.0, 0.0, 0.0};
  pose odom_start = {0.0, 0.0, 0.0};
  pose odom_target_start = {0.0, 0.0, 0.0};
  pose turn_to_point_target = {0.0, 0.0, 0.0};
  bool y_flipped = false;
  bool x_flipped = false;
  bool theta_flipped = false;
  double flip_angle_target(double target);
  double odom_imu_start = 0.0;
  int past_target = 0;
  double SPACING = 0.5;
  double LOOK_AHEAD = 7.0;
  double dlead = 0.5;
  double max_boomerang_distance = 12.0;
  double odom_turn_bias_amount = 1.375;
  drive_directions current_drive_direction = fwd;
  double h_last = 0.0, t_last = 0.0, l_last = 0.0, r_last = 0.0;
  pose l_pose{0.0, 0.0, 0.0};
  pose r_pose{0.0, 0.0, 0.0};
  pose central_pose{0.0, 0.0, 0.0};
  double xy_current_fake = 0.0;
  double xy_last_fake = 0.0;
  double xy_delta_fake = 0.0;
  double new_current_fake = 0.0;
  bool was_odom_just_set = false;
  std::pair<float, float> decide_vert_sensor(light::tracking_wheel* tracker, bool is_tracker_enabled, float ime = 0.0, float ime_track = 0.0);
  pose solve_xy_vert(float p_track_width, float current_t, float delta_vert, float delta_t);
  pose solve_xy_horiz(float p_track_width, float current_t, float delta_horiz, float delta_t);
  bool was_last_pp_mode_boomerang = false;
  bool global_forward_drive_slew_enabled = false;
  bool global_backward_drive_slew_enabled = false;
  bool global_forward_swing_slew_enabled = false;
  bool global_backward_swing_slew_enabled = false;
  double turn_tolerance = 3.0;
  bool global_turn_slew_enabled = false;
  e_angle_behavior current_angle_behavior = raw;
  e_angle_behavior default_swing_type = raw;
  e_angle_behavior default_turn_type = raw;
  e_angle_behavior default_odom_type = shortest;
  bool turn_biased_left = false;
  std::vector<odom> set_odoms_direction(std::vector<odom> inputs);
  odom set_odom_direction(odom input);
  pose flip_pose(pose input);
  bool odom_tracker_left_enabled = false;
  bool odom_tracker_right_enabled = false;
  bool odom_tracker_front_enabled = false;
  bool odom_tracker_back_enabled = false;
 
  double chain_target_start = 0.0;
  double chain_sensor_start = 0.0;
  double drive_forward_motion_chain_scale = 0.0;
  double drive_backward_motion_chain_scale = 0.0;
  double swing_forward_motion_chain_scale = 0.0;
  double swing_backward_motion_chain_scale = 0.0;
  double turn_motion_chain_scale = 0.0;
  double used_motion_chain_scale = 0.0;
  bool motion_chain_backward = false;
 
  double IMU_SCALER = 1.0;
 
  bool drive_toggle = true;
  bool print_toggle = true;
  int swing_min = 0;
  int turn_min = 0;
  bool practice_mode_is_on = false;
  int swing_opposite_speed = 0;
  bool slew_swing_fwd_using_angle = false;
  bool slew_swing_rev_using_angle = false;
  bool slew_swing_using_angle = false;
  bool pid_tuner_terminal_b = false;
  bool pid_tuner_lcd_b = true;
  void pid_tuner_print();
  void pid_tuner_value_modify(float p, float i, float d, float start);
  void pid_tuner_value_increase();
  void pid_tuner_value_decrease();
  void pid_tuner_print_brain();
  void pid_tuner_print_terminal();
  void pid_tuner_brain_init();
  int column = 0;
  int row = 0;
  std::string arrow = " <--\n";
  bool last_controller_curve_state = false;
  bool last_auton_selector_state = false;
  bool pid_tuner_on = false;
  std::string complete_pid_tuner_output = "";
  float p_increment = 0.1, i_increment = 0.001, d_increment = 0.25, start_i_increment = 1.0;
 
  void wait_until_drive(double target);
  void wait_until_turn_swing(double target);
 
  void private_drive_set(int left, int right);
 
  int clipped_joystick(int joystick);
 
  bool heading_on = true;
 
  double TICK_PER_REV;
  double TICK_PER_INCH;
  double CIRCUMFERENCE;
 
  double CARTRIDGE;
  double RATIO;
  double WHEEL_DIAMETER;
 
  int max_speed;
 
  void drive_pid_task();
  void swing_pid_task();
  void turn_pid_task();
  void ez_auto_task();
  void ptp_task();
  void boomerang_task();
  void pp_task();
 
  double l_start = 0;
  double r_start = 0;
 
  bool disable_controller = true;  // True enables, false disables.
 
  bool is_tank;
 
#define DRIVE_INTEGRATED 1
#define DRIVE_ADI_ENCODER 2
#define DRIVE_ROTATION 3
#define ODOM_TRACKER 4
 
  int is_tracker = DRIVE_INTEGRATED;
 
  void save_l_curve_sd();
  void save_r_curve_sd();
 
  struct button_ {
    bool lock = false;
    bool release_reset = false;
    int release_timer = 0;
    int hold_timer = 0;
    int increase_timer;
    pros::controller_digital_e_t button;
  };
 
  button_ l_increase_;
  button_ l_decrease_;
  button_ r_increase_;
  button_ r_decrease_;
 
  void button_press(button_* input_name, int button, std::function<void()> changeCurve, std::function<void()> save);
 
  double left_curve_scale;
  double right_curve_scale;
 
  void l_decrease();
  void l_increase();
  void r_decrease();
  void r_increase();
 
  bool is_reversed = false;
};
};  // namespace light