diff --git a/.gitignore b/.gitignore index ec1016b3..5dc6b214 100644 --- a/.gitignore +++ b/.gitignore @@ -43,4 +43,4 @@ EZ-Template-Example-Project/temp.log EZ-Template-Example-Project/temp.errors EZ-Template-Example-Project/*.ini EZ-Template-Example-Project/.d/ -.d/ +.d/ \ No newline at end of file diff --git a/EZ-Template-Example-Project-3.1.0.zip b/EZ-Template-Example-Project.zip similarity index 73% rename from EZ-Template-Example-Project-3.1.0.zip rename to EZ-Template-Example-Project.zip index 1045f9f5..85a15aa0 100644 Binary files a/EZ-Template-Example-Project-3.1.0.zip and b/EZ-Template-Example-Project.zip differ diff --git a/EZ-Template-Example-Project/EZ-Template@3.1.0.zip b/EZ-Template-Example-Project/EZ-Template@3.1.0.zip deleted file mode 100644 index 8185d1fb..00000000 Binary files a/EZ-Template-Example-Project/EZ-Template@3.1.0.zip and /dev/null differ diff --git a/EZ-Template-Example-Project/EZ-Template@3.2.0.zip b/EZ-Template-Example-Project/EZ-Template@3.2.0.zip new file mode 100644 index 00000000..adaecd8f Binary files /dev/null and b/EZ-Template-Example-Project/EZ-Template@3.2.0.zip differ diff --git a/EZ-Template-Example-Project/firmware/EZ-Template.a b/EZ-Template-Example-Project/firmware/EZ-Template.a index 40aa3d77..55af85c0 100644 Binary files a/EZ-Template-Example-Project/firmware/EZ-Template.a and b/EZ-Template-Example-Project/firmware/EZ-Template.a differ diff --git a/EZ-Template-Example-Project/firmware/libpros.a b/EZ-Template-Example-Project/firmware/libpros.a index 43f4e941..36de8fae 100644 Binary files a/EZ-Template-Example-Project/firmware/libpros.a and b/EZ-Template-Example-Project/firmware/libpros.a differ diff --git a/EZ-Template-Example-Project/include/EZ-Template/PID.hpp b/EZ-Template-Example-Project/include/EZ-Template/PID.hpp index 29605362..66d78950 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/PID.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/PID.hpp @@ -73,23 +73,23 @@ class PID { * Set's constants for exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. + * sets small_exit_time, timer for to exit within smalL_error * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. + * sets smalL_error, timer will start when error is within this * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. + * sets big_exit_time, timer for to exit within big_error * \param p_big_error - * Sets big_error. Timer will start when error is within this. + * sets big_error, timer will start when error is within this * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. + * sets velocity_exit_time, timer will start when velocity is 0 */ void exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time = 0, double p_big_error = 0, int p_velocity_exit_time = 0, int p_mA_timeout = 0); /** - * Sets target. + * Sets PID target. * * \param target - * Target for PID. + * new target for PID */ void target_set(double input); @@ -97,18 +97,19 @@ class PID { * Computes PID. * * \param current - * Current sensor value. + * current sensor value */ double compute(double current); /** - * Computes PID, but you set the error yourself. This function ignores target. - * Current is only used here for calculative derivative. + * Computes PID, but you compute the error yourself. + * + * Current is only used here for calculative derivative to solve derivative kick. * * \param err - * Error in PID, you need to calculate this yourself. + * error for the PID, you need to calculate this yourself * \param current - * Current sensor value. + * current sensor value */ double compute_error(double err, double current); @@ -122,18 +123,23 @@ class PID { */ Constants constants_get(); + /** + * Returns true if PID constants are set, returns false if they're all 0. + */ + bool constants_set_check(); + /** * Resets all variables to 0. This does not reset constants. */ void variables_reset(); /** - * Constants + * Constants. */ Constants constants; /** - * Exit + * Exit. */ exit_condition_ exit; @@ -141,7 +147,7 @@ class PID { * Updates a secondary sensor for velocity exiting. Ideal use is IMU during normal drive motions. * * \param secondary_sensor - * double for a secondary sensor. + * secondary sensor value */ void velocity_sensor_secondary_set(double secondary_sensor); @@ -154,7 +160,7 @@ class PID { * Boolean for if the secondary sensor will be updated or not. True uses this sensor, false does not. * * \param toggle - * True uses this sensor, false does not. + * true uses this sensor, false does not */ void velocity_sensor_secondary_toggle_set(bool toggle); @@ -164,7 +170,7 @@ class PID { bool velocity_sensor_secondary_toggle_get(); /** - * Sets the threshold that the main sensor will return 0 velocity within + * Sets the threshold that the main sensor will return 0 velocity within. * * \param zero * a small double @@ -172,12 +178,12 @@ class PID { void velocity_sensor_main_exit_set(double zero); /** - * Returns the threshold that the main sensor will return 0 velocity within + * Returns the threshold that the main sensor will return 0 velocity within. */ double velocity_sensor_main_exit_get(); /** - * Sets the threshold that the secondary sensor will return 0 velocity within + * Sets the threshold that the secondary sensor will return 0 velocity within. * * \param zero * a small double @@ -185,7 +191,7 @@ class PID { void velocity_sensor_secondary_exit_set(double zero); /** - * Returns the threshold that the secondary sensor will return 0 velocity within + * Returns the threshold that the secondary sensor will return 0 velocity within. */ double velocity_sensor_secondary_exit_get(); @@ -193,7 +199,7 @@ class PID { * Iterative exit condition for PID. * * \param print = false - * if true, prints when complete. + * if true, prints when complete */ ez::exit_output exit_condition(bool print = false); @@ -201,9 +207,9 @@ class PID { * Iterative exit condition for PID. * * \param sensor - * A pros motor on your mechanism. + * a pros motor on your mechanism * \param print = false - * if true, prints when complete. + * if true, prints when complete */ ez::exit_output exit_condition(pros::Motor sensor, bool print = false); @@ -211,17 +217,27 @@ class PID { * Iterative exit condition for PID. * * \param sensor - * Pros motors on your mechanism. + * pros motors on your mechanism * \param print = false - * if true, prints when complete. + * if true, prints when complete */ ez::exit_output exit_condition(std::vector sensor, bool print = false); + /** + * Iterative exit condition for PID. + * + * \param sensor + * pros motor group on your mechanism + * \param print = false + * if true, prints when complete + */ + ez::exit_output exit_condition(pros::MotorGroup sensor, bool print = false); + /** * Sets the name of the PID that prints during exit conditions. * * \param name - * a string that is the name you want to print + * the name of the mechanism for printing */ void name_set(std::string name); @@ -231,7 +247,9 @@ class PID { std::string name_get(); /** - * Enables / disables i resetting when sgn of error changes. True resets, false doesn't. + * Enables / disables i resetting when sgn of error changes. + * + * True resets, false doesn't. * * \param toggle * true resets, false doesn't @@ -239,7 +257,9 @@ class PID { void i_reset_toggle(bool toggle); /** - * Returns if i will reset when sgn of error changes. True resets, false doesn't. + * Returns if i will reset when sgn of error changes. + * + * True resets, false doesn't. */ bool i_reset_get(); @@ -264,7 +284,7 @@ class PID { private: double velocity_zero_main = 0.05; - double velocity_zero_secondary = 0.1; + double velocity_zero_secondary = 0.075; int i = 0, j = 0, k = 0, l = 0, m = 0; bool is_mA = false; double second_sensor = 0.0; diff --git a/EZ-Template-Example-Project/include/EZ-Template/api.hpp b/EZ-Template-Example-Project/include/EZ-Template/api.hpp index 63bc6aea..7989cf01 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/api.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/api.hpp @@ -13,4 +13,5 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include "EZ-Template/piston.hpp" #include "EZ-Template/sdcard.hpp" #include "EZ-Template/slew.hpp" +#include "EZ-Template/tracking_wheel.hpp" #include "EZ-Template/util.hpp" \ No newline at end of file diff --git a/EZ-Template-Example-Project/include/EZ-Template/auton_selector.hpp b/EZ-Template-Example-Project/include/EZ-Template/auton_selector.hpp index 10d0c19a..e8f46feb 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/auton_selector.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/auton_selector.hpp @@ -17,6 +17,7 @@ class AutonSelector { std::vector Autons; int auton_page_current; int auton_count; + int last_auton_page_current; AutonSelector(); AutonSelector(std::vector autons); void selected_auton_call(); diff --git a/EZ-Template-Example-Project/include/EZ-Template/drive/drive.hpp b/EZ-Template-Example-Project/include/EZ-Template/drive/drive.hpp index 582792d2..6a4004d0 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/drive/drive.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/drive/drive.hpp @@ -12,10 +12,12 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include "EZ-Template/PID.hpp" #include "EZ-Template/slew.hpp" +#include "EZ-Template/tracking_wheel.hpp" #include "EZ-Template/util.hpp" #include "okapi/api/units/QAngle.hpp" #include "okapi/api/units/QLength.hpp" #include "okapi/api/units/QTime.hpp" +#include "pros/motor_group.hpp" #include "pros/motors.h" using namespace ez; @@ -24,7 +26,9 @@ namespace ez { class Drive { public: /** - * Joysticks will return 0 when they are within this number. Set with opcontrol_joystick_threshold_set() + * Joysticks will return 0 when they are within this number. + * + * Set with opcontrol_joystick_threshold_set() */ int JOYSTICK_THRESHOLD; @@ -64,37 +68,67 @@ class Drive { pros::Imu imu; /** - * Left tracking wheel. + * Deprecated left tracking wheel. */ pros::adi::Encoder left_tracker; /** - * Right tracking wheel. + * Deprecated right tracking wheel. */ pros::adi::Encoder right_tracker; /** - * Left rotation tracker. + * Deprecated left rotation tracker. */ pros::Rotation left_rotation; /** - * Right rotation tracker. + * Deprecated right rotation tracker. */ pros::Rotation right_rotation; + /** + * Left vertical tracking wheel. + */ + tracking_wheel* odom_tracker_left; + + /** + * Right vertical tracking wheel. + */ + tracking_wheel* odom_tracker_right; + + /** + * Front horizontal tracking wheel. + */ + tracking_wheel* odom_tracker_front; + + /** + * Back horizontal tracking wheel. + */ + tracking_wheel* odom_tracker_back; + /** * PID objects. */ PID headingPID; PID turnPID; - PID forward_drivePID; 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; /** * Slew objects. @@ -109,105 +143,219 @@ class Drive { ez::slew slew_swing; /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for swing movements. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi distance unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_swing_constants_set(okapi::QLength distance, int min_speed); /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for forward swing movements. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi distance unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_swing_constants_forward_set(okapi::QLength distance, int min_speed); /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for backward swing movements. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi distance unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_swing_constants_backward_set(okapi::QLength distance, int min_speed); /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for swing movements. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi angle unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_swing_constants_set(okapi::QAngle distance, int min_speed); /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for swing forward movements. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi angle unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_swing_constants_forward_set(okapi::QAngle distance, int min_speed); /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for swing backward movements. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi angle unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_swing_constants_backward_set(okapi::QAngle distance, int min_speed); /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for turns. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi angle unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_turn_constants_set(okapi::QAngle distance, int min_speed); /** - * Sets constants for slew for driving forward. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for driving forward. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi distance unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_drive_constants_forward_set(okapi::QLength distance, int min_speed); /** - * Sets constants for slew for driving backward. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for driving backward. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi distance unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_drive_constants_backward_set(okapi::QLength distance, int min_speed); /** - * Sets constants for slew for driving. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for driving. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi distance unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_drive_constants_set(okapi::QLength distance, int min_speed); + /** + * Sets the default slew for drive forwards and backwards motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_drive_set(bool slew_on); + + /** + * Sets the default slew for drive forward motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_drive_forward_set(bool slew_on); + + /** + * Returns true if slew is enabled for all drive forward movements, false otherwise. + */ + bool slew_drive_forward_get(); + + /** + * Sets the default slew for drive backward motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_drive_backward_set(bool slew_on); + + /** + * Returns true if slew is enabled for all drive backward movements, false otherwise. + */ + bool slew_drive_backward_get(); + + /** + * Sets the default slew for swing forward and backward motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_swing_set(bool slew_on); + + /** + * Sets the default slew for swing forward motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_swing_forward_set(bool slew_on); + + /** + * Returns true if slew is enabled for all swing forward motions, false otherwise. + */ + bool slew_swing_forward_get(); + + /** + * Sets the default slew for swing backward motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_swing_backward_set(bool slew_on); + + /** + * Returns true if slew is enabled for all swing backward motions, false otherwise. + */ + bool slew_swing_backward_get(); + + /** + * Sets the default slew for turn motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_turn_set(bool slew_on); + + /** + * Returns true if slew is enabled for all turn motions, false otherwise. + */ + bool slew_turn_get(); + + /** + * Allows slew to reenable when the new input speed is larger than the current speed during pure pursuits. + * + * \param slew_on + * true enables, false disables + */ + void slew_odom_reenable(bool reenable); + + /** + * Returns if slew will reenable when the new input speed is larger than the current speed during pure pursuits. + */ + bool slew_odom_reenabled(); + /** * Current mode of the drive. */ @@ -215,8 +363,13 @@ class Drive { /** * Sets current mode of drive. + * + * \param p_mode + * the new drive mode + * \param stop_drive + * if the drive will stop when p_mode is DISABLED */ - void drive_mode_set(e_mode p_mode); + void drive_mode_set(e_mode p_mode, bool stop_drive = true); /** * Returns current mode of drive. @@ -237,17 +390,17 @@ class Drive { * Creates a Drive Controller using internal encoders. * * \param left_motor_ports - * Input {1, -2...}. Make ports negative if reversed! + * input {1, -2...}. make ports negative if reversed * \param right_motor_ports - * Input {-3, 4...}. Make ports negative if reversed! + * input {-3, 4...}. make ports negative if reversed * \param imu_port - * Port the IMU is plugged into. + * port the IMU is plugged into * \param wheel_diameter - * Diameter of your drive wheels. Remember 4" is 4.125"! + * diameter of your drive wheels * \param ticks - * Motor cartridge RPM + * motor cartridge RPM * \param ratio - * External gear ratio, wheel gear / motor gear. + * external gear ratio, wheel gear / motor gear */ Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio = 1.0); @@ -255,67 +408,67 @@ class Drive { * Creates a Drive Controller using encoders plugged into the brain. * * \param left_motor_ports - * Input {1, -2...}. Make ports negative if reversed! + * input {1, -2...}. make ports negative if reversed * \param right_motor_ports - * Input {-3, 4...}. Make ports negative if reversed! + * input {-3, 4...}. make ports negative if reversed * \param imu_port - * Port the IMU is plugged into. + * port the IMU is plugged into * \param wheel_diameter - * Diameter of your sensored wheels. Remember 4" is 4.125"! + * diameter of your sensored wheel * \param ticks - * Ticks per revolution of your encoder. + * ticks per revolution of your encoder * \param ratio - * External gear ratio, wheel gear / sensor gear. + * external gear ratio, wheel gear / sensor gear * \param left_tracker_ports - * Input {1, 2}. Make ports negative if reversed! + * input {1, 2}. make ports negative if reversed * \param right_tracker_ports - * Input {3, 4}. Make ports negative if reversed! + * input {3, 4}. make ports negative if reversed */ - Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector left_tracker_ports, std::vector right_tracker_ports); + Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector left_tracker_ports, std::vector right_tracker_ports) __attribute__((deprecated("Use the integrated encoder constructor with odom_tracker_left_set() and odom_tracker_right_set() instead!"))); /** * Creates a Drive Controller using encoders plugged into a 3 wire expander. * * \param left_motor_ports - * Input {1, -2...}. Make ports negative if reversed! + * input {1, -2...}. make ports negative if reversed * \param right_motor_ports - * Input {-3, 4...}. Make ports negative if reversed! + * input {-3, 4...}. make ports negative if reversed * \param imu_port - * Port the IMU is plugged into. + * port the IMU is plugged into * \param wheel_diameter - * Diameter of your sensored wheels. Remember 4" is 4.125"! + * diameter of your sensored wheel * \param ticks - * Ticks per revolution of your encoder. + * ticks per revolution of your encoder * \param ratio - * External gear ratio, wheel gear / sensor gear. + * external gear ratio, wheel gear / sensor gear * \param left_tracker_ports - * Input {1, 2}. Make ports negative if reversed! + * input {1, 2}. make ports negative if reversed * \param right_tracker_ports - * Input {3, 4}. Make ports negative if reversed! + * input {3, 4}. make ports negative if reversed * \param expander_smart_port - * Port the expander is plugged into. + * port the expander is plugged into */ - Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector left_tracker_ports, std::vector right_tracker_ports, int expander_smart_port); + Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector left_tracker_ports, std::vector 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!"))); /** * Creates a Drive Controller using rotation sensors. * * \param left_motor_ports - * Input {1, -2...}. Make ports negative if reversed! + * input {1, -2...}. make ports negative if reversed * \param right_motor_ports - * Input {-3, 4...}. Make ports negative if reversed! + * input {-3, 4...}. make ports negative if reversed * \param imu_port - * Port the IMU is plugged into. + * port the IMU is plugged into * \param wheel_diameter - * Diameter of your sensored wheels. Remember 4" is 4.125"! + * diameter of your sensored wheel * \param ratio - * External gear ratio, wheel gear / sensor gear. + * external gear ratio, wheel gear / sensor gear * \param left_tracker_port - * Make ports negative if reversed! + * make ports negative if reversed * \param right_tracker_port - * Make ports negative if reversed! + * make ports negative if reversed */ - Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ratio, int left_rotation_port, int right_rotation_port); + Drive(std::vector left_motor_ports, std::vector 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!"))); /** * Sets drive defaults. @@ -324,474 +477,1943 @@ class Drive { ///// // - // User Control + // General Odometry // ///// /** - * Sets the chassis to controller joysticks using tank control. Run is usercontrol. - * This passes the controller through the curve functions, but is disabled by default. Use opcontrol_curve_buttons_toggle() to enable it. + * Tasks for tracking. */ - void opcontrol_tank(); + void ez_tracking_task(); /** - * Sets the chassis to controller joysticks using standard arcade control. Run is usercontrol. - * This passes the controller through the curve functions, but is disabled by default. Use opcontrol_curve_buttons_toggle() to enable it. + * Enables / disables tracking. * - * \param stick_type - * ez::SINGLE or ez::SPLIT control + * \param input + * true enables tracking, false disables tracking */ - void opcontrol_arcade_standard(e_type stick_type); + void odom_enable(bool input); /** - * Sets the chassis to controller joysticks using flipped arcade control. Run is usercontrol. - * This passes the controller through the curve functions, but is disabled by default. Use opcontrol_curve_buttons_toggle() to enable it. + * Returns whether the bot is tracking with odometry. * - * \param stick_type - * ez::SINGLE or ez::SPLIT control + * True means tracking is enabled, false means tracking is disabled. */ - void opcontrol_arcade_flipped(e_type stick_type); + bool odom_enabled(); /** - * Initializes left and right curves with the SD card, recommended to run in initialize(). + * Sets the width of the drive. + * + * This is used for tracking. + * + * \param input + * a unit in inches, from center of the wheel to center of the wheel */ - void opcontrol_curve_sd_initialize(); + void drive_width_set(double input); /** - * Sets the default joystick curves. + * Sets the width of the drive. * - * \param left - * Left default curve. - * \param right - * Right default curve. + * This is used for tracking. + * + * \param input + * an okapi unit, from center of the wheel to center of the wheel */ - void opcontrol_curve_default_set(double left, double right = 0); + void drive_width_set(okapi::QLength p_input); /** - * Gets the default joystick curves, in {left, right} + * Returns the width of the drive */ - std::vector opcontrol_curve_default_get(); + double drive_width_get(); /** - * Runs a P loop on the drive when the joysticks are released. + * Sets the current X coordinate of the robot. * - * \param kp - * Constant for the p loop. + * \param x + * new x coordinate in inches */ - void opcontrol_drive_activebrake_set(double kp); + void odom_x_set(double x); /** - * Returns kP for active brake. + * Sets the current X coordinate of the robot. + * + * \param p_x + * new x coordinate as an okapi unit */ - double opcontrol_drive_activebrake_get(); + void odom_x_set(okapi::QLength p_x); /** - * Enables/disables modifying the joystick input curves with the controller. True enables, false disables. - * - * \param input - * bool input + * Returns the current X coordinate of the robot in inches. */ - void opcontrol_curve_buttons_toggle(bool toggle); + double odom_x_get(); /** - * Gets the current state of the toggle. Enables/disables modifying the joystick input curves with the controller. True enables, false disables. + * Sets the current Y coordinate of the robot. + * + * \param y + * new y coordinate in inches */ - bool opcontrol_curve_buttons_toggle_get(); + void odom_y_set(double y); /** - * Sets buttons for modifying the left joystick curve. + * Sets the current Y coordinate of the robot. * - * \param decrease - * a pros button enumerator - * \param increase - * a pros button enumerator + * \param p_y + * new y coordinate as an okapi unit */ - void opcontrol_curve_buttons_left_set(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase); + void odom_y_set(okapi::QLength p_y); /** - * Returns a vector of pros controller buttons user for the left joystick curve, in {decrease, increase} + * Returns the current Y coordinate of the robot in inches. */ - std::vector opcontrol_curve_buttons_left_get(); + double odom_y_get(); /** - * Sets buttons for modifying the right joystick curve. + * Sets the current angle of the robot. * - * \param decrease - * a pros button enumerator - * \param increase - * a pros button enumerator + * \param a + * new angle in degrees */ - void opcontrol_curve_buttons_right_set(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase); + void odom_theta_set(double a); /** - * Returns a vector of pros controller buttons user for the right joystick curve, in {decrease, increase} + * Sets the current Theta of the robot. + * + * \param p_a + * new angle as an okapi unit */ - std::vector opcontrol_curve_buttons_right_get(); + void odom_theta_set(okapi::QAngle p_a); + + /** + * Returns the current Theta of the robot in degrees. + */ + double odom_theta_get(); /** - * Outputs a curve from 5225A In the Zone. This gives more control over the robot at lower speeds. https://www.desmos.com/calculator/rcfjjg83zx + * Sets the current pose of the robot. * - * \param x - * joystick input + * \param itarget + * {x, y, t} units in inches and degrees */ - double opcontrol_curve_left(double x); + void odom_pose_set(pose itarget); /** - * Outputs a curve from 5225A In the Zone. This gives more control over the robot at lower speeds. https://www.desmos.com/calculator/rcfjjg83zx + * Sets the current pose of the robot. * - * \param x - * joystick input + * \param itarget + * {x, y, t} as an okapi unit */ - double opcontrol_curve_right(double x); + void odom_pose_set(united_pose itarget); /** - * Sets a new threshold for the joystick. The joysticks wil not return a value if they are within this. + * Sets the current X and Y coordinate for the robot. * - * \param threshold - * new threshold + * \param x + * new x value, in inches + * \param y + * new y value, in inches */ - void opcontrol_joystick_threshold_set(int threshold); + void odom_xy_set(double x, double y); /** - * Gets a new threshold for the joystick. The joysticks wil not return a value if they are within this. + * Sets the current X and Y coordinate for the robot. + * + * \param p_x + * new x value, okapi unit + * \param p_y + * new y value, okapi unit */ - int opcontrol_joystick_threshold_get(); + void odom_xy_set(okapi::QLength p_x, okapi::QLength p_y); /** - * Resets drive sensors at the start of opcontrol. + * Sets the current X, Y, and Theta values for the robot. + * + * \param x + * new x value, in inches + * \param y + * new y value, in inches + * \param t + * new theta value, in degrees */ - void opcontrol_drive_sensors_reset(); + void odom_xyt_set(double x, double y, double t); /** - * Sets minimum value distance constants. + * Sets the current X, Y, and Theta values for the robot. * - * \param l_stick - * input for left joystick - * \param r_stick - * input for right joystick + * \param p_x + * new x value, okapi unit + * \param p_y + * new y value, okapi unit + * \param p_t + * new theta value, okapi unit */ - void opcontrol_joystick_threshold_iterate(int l_stick, int r_stick); + void odom_xyt_set(okapi::QLength p_x, okapi::QLength p_y, okapi::QAngle p_t); - ///// - // - // PTO - // - ///// + /** + * Returns the current pose of the robot. + */ + pose odom_pose_get(); /** - * Checks if the motor is currently in pto_list. Returns true if it's already in pto_list. - * - * \param check_if_pto - * motor to check. + * Resets xyt to 0. */ - bool pto_check(pros::Motor check_if_pto); + void odom_reset(); /** - * Adds motors to the pto list, removing them from the drive. + * Flips the X axis. * - * \param pto_list - * list of motors to remove from the drive. + * \param flip + * true means left is positive x, false means right is positive x */ - void pto_add(std::vector pto_list); + void odom_x_flip(bool flip = true); /** - * Removes motors from the pto list, adding them to the drive. You cannot use the first index in a pto. + * Checks if X axis is flipped. * - * \param pto_list - * list of motors to add to the drive. + * True means left is positive X, false means right is positive X. */ - void pto_remove(std::vector pto_list); + bool odom_x_direction_get(); /** - * Adds/removes motors from drive. You cannot use the first index in a pto. + * Flips the Y axis. * - * \param pto_list - * list of motors to add/remove from the drive. - * \param toggle - * if true, adds to list. if false, removes from list. + * \param flip + * true means down is positive y, false means up is positive y */ - void pto_toggle(std::vector pto_list, bool toggle); + void odom_y_flip(bool flip = true); - ///// - // - // PROS Wrapers - // - ///// + /** + * Checks if Y axis is flipped. + * + * True means down is positive Y, false means up is positive Y. + */ + bool odom_y_direction_get(); /** - * Sets the chassis to voltage. Disables PID when called. + * Flips the rotation axis. * - * \param left - * voltage for left side, -127 to 127 - * \param right - * voltage for right side, -127 to 127 + * \param flip + * true means counterclockwise is positive, false means clockwise is positive */ - void drive_set(int left, int right); + void odom_theta_flip(bool flip = true); /** - * Gets the chassis to voltage, -127 to 127. Returns {left, right} + * Checks if the rotation axis is flipped. + * + * True means counterclockwise is positive, false means clockwise is positive. */ - std::vector drive_get(); + bool odom_theta_direction_get(); /** - * Changes the way the drive behaves when it is not under active user control + * Sets a new dlead. * - * \param brake_type - * the 'brake mode' of the motor e.g. 'pros::E_MOTOR_BRAKE_COAST' 'pros::E_MOTOR_BRAKE_BRAKE' 'pros::E_MOTOR_BRAKE_HOLD' + * Dlead is a proportional value of how much to make the robot curve during boomerang motions. + * + * \param input + * a value between 0 and 1 */ - void drive_brake_set(pros::motor_brake_mode_e_t brake_type); + void odom_boomerang_dlead_set(double input); /** - * Returns the brake mode of the drive in pros_brake_mode_e_t_ + * Returns the current dlead. */ - pros::motor_brake_mode_e_t drive_brake_get(); + double odom_boomerang_dlead_get(); /** - * Sets the limit for the current on the drive. + * Sets how far away the carrot point can be from the target point. * - * \param mA - * input in milliamps + * \param distance + * distance in inches */ - void drive_current_limit_set(int mA); + void odom_boomerang_distance_set(double distance); /** - * Gets the limit for the current on the drive. + * Sets how far away the carrot point can be from the target point. + * + * \param distance + * distance as an okapi unit */ - int drive_current_limit_get(); + void odom_boomerang_distance_set(okapi::QLength p_distance); /** - * Toggles set drive in autonomous. True enables, false disables. + * Returns how far away the carrot point can be from target. */ - void pid_drive_toggle(bool toggle); + double odom_boomerang_distance_get(); /** - * Gets the current state of the toggle. This toggles set drive in autonomous. True enables, false disables. + * A proportion of how prioritized turning is during odometry motions. + * + * Turning is prioritized so the robot "applies brakes" while turning. Lower number means more braking. + * + * \param bias + * a number between 0 and 1 */ - bool pid_drive_toggle_get(); + void odom_turn_bias_set(double bias); /** - * Toggles printing in autonomous. True enables, false disables. + * Returns the proportion of how prioritized turning is during odometry motions. */ - void pid_print_toggle(bool toggle); + double odom_turn_bias_get(); /** - * Gets the current state of the toggle. This toggles printing in autonomous. True enables, false disables. + * Sets the spacing between points when points get injected into the path. + * + * \param spacing + * a small number in inches */ - bool pid_print_toggle_get(); + void odom_path_spacing_set(double spacing); - ///// - // - // Telemetry - // - ///// + /** + * Sets the spacing between points when points get injected into the path. + * + * \param spacing + * a small number in okapi units + */ + void odom_path_spacing_set(okapi::QLength p_spacing); /** - * The position of the right motor. + * Returns the spacing between points when points get injected into the path. */ - double drive_sensor_right(); + double odom_path_spacing_get(); /** - * The position of the right motor. + * Sets the constants for smoothing out a path. + * + * Path smoothing based on https://medium.com/@jaems33/understanding-robot-motion-path-smoothing-5970c8363bc4 + * + * \param weight_smooth + * how much weight to update the data + * \param weight_data + * how much weight to smooth the coordinates + * \param tolerance + * how much change per iteration is necessary to keep iterating */ - int drive_sensor_right_raw(); + void odom_path_smooth_constants_set(double weight_smooth, double weight_data, double tolerance); /** - * The velocity of the right motor. + * Returns the constants for smoothing out a path. + * + * In order of: + * - weight_smooth + * - weight_data + * - tolerance */ - int drive_velocity_right(); + std::vector odom_path_smooth_constants_get(); /** - * The watts of the right motor. + * Prints the current path the robot is following. */ - double drive_mA_right(); + void odom_path_print(); /** - * Return TRUE when the motor is over current. + * Sets how far away the robot looks in the path during pure pursuits. + * + * \param distance + * how long the "carrot on a stick" is, in inches */ - bool drive_current_right_over(); + void odom_look_ahead_set(double distance); /** - * The position of the left motor. + * Sets how far away the robot looks in the path during pure pursuits. + * + * \param distance + * how long the "carrot on a stick" is, in okapi units */ - double drive_sensor_left(); + void odom_look_ahead_set(okapi::QLength p_distance); /** - * The position of the left motor. + * Returns how far away the robot looks in the path during pure pursuits. */ - int drive_sensor_left_raw(); + double odom_look_ahead_get(); /** - * The velocity of the left motor. + * Sets the parallel left tracking wheel for odometry. + * + * \param input + * an ez::tracking_wheel */ - int drive_velocity_left(); + void odom_tracker_left_set(tracking_wheel* input); /** - * The watts of the left motor. + * Sets the parallel right tracking wheel for odometry. + * + * \param input + * an ez::tracking_wheel */ - double drive_mA_left(); + void odom_tracker_right_set(tracking_wheel* input); /** - * Return TRUE when the motor is over current. + * Sets the perpendicular front tracking wheel for odometry. + * + * \param input + * an ez::tracking_wheel */ - bool drive_current_left_over(); + void odom_tracker_front_set(tracking_wheel* input); /** - * Reset all the chassis motors, recommended to run at the start of your autonomous routine. + * Sets the perpendicular back tracking wheel for odometry. + * + * \param input + * an ez::tracking_wheel */ - void drive_sensor_reset(); + void odom_tracker_back_set(tracking_wheel* input); /** - * Resets the current imu value. Defaults to 0, recommended to run at the start of your autonomous routine. + * Sets the default behavior for turns in odom, swinging, and turning. * - * \param new_heading - * New heading value. + * \param behavior + * ez::shortest, ez::longest, ez::left, ez::right, ez::raw */ - void drive_imu_reset(double new_heading = 0); + void pid_angle_behavior_set(e_angle_behavior behavior); /** - * Returns the current imu rotation value. + * Sets the default behavior for turns in turning movements. + * + * \param behavior + * ez::shortest, ez::longest, ez::left, ez::right, ez::raw */ - double drive_imu_get(); + void pid_turn_behavior_set(e_angle_behavior behavior); + + /** + * Sets the default behavior for turns in swinging movements. + * + * \param behavior + * ez::shortest, ez::longest, ez::left, ez::right, ez::raw + */ + void pid_swing_behavior_set(e_angle_behavior behavior); + + /** + * Sets the default behavior for turns in odom turning movements. + * + * \param behavior + * ez::shortest, ez::longest, ez::left, ez::right, ez::raw + */ + void pid_odom_behavior_set(e_angle_behavior behavior); + + /** + * Returns the turn behavior for turns. + */ + e_angle_behavior pid_turn_behavior_get(); + + /** + * Returns the turn behavior for swings. + */ + e_angle_behavior pid_swing_behavior_get(); + + /** + * Returns the turn behavior for odom turns. + */ + e_angle_behavior pid_odom_behavior_get(); + + /** + * Gives some wiggle room in shortest vs longest, so a 180.1 and 179.9 degree turns have consistent behavior. + * + * \param p_tolerance + * angle wiggle room, an okapi unit + */ + void pid_angle_behavior_tolerance_set(okapi::QAngle p_tolerance); + + /** + * Gives some wiggle room in shortest vs longest, so a 180.1 and 179.9 degree turns have consistent behavior. + * + * \param p_tolerance + * angle wiggle room, in degrees + */ + void pid_angle_behavior_tolerance_set(double tolerance); + + /** + * Returns the wiggle room in shortest vs longest, so a 180.1 and 179.9 degree turns have consistent behavior. + */ + double pid_angle_behavior_tolerance_get(); + + /** + * When a turn is within its tolerance, you can have it bias left or right. + * + * \param behavior + * ez::left or ez::right + */ + void pid_angle_behavior_bias_set(e_angle_behavior behavior); + + /** + * Returns the behavior when a turn is within its tolerance, you can have it bias left or right. + */ + e_angle_behavior pid_angle_behavior_bias_get(); + + ///// + // + // User Control + // + ///// + + /** + * Sets the chassis to controller joysticks using tank control. + * Run in usercontrol. + * + * This passes the controller through the curve functions, but is disabled by default. + * Use opcontrol_curve_buttons_toggle() to enable it. + */ + void opcontrol_tank(); + + /** + * Sets the chassis to controller joysticks using standard arcade control, where left stick is fwd/rev. + * Run in usercontrol. + * + * This passes the controller through the curve functions, but is disabled by default. + * Use opcontrol_curve_buttons_toggle() to enable it. + * + * \param stick_type + * ez::SINGLE or ez::SPLIT control + */ + void opcontrol_arcade_standard(e_type stick_type); + + /** + * Sets the chassis to controller joysticks using flipped arcade control, where right stick is fwd/rev. + * Run in usercontrol. + * + * This passes the controller through the curve functions, but is disabled by default. + * Use opcontrol_curve_buttons_toggle() to enable it. + * + * \param stick_type + * ez::SINGLE or ez::SPLIT control + */ + void opcontrol_arcade_flipped(e_type stick_type); + + /** + * Initializes left and right curves with the SD card, recommended to run in initialize(). + */ + void opcontrol_curve_sd_initialize(); + + /** + * Sets the default joystick curves. + * + * \param left + * left default curve + * \param right + * right default curve + */ + void opcontrol_curve_default_set(double left, double right = 0); + + /** + * Gets the default joystick curves, in {left, right} + */ + std::vector opcontrol_curve_default_get(); + + /** + * Runs a PID loop on the drive when the joysticks are released. + * + * \param kp + * proportional term + * \param ki + * integral term + * \param kd + * derivative term + * \param start_i + * error threshold to start integral + */ + void opcontrol_drive_activebrake_set(double kp, double ki = 0.0, double kd = 0.0, double start_i = 0.0); + + /** + * Returns kP for active brake. + */ + double opcontrol_drive_activebrake_get(); + + /** + * Returns all PID constants for active brake. + */ + PID::Constants opcontrol_drive_activebrake_constants_get(); + + /** + * Enables/disables modifying the joystick input curves with the controller. + * + * \param input + * true enables, false disables + */ + void opcontrol_curve_buttons_toggle(bool toggle); + + /** + * Gets the current state of the toggle. Enables/disables modifying the joystick input curves with the controller. + * + * True enabled, false disabled. + */ + bool opcontrol_curve_buttons_toggle_get(); + + /** + * Sets buttons for modifying the left joystick curve. + * + * \param decrease + * a pros button enumerator + * \param increase + * a pros button enumerator + */ + void opcontrol_curve_buttons_left_set(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase); + + /** + * Returns a vector of pros controller buttons user for the left joystick curve, in {decrease, increase} + */ + std::vector opcontrol_curve_buttons_left_get(); + + /** + * Sets buttons for modifying the right joystick curve. + * + * \param decrease + * a pros button enumerator + * \param increase + * a pros button enumerator + */ + void opcontrol_curve_buttons_right_set(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase); + + /** + * Returns a vector of pros controller buttons user for the right joystick curve, in {decrease, increase} + */ + std::vector opcontrol_curve_buttons_right_get(); + + /** + * Outputs a curve from 5225A In the Zone. + * + * This gives more control over the robot at lower speeds. + * + * \param x + * joystick input + */ + double opcontrol_curve_left(double x); + + /** + * Outputs a curve from 5225A In the Zone. + * + * This gives more control over the robot at lower speeds. + * + * \param x + * joystick input + */ + double opcontrol_curve_right(double x); + + /** + * Sets a new threshold for the joystick. + * + * The joysticks wil not return a value if they are within this. + * + * \param threshold + * new threshold + */ + void opcontrol_joystick_threshold_set(int threshold); + + /** + * Gets the threshold for the joystick. + */ + int opcontrol_joystick_threshold_get(); + + /** + * Resets drive sensors at the start of opcontrol. + */ + void opcontrol_drive_sensors_reset(); + + /** + * Sets minimum value distance constants. + * + * \param l_stick + * input for left joystick + * \param r_stick + * input for right joystick + */ + void opcontrol_joystick_threshold_iterate(int l_stick, int r_stick); + + ///// + // + // PTO + // + ///// + + /** + * Checks if the motor is currently in pto_list. + * + * Returns true if it's already in pto_list. + * + * \param check_if_pto + * motor to check + */ + bool pto_check(pros::Motor check_if_pto); + + /** + * Adds motors to the pto list, removing them from the drive. + * + * You cannot add the first index because it's used for autonomous. + * + * \param pto_list + * list of motors to remove from the drive + */ + void pto_add(std::vector pto_list); + + /** + * Removes motors from the pto list, adding them to the drive. + * + * \param pto_list + * list of motors to add to the drive + */ + void pto_remove(std::vector pto_list); + + /** + * Adds/removes motors from drive. + * + * You cannot add the first index because it's used for autonomous. + * + * \param pto_list + * list of motors to add/remove from the drive + * \param toggle + * list of motors to add/remove from the drive + */ + void pto_toggle(std::vector pto_list, bool toggle); + + ///// + // + // PROS Wrappers + // + ///// + + /** + * Sets the chassis to voltage. + * + * Disables PID when called. + * + * \param left + * voltage for left side, -127 to 127 + * \param right + * voltage for right side, -127 to 127 + */ + void drive_set(int left, int right); + + /** + * Gets the chassis to voltage, -127 to 127. Returns {left, right}. + */ + std::vector drive_get(); + + /** + * Changes the way the drive behaves when it is not under active user control. + * + * \param brake_type + * the 'brake mode' of the motor e.g. 'pros::E_MOTOR_BRAKE_COAST' 'pros::E_MOTOR_BRAKE_BRAKE' 'pros::E_MOTOR_BRAKE_HOLD' + */ + void drive_brake_set(pros::motor_brake_mode_e_t brake_type); + + /** + * Returns the brake mode of the drive in pros_brake_mode_e_t_. + */ + pros::motor_brake_mode_e_t drive_brake_get(); + + /** + * Sets the limit for the current on the drive. + * + * \param mA + * input in milliamps + */ + void drive_current_limit_set(int mA); + + /** + * Gets the limit for the current on the drive. + */ + int drive_current_limit_get(); + + /** + * Toggles set drive in autonomous. + * + * \param toggle + * true enables, false disables + */ + void pid_drive_toggle(bool toggle); + + /** + * Gets the current state of the toggle. + * + * This toggles set drive in autonomous. + * + * True enabled, false disabled. + */ + bool pid_drive_toggle_get(); + + /** + * Toggles printing in autonomous. + * + * \param toggle + * true enables, false disables + */ + void pid_print_toggle(bool toggle); + + /** + * Gets the current state of the toggle. + * + * This toggles printing in autonomous. + * + * True enabled, false disabled. + */ + bool pid_print_toggle_get(); + + ///// + // + // Telemetry + // + ///// + + /** + * The position of the right sensor in inches. + * + * If you have two parallel tracking wheels, this will return tracking wheel position. Otherwise this returns motor position. + */ + double drive_sensor_right(); + + /** + * The position of the right sensor. + * + * If you have two parallel tracking wheels, this will return tracking wheel position. Otherwise this returns motor position. + */ + int drive_sensor_right_raw(); + + /** + * The velocity of the right motor. + */ + int drive_velocity_right(); + + /** + * The watts of the right motor. + */ + double drive_mA_right(); + + /** + * Return true when the motor is over current. + */ + bool drive_current_right_over(); + + /** + * The position of the left sensor in inches. + * + * If you have two parallel tracking wheels, this will return tracking wheel position. Otherwise this returns motor position. + */ + double drive_sensor_left(); + + /** + * The position of the left sensor. + * + * If you have two parallel tracking wheels, this will return tracking wheel position. Otherwise this returns motor position. + */ + int drive_sensor_left_raw(); + + /** + * The velocity of the left motor. + */ + int drive_velocity_left(); + + /** + * The watts of the left motor. + */ + double drive_mA_left(); + + /** + * Return true when the motor is over current. + */ + bool drive_current_left_over(); + + /** + * Reset all the chassis motors and tracking wheels, recommended to run at the start of your autonomous routine. + */ + void drive_sensor_reset(); + + /** + * Resets the current imu value. Defaults to 0, recommended to run at the start of your autonomous routine. + * + * \param new_heading + * new heading value + */ + void drive_imu_reset(double new_heading = 0); + + /** + * Returns the current imu rotation value in degrees. + */ + double drive_imu_get(); /** * Returns the current imu accel x + accel y value. */ - double drive_imu_accel_get(); + double drive_imu_accel_get(); + + /** + * Sets a new imu scaling factor. + * + * This value is multiplied by the imu to change its output. + * + * \param scaler + * factor to scale the imu by + */ + void drive_imu_scaler_set(double scaler); + + /** + * Returns the current imu scaling factor. + */ + double drive_imu_scaler_get(); + + /** + * Calibrates the IMU, recommended to run in initialize(). + * + * \param run_loading_animation + * true runs the animation, false doesn't + */ + bool drive_imu_calibrate(bool run_loading_animation = true); + + /** + * Checks if the imu calibrated successfully or if it took longer than expected. + * + * Returns true if calibrated successfully, and false if unsuccessful. + */ + bool drive_imu_calibrated(); + + /** + * Loading display while the IMU calibrates. + */ + void drive_imu_display_loading(int iter); + + /** + * Practice mode for driver practice that shuts off the drive if you go max speed. + * + * \param toggle + * true enables, false disables + */ + void opcontrol_joystick_practicemode_toggle(bool toggle); + + /** + * Gets current state of the toggle. + * + * True is enabled, false is disabled. + */ + bool opcontrol_joystick_practicemode_toggle_get(); + + /** + * Reversal for drivetrain in opcontrol that flips the left and right side and the direction of the drive. + * + * \param toggle + * true if you want your drivetrain reversed and false if you do not + */ + void opcontrol_drive_reverse_set(bool toggle); + + /** + * Gets current state of the toggle. + * + * Reversal for drivetrain in opcontrol that flips the left and right side and the direction of the drive. + */ + bool opcontrol_drive_reverse_get(); + + ///// + // + // Autonomous Functions + // + ///// + + /** + * Sets the robot to move forward using PID without okapi units, only using slew if globally enabled. + * + * This function is actually odom. + * + * \param target + * target value as a double, unit is inches + * \param speed + * 0 to 127, max speed during motion + */ + void pid_odom_set(double target, int speed); + + /** + * Sets the robot to move forward using PID without okapi units, using slew if enabled for this motion. + * + * This function is actually odom + * + * \param target + * target value as a double, unit is inches + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_set(double target, int speed, bool slew_on); + + /** + * Sets the robot to move forward using PID with okapi units, only using slew if globally enabled. + * + * This function is actually odom + * + * \param target + * target value in inches + * \param speed + * 0 to 127, max speed during motion + */ + void pid_odom_set(okapi::QLength p_target, int speed); + + /** + * Sets the robot to move forward using PID with okapi units, using slew if enabled for this motion. + * + * This function is actually odom + * + * \param target + * target value in inches + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + * \param toggle_heading + * toggle for heading correction. true enables, false disables + */ + void pid_odom_set(okapi::QLength p_target, int speed, bool slew_on); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if globally enabled. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement + */ + void pid_odom_set(odom imovement); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if enabled for this motion. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_set(odom imovement, bool slew_on); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if globally enabled. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement + */ + void pid_odom_ptp_set(odom imovement); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if enabled for this motion. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_ptp_set(odom imovement, bool slew_on); + + /** + * Takes in an odom movement to go to a single point using boomerang. If an angle is set, this will run boomerang. Uses slew if globally enabled. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement + */ + void pid_odom_boomerang_set(odom imovement); + + /** + * Takes in an odom movement to go to a single point using boomerang. If an angle is set, this will run boomerang. Uses slew if enabled for this motion. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_boomerang_set(odom imovement, bool slew_on); + + /** + * Takes in an odom movement to go to a single point using boomerang. If an angle is set, this will run boomerang. Uses slew if globally enabled. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement. values are united here with okapi units + */ + void pid_odom_boomerang_set(united_odom p_imovement); + + /** + * Takes in an odom movement to go to a single point using boomerang. If an angle is set, this will run boomerang. Uses slew if enabled for this motion. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_boomerang_set(united_odom p_imovement, bool slew_on); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if globally enabled. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement. values are united here with okapi units + */ + void pid_odom_ptp_set(united_odom p_imovement); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if enabled for this motion. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_ptp_set(united_odom p_imovement, bool slew_on); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if globally enabled. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement. values are united here with okapi units + */ + void pid_odom_set(united_odom p_imovement); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if enabled for this motion. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_set(united_odom p_imovement, bool slew_on); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + */ + void pid_odom_set(std::vector imovements); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_set(std::vector imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + */ + void pid_odom_pp_set(std::vector imovements); + + /** + * Takes in odom movements to go through multiple points. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_pp_set(std::vector imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points, will inject into the path. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + */ + void pid_odom_injected_pp_set(std::vector imovements); + + /** + * Takes in odom movements to go through multiple points, will inject into the path. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_injected_pp_set(std::vector imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + */ + void pid_odom_smooth_pp_set(std::vector imovements); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_smooth_pp_set(std::vector imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + */ + void pid_odom_smooth_pp_set(std::vector p_imovements); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_smooth_pp_set(std::vector p_imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points, will inject into the path. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + */ + void pid_odom_injected_pp_set(std::vector p_imovements); + + /** + * Takes in odom movements to go through multiple points, will inject into the path. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_injected_pp_set(std::vector p_imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + */ + void pid_odom_pp_set(std::vector p_imovements); + + /** + * Takes in odom movements to go through multiple points. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_pp_set(std::vector p_imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + */ + void pid_odom_set(std::vector p_imovements); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_set(std::vector p_imovements, bool slew_on); + + /** + * Sets the robot to move forward using PID with okapi units, only using slew if globally enabled. + * + * \param p_target + * target okapi unit + * \param speed + * 0 to 127, max speed during motion + */ + void pid_drive_set(okapi::QLength p_target, int speed); + + /** + * Sets the robot to move forward using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target okapi unit + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + * \param toggle_heading + * toggle for heading correction. true enables, false disables + */ + void pid_drive_set(okapi::QLength p_target, int speed, bool slew_on, bool toggle_heading = true); + + /** + * Sets the robot to move forward using PID without okapi units, only using slew if globally enabled. + * + * \param target + * target value in inches + * \param speed + * 0 to 127, max speed during motion + */ + void pid_drive_set(double target, int speed); + + /** + * Sets the robot to move forward using PID without okapi units, using slew if enabled for this motion. + * + * \param target + * target value in inches + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + * \param toggle_heading + * toggle for heading correction. true enables, false disables + */ + void pid_drive_set(double target, int speed, bool slew_on, bool toggle_heading = true); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face + * \param speed + * 0 to 127, max speed during motion + */ + void pid_turn_set(pose itarget, drive_directions dir, int speed); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(pose itarget, drive_directions dir, int speed, bool slew_on); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_set(pose itarget, drive_directions dir, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(pose itarget, drive_directions dir, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face. this uses okapi units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face. this uses okapi units + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, bool slew_on); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face. this uses okapi units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face. this uses okapi units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn relative to initial heading using PID. + * + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(double target, int speed); + + /** + * Sets the robot to turn relative to initial heading using PID. + * + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_set(double target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn relative to initial heading using PID, using slew if enabled for this motion. + * + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(double target, int speed, bool slew_on); + + /** + * Sets the robot to turn relative to initial heading using PID, using slew if enabled for this motion. + * + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(double target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn relative to initial heading using PID with okapi units. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_turn_set(okapi::QAngle p_target, int speed); + + /** + * Sets the robot to turn relative to initial heading using PID with okapi units. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn relative to initial heading using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(okapi::QAngle p_target, int speed, bool slew_on); + + /** + * Sets the robot to turn relative to initial heading using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn relative to current heading using PID with okapi units, only using slew if globally enabled. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_turn_relative_set(okapi::QAngle p_target, int speed); + + /** + * Sets the robot to turn relative to current heading using PID with okapi units, only using slew if globally enabled. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_relative_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn relative to current heading using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_relative_set(okapi::QAngle p_target, int speed, bool slew_on); + + /** + * Sets the robot to turn relative to current heading using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_relative_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn relative to current heading using PID without okapi units, only using slew if globally enabled. + * + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_turn_relative_set(double target, int speed); + + /** + * Sets the robot to turn relative to current heading using PID without okapi units, only using slew if globally enabled. + * + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_relative_set(double target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn relative to current heading using PID without okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_relative_set(double target, int speed, bool slew_on); + + /** + * Sets the robot to turn relative to current heading using PID without okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_relative_set(double target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, double target, int speed); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, double target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, double target, int speed, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, double target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, double target, int speed, int opposite_speed); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior); /** - * Sets a new imu scaling factor. This value is multiplied by the imu to change its output. + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, using slew if enabled for this motion. * - * \param scaler - * Factor to scale the imu by. + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void drive_imu_scaler_set(double scaler); + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, bool slew_on); /** - * Returns the current imu scaling factor. + * Sets the robot to turn only using the left or right side relative to initial heading using PID with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - double drive_imu_scaler_get(); + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on); /** - * Calibrates the IMU, recommended to run in initialize(). + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, only using slew if globally enabled. * - * \param run_loading_animation - * bool for running loading animation + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion */ - bool drive_imu_calibrate(bool run_loading_animation = true); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed); /** - * Loading display while the IMU calibrates. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion */ - void drive_imu_display_loading(int iter); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior); /** - * Practice mode for driver practice that shuts off the drive if you go max speed. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, using slew if enabled for this motion. * - * @param toggle True if you want this mode enables and False if you want it disabled. + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion */ - void opcontrol_joystick_practicemode_toggle(bool toggle); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, bool slew_on); /** - * Gets current state of the toggle. Practice mode for driver practice that shuts off the drive if you go max speed. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion */ - bool opcontrol_joystick_practicemode_toggle_get(); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on); /** - * Reversal for drivetrain in opcontrol that flips the left and right side and the direction of the drive + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, only using slew if globally enabled. * - * @param toggle True if you want your drivetrain reversed and False if you do not. + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void opcontrol_drive_reverse_set(bool toggle); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed); /** - * Gets current state of the toggle. Reversal for drivetrain in opcontrol that flips the left and right side and the direction of the drive. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - bool opcontrol_drive_reverse_get(); - - ///// - // - // Autonomous Functions - // - ///// + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior); /** - * Sets the robot to move forward using PID with okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, using slew if enabled for this motion. * - * \param target - * target value in inches + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units * \param speed * 0 to 127, max speed during motion - * \param slew_on - * ramp up from a lower speed to your target speed - * \param toggle_heading - * toggle for heading correction + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void pid_drive_set(okapi::QLength p_target, int speed, bool slew_on = false, bool toggle_heading = true); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, bool slew_on); /** - * Sets the robot to move forward using PID without okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, using slew if enabled for this motion. * - * \param target - * target value as a double, unit is inches + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units * \param speed * 0 to 127, max speed during motion - * \param slew_on - * ramp up from a lower speed to your target speed - * \param toggle_heading - * toggle for heading correction + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void pid_drive_set(double target, int speed, bool slew_on = false, bool toggle_heading = true); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on); /** - * Sets the robot to turn using PID. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, only using slew if globally enabled. * - * \param target + * \param type + * L_SWING or R_SWING + * \param p_target * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion - * \param slew_on - * ramp up from a lower speed to your target speed */ - void pid_turn_set(double target, int speed, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed); /** - * Sets the robot to turn using PID with okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, only using slew if globally enabled. * + * \param type + * L_SWING or R_SWING * \param p_target - * target value in degrees + * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion - * \param slew_on - * ramp up from a lower speed to your target speed */ - void pid_turn_set(okapi::QAngle p_target, int speed, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, e_angle_behavior behavior); /** - * Sets the robot to turn relative to current heading using PID with okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, using slew if enabled for this motion. * + * \param type + * L_SWING or R_SWING * \param p_target - * target value in okapi angle units + * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion - * \param slew_on - * ramp up from a lower speed to your target speed */ - void pid_turn_relative_set(okapi::QAngle p_target, int speed, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, bool slew_on); /** - * Sets the robot to turn relative to current heading using PID without okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, using slew if enabled for this motion. * + * \param type + * L_SWING or R_SWING * \param p_target * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion - * \param slew_on - * ramp up from a lower speed to your target speed */ - void pid_turn_relative_set(double target, int speed, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, e_angle_behavior behavior, bool slew_on); /** - * Turn using only the left or right side without okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, only using slew if globally enabled. * * \param type * L_SWING or R_SWING @@ -800,40 +2422,40 @@ class Drive { * \param speed * 0 to 127, max speed during motion * \param opposite_speed - * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void pid_swing_set(e_swing type, double target, int speed, int opposite_speed = 0, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed); /** - * Turn using only the left or right side with okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, only using slew if globally enabled. * * \param type * L_SWING or R_SWING * \param p_target - * target value in degrees + * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion * \param opposite_speed - * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed = 0, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior); /** - * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, using slew if enabled for this motion. * * \param type * L_SWING or R_SWING * \param p_target - * target value in okapi angle units + * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion * \param opposite_speed - * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed = 0, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on); /** - * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, using slew if enabled for this motion. * * \param type * L_SWING or R_SWING @@ -842,9 +2464,9 @@ class Drive { * \param speed * 0 to 127, max speed during motion * \param opposite_speed - * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed = 0, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on); /** * Resets all PID targets to 0. @@ -852,12 +2474,12 @@ class Drive { void pid_targets_reset(); /** - * Sets heading of imo and target of PID, okapi angle. + * Sets heading of imu and target of PID, okapi angle. */ void drive_angle_set(okapi::QAngle p_angle); /** - * Sets heading of imo and target of PID, takes double as an angle. + * Sets heading of imu and target of PID, takes double as an angle. */ void drive_angle_set(double angle); @@ -886,40 +2508,97 @@ class Drive { * Lock the code in a while loop until this position has passed for driving without okapi units. * * \param target - * for driving or turning, using a double. degrees for turns/swings, inches for driving. + * for driving or turning, using a double. degrees for turns/swings, inches for driving */ void pid_wait_until(double target); /** * Lock the code in a while loop until the robot has settled. - * Wrapper for pid_wait_until(target), target is your previously input target + * + * Wrapper for pid_wait_until(target), target is your previously input target. */ void pid_wait_quick(); /** * Lock the code in a while loop until the robot has settled. - * This also adds distance to target, and then exits with pid_wait_quick + * + * This also adds distance to target, and then exits with pid_wait_quick. + * * This will exit the motion while carrying momentum into the next motion. */ void pid_wait_quick_chain(); /** - * Autonomous interference detection. Returns true when interfered, and false when nothing happened. + * Lock the code in a while loop until this point has been passed. + * + * \param index + * index of your input points, 0 is the first point in the index + */ + void pid_wait_until_index(int index); + + /** + * Lock the code in a while loop until this point becomes the target. + * + * \param index + * index of your input points, 0 is the first point in the index + */ + void pid_wait_until_index_started(int index); + + /** + * Lock the code in a while loop until this point has been passed. + * + * \param target + * {x, y} pose for the robot to pass through before the while loop is released + */ + void pid_wait_until_point(pose target); + + /** + * Lock the code in a while loop until this point has been passed, with okapi units. + * + * \param target + * {x, y} pose with units for the robot to pass through before the while loop is released + */ + void pid_wait_until_point(united_pose target); + + /** + * Lock the code in a while loop until this point has been passed. + * + * Wrapper for pid_wait_until_point. + * + * \param target + * {x, y} a pose for the robot to pass through before the while loop is released + */ + void pid_wait_until(pose target); + + /** + * Lock the code in a while loop until this point has been passed, with okapi units. + * + * Wrapper for pid_wait_until_point. + * + * \param target + * {x, y} a pose with units for the robot to pass through before the while loop is released + */ + void pid_wait_until(united_pose target); + + /** + * Autonomous interference detection. + * + * Returns true when interfered, and false when nothing happened. */ bool interfered = false; /** - * @brief Set the ratio of the robot + * Set the ratio of the robot. * - * @param ratio + * \param ratio * ratio of the gears */ void drive_ratio_set(double ratio); /** - * @brief Set the cartridge/wheel rpm of the robot + * Set the cartridge/wheel rpm of the robot. * - * @param rpm + * \param rpm * rpm of the cartridge or wheel */ void drive_rpm_set(double rpm); @@ -938,7 +2617,7 @@ class Drive { * Changes max speed during a drive motion. * * \param speed - * new clipped speed + * new clipped speed, between 0 and 127 */ void pid_speed_max_set(int speed); @@ -948,27 +2627,27 @@ class Drive { int pid_speed_max_get(); /** - * @brief Set the turn pid constants object + * Set the turn pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_turn_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. - * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns PID constants with PID::Constants. */ PID::Constants pid_turn_constants_get(); /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This sets turning constants. * * \param input @@ -978,6 +2657,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This sets turning constants. * * \param input @@ -991,67 +2671,67 @@ class Drive { double pid_turn_chain_constant_get(); /** - * @brief Set the swing pid constants object + * Set the swing pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_swing_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. Returns -1 if fwd and rev constants aren't the same! + * Returns PID constants with PID::Constants. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns -1 if fwd and rev constants aren't the same! */ PID::Constants pid_swing_constants_get(); /** - * @brief Set the forward swing pid constants object + * Set the forward swing pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_swing_constants_forward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. - * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns PID constants with PID::Constants. */ PID::Constants pid_swing_constants_forward_get(); /** - * @brief Set the backward swing pid constants object + * Set the backward swing pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_swing_constants_backward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. - * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns PID constants with PID::Constants. */ PID::Constants pid_swing_constants_backward_get(); /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This sets forward and backwards swing constants. * * \param input @@ -1061,6 +2741,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets forward swing constants. * * \param input @@ -1070,6 +2751,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets backward swing constants. * * \param input @@ -1079,6 +2761,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This sets forward and backwards swing constants. * * \param input @@ -1088,6 +2771,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets forward constants. * * \param input @@ -1102,6 +2786,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets backwards swing constants. * * \param input @@ -1115,87 +2800,86 @@ class Drive { double pid_swing_chain_backward_constant_get(); /** - * @brief Set the heading pid constants object + * Set the heading pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_heading_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. - * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns PID constants with PID::Constants. */ PID::Constants pid_heading_constants_get(); /** - * @brief Set the drive pid constants object + * Set the drive pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_drive_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. Returns -1 if fwd and rev constants aren't the same! + * Returns PID constants with PID::Constants. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns -1 if fwd and rev constants aren't the same! */ PID::Constants pid_drive_constants_get(); /** - * @brief Set the forward pid constants object + * Set the forward pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_drive_constants_forward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. - * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns PID constants with PID::Constants. */ PID::Constants pid_drive_constants_forward_get(); /** - * @brief Set the backwards pid constants object + * Set the backwards pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_drive_constants_backward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. - * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns PID constants with PID::Constants. */ PID::Constants pid_drive_constants_backward_get(); /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This sets forward and backwards driving constants. * * \param input @@ -1205,6 +2889,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets forward driving constants. * * \param input @@ -1214,6 +2899,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets backward driving constants. * * \param input @@ -1223,6 +2909,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This sets forward and backwards driving constants. * * \param input @@ -1232,6 +2919,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets forward driving constants. * * \param input @@ -1246,6 +2934,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets backward driving constants. * * \param input @@ -1284,21 +2973,131 @@ class Drive { */ int pid_turn_min_get(); + /** + * Set the odom angular pid constants object. + * + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral + */ + void pid_odom_angular_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); + + /** + * Set the odom boomerang pid constants object. + * + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral + */ + void pid_odom_boomerang_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); + + /** + * Set's constants for odom driving exit conditions. + * + * \param p_small_exit_time + * time to exit when within smalL_error, in ms + * \param p_small_error + * small timer will start when error is within this, in inches + * \param p_big_exit_time + * time to exit when within big_error, in ms + * \param p_big_error + * big timer will start when error is within this, in inches + * \param p_velocity_exit_time + * velocity timer will start when velocity is 0, in ms + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, in ms + * \param use_imu + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't + */ + 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); + + /** + * Set's constants for odom turning exit conditions. + * + * \param p_small_exit_time + * time to exit when within smalL_error, in ms + * \param p_small_error + * small timer will start when error is within this, in degrees + * \param p_big_exit_time + * time to exit when within big_error, in ms + * \param p_big_error + * big timer will start when error is within this, in degrees + * \param p_velocity_exit_time + * velocity timer will start when velocity is 0, in ms + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, in ms + * \param use_imu + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't + */ + 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); + + /** + * Set's constants for odom turning exit conditions. + * + * \param p_small_exit_time + * time to exit when within smalL_error, okapi unit + * \param p_small_error + * small timer will start when error is within this, okapi unit + * \param p_big_exit_time + * time to exit when within big_error, okapi unit + * \param p_big_error + * big timer will start when error is within this, okapi unit + * \param p_velocity_exit_time + * velocity timer will start when velocity is 0, okapi unit + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, okapi unit + * \param use_imu + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't + */ + 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); + + /** + * Set's constants for odom driving exit conditions. + * + * \param p_small_exit_time + * time to exit when within smalL_error, okapi unit + * \param p_small_error + * small timer will start when error is within this, okapi unit + * \param p_big_exit_time + * time to exit when within big_error, okapi unit + * \param p_big_error + * big timer will start when error is within this, okapi unit + * \param p_velocity_exit_time + * velocity timer will start when velocity is 0, okapi unit + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, okapi unit + * \param use_imu + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't + */ + 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); + /** * Set's constants for drive exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. In okapi units. + * time to exit when within smalL_error, okapi unit * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. In okapi units. + * small timer will start when error is within this, okapi unit * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. In okapi units. + * time to exit when within big_error, okapi unit * \param p_big_error - * Sets big_error. Timer will start when error is within this. In okapi units. + * big timer will start when error is within this, okapi unit * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. In okapi units. + * velocity timer will start when velocity is 0, okapi unit + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, okapi unit * \param use_imu - * Adds the Imu for velocity calculation in conjunction with the main sensor. + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't */ 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); @@ -1306,17 +3105,19 @@ class Drive { * Set's constants for turn exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. In okapi units. + * time to exit when within smalL_error, okapi unit * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. In okapi units. + * small timer will start when error is within this, okapi unit * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. In okapi units. + * time to exit when within big_error, okapi unit * \param p_big_error - * Sets big_error. Timer will start when error is within this. In okapi units. + * big timer will start when error is within this, okapi unit * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. In okapi units. + * velocity timer will start when velocity is 0, okapi unit + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, okapi unit * \param use_imu - * Adds the Imu for velocity calculation in conjunction with the main sensor. + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't */ 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); @@ -1324,17 +3125,19 @@ class Drive { * Set's constants for swing exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. In okapi units. + * time to exit when within smalL_error, okapi unit * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. In okapi units. + * small timer will start when error is within this, okapi unit * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. In okapi units. + * time to exit when within big_error, okapi unit * \param p_big_error - * Sets big_error. Timer will start when error is within this. In okapi units. + * big timer will start when error is within this, okapi unit * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. In okapi units. + * velocity timer will start when velocity is 0, okapi unit + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, okapi unit * \param use_imu - * Adds the Imu for velocity calculation in conjunction with the main sensor. + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't */ 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); @@ -1342,17 +3145,19 @@ class Drive { * Set's constants for drive exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. + * time to exit when within smalL_error, in ms * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. + * small timer will start when error is within this, in inches * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. + * time to exit when within big_error, in ms * \param p_big_error - * Sets big_error. Timer will start when error is within this. + * big timer will start when error is within this, in inches * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. + * velocity timer will start when velocity is 0, in ms + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, in ms * \param use_imu - * Adds the Imu for velocity calculation in conjunction with the main sensor. + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't */ 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); @@ -1360,17 +3165,19 @@ class Drive { * Set's constants for turn exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. + * time to exit when within smalL_error, in ms * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. + * small timer will start when error is within this, in degrees * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. + * time to exit when within big_error, in ms * \param p_big_error - * Sets big_error. Timer will start when error is within this. + * big timer will start when error is within this, in degrees * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. + * velocity timer will start when velocity is 0, in ms + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, in ms * \param use_imu - * Adds the Imu for velocity calculation in conjunction with the main sensor. + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't */ 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); @@ -1378,57 +3185,61 @@ class Drive { * Set's constants for swing exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. + * time to exit when within smalL_error, in ms * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. + * small timer will start when error is within this, in degrees * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. + * time to exit when within big_error, in ms * \param p_big_error - * Sets big_error. Timer will start when error is within this. + * big timer will start when error is within this, in degrees * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. + * velocity timer will start when velocity is 0, in ms + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, in ms * \param use_imu - * Adds the Imu for velocity calculation in conjunction with the main sensor. + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't */ 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); /** - * Returns current tick_per_inch() + * Returns current tick_per_inch. */ double drive_tick_per_inch(); /** - * Returns current tick_per_inch() + * Iterates modifying controller curves with the controller. */ void opcontrol_curve_buttons_iterate(); /** - * Enables PID Tuner + * Enables PID Tuner. */ void pid_tuner_enable(); /** - * Disables PID Tuner + * Disables PID Tuner. */ void pid_tuner_disable(); /** - * Toggles PID tuner between enabled and disables + * Toggles PID tuner between enabled and disabled. */ void pid_tuner_toggle(); /** - * Checks if PID Tuner is enabled. True is enabled, false is disables. + * Checks if PID Tuner is enabled. + * + * True is enabled, false is disabled. */ bool pid_tuner_enabled(); /** - * Iterates through controller inputs to modify PID constants + * Iterates through controller inputs to modify PID constants. */ void pid_tuner_iterate(); /** - * Toggle for printing the display of the PID Tuner to the brain + * Toggle for printing the display of the PID Tuner to the brain. * * \param input * true prints to brain, false doesn't @@ -1436,7 +3247,7 @@ class Drive { void pid_tuner_print_brain_set(bool input); /** - * Toggle for printing the display of the PID Tuner to the terminal + * Toggle for printing the display of the PID Tuner to the terminal. * * \param input * true prints to terminal, false doesn't @@ -1444,68 +3255,230 @@ class Drive { void pid_tuner_print_terminal_set(bool input); /** - * Returns true if printing to terminal is enabled + * Returns true if printing to terminal is enabled. */ bool pid_tuner_print_terminal_enabled(); /** - * Returns true if printing to brain is enabled + * Returns true if printing to brain is enabled. */ bool pid_tuner_print_brain_enabled(); /** - * Sets the value that PID Tuner increments P + * Sets the value that PID Tuner increments P. * * \param p - * double, p will increase by this + * p will increase by this */ void pid_tuner_increment_p_set(double p); /** - * Sets the value that PID Tuner increments I + * Sets the value that PID Tuner increments I. * * \param p - * double, i will increase by this + * i will increase by this */ void pid_tuner_increment_i_set(double i); /** - * Sets the value that PID Tuner increments D + * Sets the value that PID Tuner increments D. * * \param p - * double, d will increase by this + * d will increase by this */ void pid_tuner_increment_d_set(double d); /** - * Sets the value that PID Tuner increments Start I + * Sets the value that PID Tuner increments Start I. * * \param p - * double, start i will increase by this + * start i will increase by this */ void pid_tuner_increment_start_i_set(double start_i); /** - * Returns the value that PID Tuner increments P + * Returns the value that PID Tuner increments P. */ double pid_tuner_increment_p_get(); /** - * Returns the value that PID Tuner increments I + * Returns the value that PID Tuner increments I. */ double pid_tuner_increment_i_get(); /** - * Returns the value that PID Tuner increments D + * Returns the value that PID Tuner increments D. */ double pid_tuner_increment_d_get(); /** - * Returns the value that PID Tuner increments Start I + * Returns the value that PID Tuner increments Start I. */ double pid_tuner_increment_start_i_get(); - private: // !Auton + /** + * Enables the full PID tuner with unique fwd/rev constants. + * + * \param enable + * true will enable the full PID tuner, false will use the simplified PID tuner + */ + void pid_tuner_full_enable(bool enable); + + /** + * Returns if the full PID tuner with unique fwd/rev constants is enabled. + * + * True means the full PID tuner is enabled, false means the simplified PID tuner is enabled. + */ + bool pid_tuner_full_enabled(); + + struct const_and_name { + std::string name = ""; + PID::Constants* consts; + }; + + /** + * Vector used for a simplified PID Tuner + */ + std::vector 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}}; + + /** + * Vector used for the full PID Tuner + */ + std::vector 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}}; + + /** + * Sets the max speed for user control. + * + * \param int + * the speed limit + */ + void opcontrol_speed_max_set(int speed); + + /** + * Returns the max speed for user control. + */ + int opcontrol_speed_max_get(); + + /** + * Toggles vector scaling for arcade control. True enables, false disables. + * + * \param bool + * true enables, false disables + */ + void opcontrol_arcade_scaling(bool enable); + + /** + * Returns if vector scaling for arcade control is enabled. True enables, false disables. + */ + 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* used_pid_tuner_pids; + double opcontrol_speed_max = 127.0; + bool arcade_vector_scaling = false; + // odom privates + std::vector pp_movements; + std::vector injected_pp_index; + int pp_index = 0; + std::vector smooth_path(std::vector 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 imovements, bool slew_on); + bool ptf1_running = false; + std::vector 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 inject_points(std::vector imovements); + std::vector 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, ez::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 decide_vert_sensor(ez::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 set_odoms_direction(std::vector 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; @@ -1529,11 +3502,6 @@ class Drive { bool slew_swing_using_angle = false; bool pid_tuner_terminal_b = false; bool pid_tuner_lcd_b = true; - struct const_and_name { - std::string name = ""; - PID::Constants *consts; - }; - std::vector constants; void pid_tuner_print(); void pid_tuner_value_modify(float p, float i, float d, float start); void pid_tuner_value_increase(); @@ -1576,11 +3544,6 @@ class Drive { */ bool heading_on = true; - /** - * Active brake kp constant. - */ - double active_brake_kp = 0; - /** * Tick per inch calculation. */ @@ -1604,6 +3567,9 @@ class Drive { void swing_pid_task(); void turn_pid_task(); void ez_auto_task(); + void ptp_task(); + void boomerang_task(); + void pp_task(); /** * Starting value for left/right @@ -1624,6 +3590,7 @@ class Drive { #define DRIVE_INTEGRATED 1 #define DRIVE_ADI_ENCODER 2 #define DRIVE_ROTATION 3 +#define ODOM_TRACKER 4 /** * Is tracking? @@ -1656,7 +3623,7 @@ class Drive { /** * Function for button presses. */ - void button_press(button_ *input_name, int button, std::function changeCurve, std::function save); + void button_press(button_* input_name, int button, std::function changeCurve, std::function save); /** * The left and right curve scalers. @@ -1677,4 +3644,4 @@ class Drive { */ bool is_reversed = false; }; -}; // namespace ez \ No newline at end of file +}; // namespace ez diff --git a/EZ-Template-Example-Project/include/EZ-Template/piston.hpp b/EZ-Template-Example-Project/include/EZ-Template/piston.hpp index ec091ea8..af11a223 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/piston.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/piston.hpp @@ -17,22 +17,26 @@ class Piston { pros::adi::DigitalOut piston; /** - * Piston constructor. This class keeps track of piston state. The starting position of your piston is FALSE. + * Piston constructor. + * + * The starting position of your piston defaults to false. * * \param input_port - * The ports of your pistons. + * the ports of your pistons * \param default_state - * Starting state of your piston. + * starting state of your piston */ Piston(int input_port, bool default_state = false); /** - * Piston constructor in 3 wire expander. The starting position of your piston is FALSE. + * Piston constructor in 3 wire expander. + * + * The starting position of your piston defaults to false. * * \param input_ports - * The ports of your pistons. + * the ports of your pistons * \param default_state - * Starting state of your piston. + * starting state of your piston */ Piston(int input_port, int expander_smart_port, bool default_state = false); @@ -40,7 +44,7 @@ class Piston { * Sets the piston to the input. * * \param input - * True or false. True sets to the opposite of the starting position. + * true sets to the opposite of the starting position */ void set(bool input); @@ -53,7 +57,7 @@ class Piston { * One button toggle for the piston. * * \param toggle - * An input button. + * an input button */ void button_toggle(int toggle); @@ -61,9 +65,9 @@ class Piston { * Two buttons trigger the piston. Active is enabled, deactive is disabled. * * \param active - * Sets piston to true. + * sets piston to true * \param active - * Sets piston to false. + * sets piston to false */ void buttons(int active, int deactive); diff --git a/EZ-Template-Example-Project/include/EZ-Template/sdcard.hpp b/EZ-Template-Example-Project/include/EZ-Template/sdcard.hpp index ea8d5427..ce1250e8 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/sdcard.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/sdcard.hpp @@ -12,6 +12,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. namespace ez { namespace as { extern AutonSelector auton_selector; + /** * Sets sd card to current page. */ @@ -43,7 +44,7 @@ void initialize(); void shutdown(); /** - * Returns true if the auton selector is running + * Returns true if the auton selector is running. */ bool enabled(); @@ -53,8 +54,9 @@ extern bool turn_off; extern pros::adi::DigitalIn* limit_switch_left; extern pros::adi::DigitalIn* limit_switch_right; + /** - * Initialize two limitswithces to change pages on the lcd + * Initialize two limit switches to change pages on the lcd. * * @param left_limit_port * port for the left limit switch @@ -67,5 +69,35 @@ void limit_switch_lcd_initialize(pros::adi::DigitalIn* right_limit, pros::adi::D * pre_auto_task */ void limitSwitchTask(); + +/** + * Returns the current blank page that is on. Negative value means the current page isn't blank. + */ +int page_blank_current(); + +/** + * Checks if this blank page is open. If this page doesn't exist, this will create it. + */ +bool page_blank_is_on(int page); + +/** + * Removes the blank page if it exists, and previous ones. + */ +void page_blank_remove(int page); + +/** + * Removes all blank pages. + */ +void page_blank_remove_all(); + +/** + * Removes the current amount of blank pages. + */ +int page_blank_amount(); + +/** + * Current amount of blank pages. + */ +extern int amount_of_blank_pages; } // namespace as } // namespace ez diff --git a/EZ-Template-Example-Project/include/EZ-Template/slew.hpp b/EZ-Template-Example-Project/include/EZ-Template/slew.hpp index 53e1fa21..4f33e302 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/slew.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/slew.hpp @@ -77,7 +77,7 @@ class slew { double output(); /** - * Sets the max speed the slew can be + * Sets the max speed the slew can be. * * \param speed * maximum speed @@ -85,7 +85,7 @@ class slew { void speed_max_set(double speed); /** - * Returns the max speed the slew can be + * Returns the max speed the slew can be. */ double speed_max_get(); diff --git a/EZ-Template-Example-Project/include/EZ-Template/tracking_wheel.hpp b/EZ-Template-Example-Project/include/EZ-Template/tracking_wheel.hpp new file mode 100644 index 00000000..05a60b6b --- /dev/null +++ b/EZ-Template-Example-Project/include/EZ-Template/tracking_wheel.hpp @@ -0,0 +1,162 @@ +/* +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 "pros/adi.hpp" +#include "pros/rotation.hpp" + +namespace ez { +class tracking_wheel { + public: + pros::adi::Encoder adi_encoder; + pros::Rotation smart_encoder; + + /** + * Creates a new tracking wheel with an ADI Encoder. + * + * \param ports + * {'A', 'B'}. make the encoder negative if reversed + * \param wheel_diameter + * assumed inches, this is the diameter of your wheel + * \param distance_to_center + * the distance to the center of your robot, this is used for tracking + * \param ratio + * the gear ratio of your tracking wheel. if it's not geared, this should be 1.0 + */ + tracking_wheel(std::vector ports, double wheel_diameter, double distance_to_center = 0.0, double ratio = 1.0); + + /** + * Creates a new tracking wheel with an ADI Encoder plugged into a 3-wire expander. + * + * \param smart_port + * the smart port your ADI Expander is plugged into + * \param ports + * {'A', 'B'}. make the encoder negative if reversed + * \param wheel_diameter + * assumed inches, this is the diameter of your wheel + * \param distance_to_center + * the distance to the center of your robot, this is used for tracking + * \param ratio + * the gear ratio of your tracking wheel. if it's not geared, this should be 1.0 + */ + tracking_wheel(int smart_port, std::vector ports, double wheel_diameter, double distance_to_center = 0.0, double ratio = 1.0); + + /** + * Creates a new tracking wheel with a Rotation sensor. + * + * \param port + * the port your Rotation sensor is plugged into, make this negative if reversed + * \param wheel_diameter + * assumed inches, this is the diameter of your wheel + * \param distance_to_center + * the distance to the center of your robot, this is used for tracking + * \param ratio + * the gear ratio of your tracking wheel. if it's not geared, this should be 1.0 + */ + tracking_wheel(int port, double wheel_diameter, double distance_to_center = 0.0, double ratio = 1.0); + + /** + * Returns how far the wheel has traveled in inches. + */ + double get(); + + /** + * Returns the raw sensor value. + */ + double get_raw(); + + /** + * Sets the distance to the center of the robot. + * + * \param input + * distance to the center of your robot in inches + */ + void distance_to_center_set(double input); + + /** + * Returns the distance to the center of the robot. + */ + double distance_to_center_get(); + + /** + * Sets the distance to the center to be flipped to negative or not. + * + * \param input + * flips distance to center to negative. false leaves it alone, true flips it + */ + void distance_to_center_flip_set(bool input); + + /** + * Returns if the distance to center is flipped or not. False is not, true is. + */ + bool distance_to_center_flip_get(); + + /** + * Resets your sensor. + */ + void reset(); + + /** + * Returns the constant for how many ticks is 1 inch. + */ + double ticks_per_inch(); + + /** + * Sets the amount of ticks per revolution of your sensor. + * + * This is useful for custom encoders. + * + * \param input + * ticks per revolution + */ + void ticks_per_rev_set(double input); + + /** + * Returns the amount of ticks per revolution of your sensor. + */ + double ticks_per_rev_get(); + + /** + * Sets the gear ratio for your tracking wheel. + * + * \param input + * gear ratio of tracking wheel + */ + void ratio_set(double input); + + /** + * Returns the gear ratio for your tracking wheel. + */ + double ratio_get(); + + /** + * Sets the diameter of your wheel. + * + * \param input + * wheel diameter + */ + void wheel_diameter_set(double input); + + /** + * Returns the diameter of your wheel. + */ + double wheel_diameter_get(); + + private: +#define DRIVE_ADI_ENCODER 2 +#define DRIVE_ROTATION 3 + int IS_TRACKER = 0; + + bool IS_FLIPPED = false; + + double DISTANCE_TO_CENTER = 0.0; + double WHEEL_DIAMETER = 0.0; + double RATIO = 1.0; + double ENCODER_TICKS_PER_REV = 0.0; + double WHEEL_TICK_PER_REV = 0.0; +}; +}; // namespace ez diff --git a/EZ-Template-Example-Project/include/EZ-Template/util.hpp b/EZ-Template-Example-Project/include/EZ-Template/util.hpp index 531ab144..94d9d1dc 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/util.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/util.hpp @@ -11,6 +11,12 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include #include "api.h" +#include "okapi/api/units/QAngle.hpp" +#include "okapi/api/units/QLength.hpp" +#include "okapi/api/units/QTime.hpp" +#include "util.hpp" + +using namespace okapi::literals; /** * Controller. @@ -20,18 +26,19 @@ extern pros::Controller master; namespace ez { /** - * Prints our branding all over your pros terminal + * Prints our branding all over your pros terminal. */ void ez_template_print(); /** - * Prints to the brain screen in one string. Splits input between lines with - * '\n' or when text longer then 32 characters. + * Prints to the brain screen in one string. + * + * Splits input between lines with '\n' or when text longer then 32 characters. * - * @param text - * Input string. Use '\n' for a new line - * @param line - * Starting line to print on, defaults to 0 + * \param text + * input string + * \param line + * starting line to print on, defaults to 0 */ void screen_print(std::string text, int line = 0); @@ -69,7 +76,78 @@ enum exit_output { RUNNING = 1, enum e_mode { DISABLE = 0, SWING = 1, TURN = 2, - DRIVE = 3 }; + TURN_TO_POINT = 3, + DRIVE = 4, + POINT_TO_POINT = 5, + PURE_PURSUIT = 6 }; + +/** + * Enum for drive directions. + */ +enum drive_directions { FWD = 0, + FORWARD = FWD, + fwd = FWD, + forward = FWD, + REV = 1, + REVERSE = REV, + rev = REV, + reverse = REV }; + +/** + * Enum for turn types. + */ +enum e_angle_behavior { raw = 0, + left_turn = 1, + LEFT_TURN = 1, + counterclockwise = 1, + ccw = 1, + right_turn = 2, + RIGHT_TURN = 2, + clockwise = 2, + cw = 2, + shortest = 3, + longest = 4 }; + +const double ANGLE_NOT_SET = 0.0000000000000000000001; +const okapi::QAngle p_ANGLE_NOT_SET = 0.0000000000000000000001_deg; + +/** + * Struct for coordinates. + */ +typedef struct pose { + double x; + double y; + double theta = ANGLE_NOT_SET; +} pose; + +/** + * Struct for united coordinates. + */ +typedef struct united_pose { + okapi::QLength x; + okapi::QLength y; + okapi::QAngle theta = p_ANGLE_NOT_SET; +} united_pose; + +/** + * Struct for odom movements. + */ +typedef struct odom { + pose target; + drive_directions drive_direction; + int max_xy_speed; + e_angle_behavior turn_behavior = shortest; +} odom; + +/** + * Struct for united odom movements. + */ +typedef struct united_odom { + united_pose target; + drive_directions drive_direction; + int max_xy_speed; + e_angle_behavior turn_behavior = shortest; +} united_odom; /** * Outputs string for exit_condition enum. @@ -80,28 +158,176 @@ namespace util { extern bool AUTON_RAN; /** - * Returns 1 if input is positive and -1 if input is negative + * Returns the amount of places after a decimal, maxing out at 6. + * + * \param input + * your input value with decimals + * \param min + * minimum number of decimal places to print, this is defaulted to 0 + */ +int places_after_decimal(double input, int min = 0); + +/** + * Returns a string with a specific number of decimal points. + * + * \param input + * your input value + * \param n + * the amount of decimals you want to display + */ +std::string to_string_with_precision(double input, int n = 2); + +/** + * Returns 1 if input is positive and -1 if input is negative. + * + * \param input + * your input value */ int sgn(double input); /** - * Returns true if the input is < 0 + * Returns true if the input is < 0. + * + * \param input + * your input value */ bool reversed_active(double input); /** - * Returns input restricted to min-max threshold + * Returns input restricted to min-max threshold. + * + * \param input + * your input value + * \param max + * the maximum input can be + * \param min + * the minimum input can be */ double clamp(double input, double max, double min); +/** + * Returns input restricted to min-max threshold. + * + * The minimum used is negative max. + * + * \param input + * your input value + * \param max + * the absolute value maximum input can be + */ +double clamp(double input, double max); + /** * Is the SD card plugged in? */ const bool SD_CARD_ACTIVE = pros::usd::is_installed(); /** - * Delay time for tasks + * Delay time for tasks, this is set to 10 ms. */ const int DELAY_TIME = 10; + +/** + * Converts radians to degrees. + * + * \param input + * your input radian + */ +double to_deg(double input); + +/** + * Converts degrees to radians. + * + * \param input + * your input degree + */ +double to_rad(double input); + +/** + * Returns the angle between two points. + * + * \param itarget + * target position + * \param icurrent + * current position + */ +double absolute_angle_to_point(pose itarget, pose icurrent); + +/** + * Returns the distance between two points. + * + * \param itarget + * target position + * \param icurrent + * current position + */ +double distance_to_point(pose itarget, pose icurrent); + +/** + * Constrains an angle between 180 and -180. + * + * \param theta + * input angle in degrees + */ +double wrap_angle(double theta); + +/** + * Returns a new pose that is projected off of the current pose. + * + * \param added + * how far to project a new point + * \param icurrent + * point to project off of + */ +pose vector_off_point(double added, pose icurrent); + +/** + * Returns the shortest angle for the robot to turn to in order to get to target. + * + * \param target + * target value in degrees + * \param current + * current value in degrees + * \param print = false + * will print the new value if this is true, defaults to false + */ +double turn_shortest(double target, double current, bool print = false); + +/** + * Returns the farthest away angle for the robot to turn to in order to get to target. + * + * \param target + * target value in degrees + * \param current + * current value in degrees + * \param print = false + * will print the new value if this is true, defaults to false + */ +double turn_longest(double target, double current, bool print = false); + +/** + * Converts pose with okapi units to a pose without okapi units. + * + * \param input + * a pose with units + */ +pose united_pose_to_pose(united_pose input); + +/** + * Converts vector of poses with okapi units to a vector of poses without okapi units. + * + * \param inputs + * poses with units + */ +std::vector united_odoms_to_odoms(std::vector inputs); + +/** + * Converts odom movement with united pose to an odom movement without united pose. + * + * \param input + * odom movement with units + */ +odom united_odom_to_odom(united_odom input); + } // namespace util } // namespace ez diff --git a/EZ-Template-Example-Project/include/api.h b/EZ-Template-Example-Project/include/api.h index b97e65d6..d56915cc 100644 --- a/EZ-Template-Example-Project/include/api.h +++ b/EZ-Template-Example-Project/include/api.h @@ -41,9 +41,9 @@ #define PROS_VERSION_MAJOR 4 #define PROS_VERSION_MINOR 1 +#define PROS_VERSION_PATCH 1 +#define PROS_VERSION_STRING "4.1.1" -#define PROS_VERSION_PATCH 0 -#define PROS_VERSION_STRING "4.1.0" #include "pros/adi.h" #include "pros/colors.h" diff --git a/EZ-Template-Example-Project/include/autons.hpp b/EZ-Template-Example-Project/include/autons.hpp index 654902d8..99ae3ab9 100644 --- a/EZ-Template-Example-Project/include/autons.hpp +++ b/EZ-Template-Example-Project/include/autons.hpp @@ -1,8 +1,6 @@ #pragma once -#include "EZ-Template/drive/drive.hpp" - -extern Drive chassis; +void default_constants(); void drive_example(); void turn_example(); @@ -12,5 +10,9 @@ void swing_example(); void motion_chaining(); void combining_movements(); void interfered_example(); - -void default_constants(); \ No newline at end of file +void odom_drive_example(); +void odom_pure_pursuit_example(); +void odom_pure_pursuit_wait_until_example(); +void odom_boomerang_example(); +void odom_boomerang_injected_pure_pursuit_example(); +void measure_offsets(); \ No newline at end of file diff --git a/EZ-Template-Example-Project/include/pros/optical.h b/EZ-Template-Example-Project/include/pros/optical.h index 049c395b..61a2c86d 100644 --- a/EZ-Template-Example-Project/include/pros/optical.h +++ b/EZ-Template-Example-Project/include/pros/optical.h @@ -460,6 +460,39 @@ int32_t optical_enable_gesture(uint8_t port); */ int32_t optical_disable_gesture(uint8_t port); +/** + * Get integration time (update rate) of the optical sensor in milliseconds, with + * minimum time being + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Optical Sensor + * + * \param port + * The V5 Optical Sensor port number from 1-21 + * \return Integration time in milliseconds if the operation is successful + * or PROS_ERR if the operation failed, setting errno. + */ +double optical_get_integration_time(uint8_t port); + +/** + * Set integration time (update rate) of the optical sensor in milliseconds. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Optical Sensor + * + * \param port + * The V5 Optical Sensor port number from 1-21 + * \param time + * The desired integration time in milliseconds + * \return 1 if the operation is successful or PROS_ERR if the operation failed, + * setting errno. + */ +int32_t optical_set_integration_time(uint8_t port, double time); + ///@} ///@} diff --git a/EZ-Template-Example-Project/include/pros/optical.hpp b/EZ-Template-Example-Project/include/pros/optical.hpp index 463daa8a..b3e1d4e9 100644 --- a/EZ-Template-Example-Project/include/pros/optical.hpp +++ b/EZ-Template-Example-Project/include/pros/optical.hpp @@ -398,6 +398,36 @@ class Optical : public Device { */ virtual std::int32_t disable_gesture(); + /** + * Set integration time (update rate) of the optical sensor in milliseconds, with + * minimum time being 3 ms and maximum time being 712 ms. Default is 100 ms, with the + * optical sensor communciating with the V5 brain every 20 ms. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Optical Sensor + * + * \return 1 if the operation is successful or PROS_ERR_F if the operation failed, + * setting errno. + */ + double get_integration_time(); + + /** + * Get integration time (update rate) of the optical sensor in milliseconds. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Optical Sensor + * + * \param time + * The desired integration time in milliseconds + * \return Integration time in milliseconds if the operation is successful + * or PROS_ERR if the operation failed, setting errno. + */ + std::int32_t set_integration_time(double time); + /** * This is the overload for the << operator for printing to streams * diff --git a/EZ-Template-Example-Project/include/subsystems.hpp b/EZ-Template-Example-Project/include/subsystems.hpp index 404e2418..294a3a4d 100644 --- a/EZ-Template-Example-Project/include/subsystems.hpp +++ b/EZ-Template-Example-Project/include/subsystems.hpp @@ -1,7 +1,10 @@ #pragma once +#include "EZ-Template/api.hpp" #include "api.h" +extern Drive chassis; + // Your motors, sensors, etc. should go here. Below are examples // inline pros::Motor intake(1); diff --git a/EZ-Template-Example-Project/project.pros b/EZ-Template-Example-Project/project.pros index a728fdf3..2eff0d9c 100644 --- a/EZ-Template-Example-Project/project.pros +++ b/EZ-Template-Example-Project/project.pros @@ -5,31 +5,32 @@ "target": "v5", "templates": { "EZ-Template": { - "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\EZ-Template@3.1.0", + "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\EZ-Template@3.2.0", "metadata": { "origin": "local" }, "name": "EZ-Template", "py/object": "pros.conductor.templates.local_template.LocalTemplate", - "supported_kernels": "^4.1.0", + "supported_kernels": "^4.1.1", "system_files": [ + "include\\EZ-Template\\auton.hpp", "include\\EZ-Template\\drive\\drive.hpp", - "include\\EZ-Template\\sdcard.hpp", - "include\\EZ-Template\\auton_selector.hpp", + "include\\EZ-Template\\piston.hpp", + "include\\EZ-Template\\tracking_wheel.hpp", "include\\EZ-Template\\api.hpp", "include\\EZ-Template\\PID.hpp", - "include\\EZ-Template\\auton.hpp", - "include\\EZ-Template\\piston.hpp", + "include\\EZ-Template\\util.hpp", "include\\EZ-Template\\slew.hpp", - "firmware\\EZ-Template.a", - "include\\EZ-Template\\util.hpp" + "include\\EZ-Template\\auton_selector.hpp", + "include\\EZ-Template\\sdcard.hpp", + "firmware\\EZ-Template.a" ], "target": "v5", "user_files": [], - "version": "3.1.0" + "version": "3.2.0" }, "kernel": { - "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\kernel@4.1.0", + "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\kernel@4.1.1", "metadata": { "cold_addr": "58720256", "cold_output": "bin/cold.package.bin", @@ -42,64 +43,66 @@ "py/object": "pros.conductor.templates.local_template.LocalTemplate", "supported_kernels": null, "system_files": [ - "include\\pros\\rtos.hpp", - "firmware\\libpros.a", - "include\\pros\\imu.hpp", + "include/pros/gps.hpp", + "include/pros/motors.h", + "include/pros/error.h", + "include/pros/imu.hpp", + "include/pros/serial.h", + "include/pros/rtos.h", + "include/pros/optical.hpp", + "firmware/libc.a", + "include/pros/distance.hpp", + "include/pros/rotation.h", "common.mk", - "include\\pros\\rotation.hpp", - "include\\pros\\abstract_motor.hpp", - "include\\pros\\serial.h", - "include\\pros\\vision.hpp", - "include\\pros\\colors.h", - "include\\pros\\colors.hpp", - "include\\pros\\device.hpp", - "include\\pros\\misc.h", - "include\\api.h", - "include\\pros\\link.h", - "firmware\\v5-hot.ld", - "include\\pros\\distance.h", - "firmware\\v5-common.ld", - "include\\pros\\imu.h", - "include\\pros\\motor_group.hpp", - "include\\pros\\vision.h", - "include\\pros\\misc.hpp", - "include\\pros\\adi.h", - "include\\pros\\screen.h", - "include\\pros\\motors.h", - "include\\pros\\llemu.h", - "include\\pros\\adi.hpp", - "include\\pros\\device.h", - "include\\pros\\motors.hpp", - "include\\pros\\serial.hpp", - "include\\pros\\ext_adi.h", - "include\\pros\\apix.h", - "include\\pros\\rtos.h", - "include\\pros\\rotation.h", - "include\\pros\\llemu.hpp", - "firmware\\libc.a", - "firmware\\v5.ld", - "include\\pros\\gps.h", - "include\\pros\\optical.hpp", - "include\\pros\\gps.hpp", - "firmware\\libm.a", - "include\\pros\\distance.hpp", - "include\\pros\\link.hpp", - "include\\pros\\optical.h", - "include\\pros\\screen.hpp", - "include\\pros\\error.h" + "include/pros/motors.hpp", + "firmware/v5.ld", + "include/pros/gps.h", + "include/pros/link.hpp", + "include/pros/colors.h", + "include/pros/imu.h", + "include/pros/adi.hpp", + "include/pros/ext_adi.h", + "include/pros/misc.hpp", + "include/pros/link.h", + "include/pros/device.h", + "include/pros/misc.h", + "firmware/v5-common.ld", + "include/pros/adi.h", + "include/pros/abstract_motor.hpp", + "include/api.h", + "firmware/v5-hot.ld", + "firmware/libm.a", + "include/pros/device.hpp", + "include/pros/apix.h", + "include/pros/llemu.h", + "include/pros/vision.hpp", + "include/pros/serial.hpp", + "include/pros/llemu.hpp", + "include/pros/colors.hpp", + "include/pros/rotation.hpp", + "include/pros/distance.h", + "include/pros/rtos.hpp", + "include/pros/vision.h", + "include/pros/screen.h", + "include/pros/optical.h", + "firmware/libpros.a", + "include/pros/motor_group.hpp", + "include/pros/screen.hpp" ], "target": "v5", "user_files": [ - "src\\main.c", - "include\\main.hh", + "src/main.cpp", + "src/main.c", "Makefile", - "include\\main.h", + "src/main.cc", + "include/main.h", ".gitignore", - "src\\main.cc", - "include\\main.hpp", - "src\\main.cpp" + "include/main.hh", + "include/main.hpp", + "src\\main.cpp", + "include\\main.h" ], - "version": "4.1.0" + "version": "4.1.1" }, "liblvgl": { "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\liblvgl@8.3.8", @@ -421,7 +424,7 @@ } }, "upload_options": { - "description": "making code ez" + "description": "roboticsisez.com" }, "use_early_access": false } diff --git a/EZ-Template-Example-Project/src/autons.cpp b/EZ-Template-Example-Project/src/autons.cpp index 2ccdb230..548c4b8d 100644 --- a/EZ-Template-Example-Project/src/autons.cpp +++ b/EZ-Template-Example-Project/src/autons.cpp @@ -8,26 +8,44 @@ // These are out of 127 const int DRIVE_SPEED = 110; const int TURN_SPEED = 90; -const int SWING_SPEED = 90; +const int SWING_SPEED = 110; /// // Constants /// void default_constants() { - chassis.pid_heading_constants_set(11, 0, 20); - chassis.pid_drive_constants_set(20, 0, 100); - chassis.pid_turn_constants_set(3, 0.05, 20, 15); - chassis.pid_swing_constants_set(6, 0, 65); - - chassis.pid_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); - chassis.pid_swing_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); - chassis.pid_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 500_ms); - + // P, I, D, and Start I + chassis.pid_drive_constants_set(20.0, 0.0, 100.0); // Fwd/rev constants, used for odom and non odom motions + chassis.pid_heading_constants_set(11.0, 0.0, 20.0); // Holds the robot straight while going forward without odom + chassis.pid_turn_constants_set(3.0, 0.05, 20.0, 15.0); // Turn in place constants + chassis.pid_swing_constants_set(6.0, 0.0, 65.0); // Swing constants + chassis.pid_odom_angular_constants_set(6.5, 0.0, 52.5); // Angular control for odom motions + chassis.pid_odom_boomerang_constants_set(5.8, 0.0, 32.5); // Angular control for boomerang motions + + // Exit conditions + chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); + chassis.pid_swing_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 250_ms, 3_in, 500_ms, 500_ms); + chassis.pid_odom_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms); + chassis.pid_odom_drive_exit_condition_set(90_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms); chassis.pid_turn_chain_constant_set(3_deg); chassis.pid_swing_chain_constant_set(5_deg); chassis.pid_drive_chain_constant_set(3_in); - chassis.slew_drive_constants_set(7_in, 80); + // Slew constants + chassis.slew_turn_constants_set(3_deg, 70); + chassis.slew_drive_constants_set(3_in, 70); + chassis.slew_swing_constants_set(3_in, 80); + + // The amount that turns are prioritized over driving in odom motions + // - if you have tracking wheels, you can run this higher. 1.0 is the max + chassis.odom_turn_bias_set(0.9); + + chassis.odom_look_ahead_set(7_in); // This is how far ahead in the path the robot looks at + chassis.odom_boomerang_distance_set(16_in); // This sets the maximum distance away from target that the carrot point can be + chassis.odom_boomerang_dlead_set(0.625); // This handles how aggressive the end of boomerang motions are + + chassis.pid_angle_behavior_set(ez::shortest); // Changes the default behavior for turning, this defaults it to the shortest path there } /// @@ -218,6 +236,143 @@ void interfered_example() { chassis.pid_wait(); } +/// +// Odom Drive PID +/// +void odom_drive_example() { + // This works the same as pid_drive_set, but it uses odom instead! + // You can replace pid_drive_set with pid_odom_set and your robot will + // have better error correction. + + chassis.pid_odom_set(24_in, DRIVE_SPEED, true); + chassis.pid_wait(); + + chassis.pid_odom_set(-12_in, DRIVE_SPEED); + chassis.pid_wait(); + + chassis.pid_odom_set(-12_in, DRIVE_SPEED); + chassis.pid_wait(); +} + +/// +// Odom Pure Pursuit +/// +void odom_pure_pursuit_example() { + // Drive to 0, 30 and pass through 6, 10 and 0, 20 on the way, with slew + chassis.pid_odom_set({{{6_in, 10_in}, fwd, DRIVE_SPEED}, + {{0_in, 20_in}, fwd, DRIVE_SPEED}, + {{0_in, 30_in}, fwd, DRIVE_SPEED}}, + true); + chassis.pid_wait(); + + // Drive to 0, 0 backwards + chassis.pid_odom_set({{0_in, 0_in}, rev, DRIVE_SPEED}, + true); + chassis.pid_wait(); +} + +/// +// Odom Pure Pursuit Wait Until +/// +void odom_pure_pursuit_wait_until_example() { + chassis.pid_odom_set({{{0_in, 24_in}, fwd, DRIVE_SPEED}, + {{12_in, 24_in}, fwd, DRIVE_SPEED}, + {{24_in, 24_in}, fwd, DRIVE_SPEED}}, + true); + chassis.pid_wait_until_index(1); // Waits until the robot passes 12, 24 + // Intake.move(127); // Set your intake to start moving once it passes through the second point in the index + chassis.pid_wait(); + // Intake.move(0); // Turn the intake off +} + +/// +// Odom Boomerang +/// +void odom_boomerang_example() { + chassis.pid_odom_set({{0_in, 24_in, 45_deg}, fwd, DRIVE_SPEED}, + true); + chassis.pid_wait(); + + chassis.pid_odom_set({{0_in, 0_in, 0_deg}, rev, DRIVE_SPEED}, + true); + chassis.pid_wait(); +} + +/// +// Odom Boomerang Injected Pure Pursuit +/// +void odom_boomerang_injected_pure_pursuit_example() { + chassis.pid_odom_set({{{0_in, 24_in, 45_deg}, fwd, DRIVE_SPEED}, + {{12_in, 24_in}, fwd, DRIVE_SPEED}, + {{24_in, 24_in}, fwd, DRIVE_SPEED}}, + true); + chassis.pid_wait(); + + chassis.pid_odom_set({{0_in, 0_in, 0_deg}, rev, DRIVE_SPEED}, + true); + chassis.pid_wait(); +} + +/// +// Calculate the offsets of your tracking wheels +/// +void measure_offsets() { + // Number of times to test + int iterations = 10; + + // Our final offsets + double l_offset = 0.0, r_offset = 0.0, b_offset = 0.0, f_offset = 0.0; + + // Reset all trackers if they exist + if (chassis.odom_tracker_left != nullptr) chassis.odom_tracker_left->reset(); + if (chassis.odom_tracker_right != nullptr) chassis.odom_tracker_right->reset(); + if (chassis.odom_tracker_back != nullptr) chassis.odom_tracker_back->reset(); + if (chassis.odom_tracker_front != nullptr) chassis.odom_tracker_front->reset(); + + for (int i = 0; i < iterations; i++) { + // Reset pid targets and get ready for running an auton + chassis.pid_targets_reset(); + chassis.drive_imu_reset(); + chassis.drive_sensor_reset(); + chassis.drive_brake_set(MOTOR_BRAKE_HOLD); + chassis.odom_xyt_set(0_in, 0_in, 0_deg); + double imu_start = chassis.odom_theta_get(); + double target = i % 2 == 0 ? 90 : 270; // Switch the turn target every run from 270 to 90 + + // Turn to target at half power + chassis.pid_turn_set(target, 63, ez::raw); + chassis.pid_wait(); + pros::delay(250); + + // Calculate delta in angle + double t_delta = util::to_rad(fabs(util::wrap_angle(chassis.odom_theta_get() - imu_start))); + + // Calculate delta in sensor values that exist + double l_delta = chassis.odom_tracker_left != nullptr ? chassis.odom_tracker_left->get() : 0.0; + double r_delta = chassis.odom_tracker_right != nullptr ? chassis.odom_tracker_right->get() : 0.0; + double b_delta = chassis.odom_tracker_back != nullptr ? chassis.odom_tracker_back->get() : 0.0; + double f_delta = chassis.odom_tracker_front != nullptr ? chassis.odom_tracker_front->get() : 0.0; + + // Calculate the radius that the robot traveled + l_offset += l_delta / t_delta; + r_offset += r_delta / t_delta; + b_offset += b_delta / t_delta; + f_offset += f_delta / t_delta; + } + + // Average all offsets + l_offset /= iterations; + r_offset /= iterations; + b_offset /= iterations; + f_offset /= iterations; + + // Set new offsets to trackers that exist + if (chassis.odom_tracker_left != nullptr) chassis.odom_tracker_left->distance_to_center_set(l_offset); + if (chassis.odom_tracker_right != nullptr) chassis.odom_tracker_right->distance_to_center_set(r_offset); + if (chassis.odom_tracker_back != nullptr) chassis.odom_tracker_back->distance_to_center_set(b_offset); + if (chassis.odom_tracker_front != nullptr) chassis.odom_tracker_front->distance_to_center_set(f_offset); +} + // . . . // Make your own autonomous functions here! // . . . \ No newline at end of file diff --git a/EZ-Template-Example-Project/src/main.cpp b/EZ-Template-Example-Project/src/main.cpp index 176091e4..ce7bc638 100644 --- a/EZ-Template-Example-Project/src/main.cpp +++ b/EZ-Template-Example-Project/src/main.cpp @@ -13,7 +13,14 @@ ez::Drive chassis( 7, // IMU Port 4.125, // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!) - 343); // Wheel RPM + 343); // Wheel RPM = cartridge * (motor gear / wheel gear) + +// Are you using tracking wheels? Comment out which ones you're using here! +// `2.75` is the wheel diameter +// `4.0` is the distance from the center of the wheel to the center of the robot +// ez::tracking_wheel right_tracker({-'A', -'B'}, 2.75, 4.0); // ADI Encoders +// ez::tracking_wheel left_tracker(1, {'C', 'D'}, 2.75, 4.0); // ADI Encoders plugged into a Smart port +// ez::tracking_wheel horiz_tracker(1, 2.75, 4.0); // Rotation sensors /** * Runs initialization code. This occurs as soon as the program is started. @@ -27,10 +34,15 @@ void initialize() { pros::delay(500); // Stop the user from doing anything while legacy ports configure + // Are you using tracking wheels? Comment out which ones you're using here! + // chassis.odom_tracker_right_set(&right_tracker); + // chassis.odom_tracker_left_set(&left_tracker); + // chassis.odom_tracker_back_set(&horiz_tracker); // Replace `back` to `front` if your tracker is in the front! + // Configure your chassis controls - chassis.opcontrol_curve_buttons_toggle(true); // Enables modifying the controller curve with buttons on the joysticks - chassis.opcontrol_drive_activebrake_set(0); // Sets the active brake kP. We recommend ~2. 0 will disable. - chassis.opcontrol_curve_default_set(0, 0); // Defaults for curve. If using tank, only the first parameter is used. (Comment this line out if you have an SD card!) + chassis.opcontrol_curve_buttons_toggle(true); // Enables modifying the controller curve with buttons on the joysticks + chassis.opcontrol_drive_activebrake_set(0.0); // Sets the active brake kP. We recommend ~2. 0 will disable. + chassis.opcontrol_curve_default_set(0.0, 0.0); // Defaults for curve. If using tank, only the first parameter is used. (Comment this line out if you have an SD card!) // Set the drive to your own constants from autons.cpp! default_constants(); @@ -41,20 +53,26 @@ void initialize() { // Autonomous Selector using LLEMU ez::as::auton_selector.autons_add({ - Auton("Example Drive\n\nDrive forward and come back.", drive_example), - Auton("Example Turn\n\nTurn 3 times.", turn_example), - Auton("Drive and Turn\n\nDrive forward, turn, come back. ", drive_and_turn), - Auton("Drive and Turn\n\nSlow down during drive.", wait_until_change_speed), - Auton("Swing Example\n\nSwing in an 'S' curve", swing_example), - Auton("Motion Chaining\n\nDrive forward, turn, and come back, but blend everything together :D", motion_chaining), - Auton("Combine all 3 movements", combining_movements), - Auton("Interference\n\nAfter driving forward, robot performs differently if interfered or not.", interfered_example), + {"Drive\n\nDrive forward and come back", drive_example}, + {"Turn\n\nTurn 3 times.", turn_example}, + {"Drive and Turn\n\nDrive forward, turn, come back", drive_and_turn}, + {"Drive and Turn\n\nSlow down during drive", wait_until_change_speed}, + {"Swing Turn\n\nSwing in an 'S' curve", swing_example}, + {"Motion Chaining\n\nDrive forward, turn, and come back, but blend everything together :D", motion_chaining}, + {"Combine all 3 movements", combining_movements}, + {"Interference\n\nAfter driving forward, robot performs differently if interfered or not", interfered_example}, + {"Simple Odom\n\nThis is the same as the drive example, but it uses odom instead!", odom_drive_example}, + {"Pure Pursuit\n\nGo to (0, 30) and pass through (6, 10) on the way. Come back to (0, 0)", odom_pure_pursuit_example}, + {"Pure Pursuit Wait Until\n\nGo to (24, 24) but start running an intake once the robot passes (12, 24)", odom_pure_pursuit_wait_until_example}, + {"Boomerang\n\nGo to (0, 24, 45) then come back to (0, 0, 0)", odom_boomerang_example}, + {"Boomerang Pure Pursuit\n\nGo to (0, 24, 45) on the way to (24, 24) then come back to (0, 0, 0)", odom_boomerang_injected_pure_pursuit_example}, + {"Measure Offsets\n\nThis will turn the robot a bunch of times and calculate your offsets for your tracking wheels.", odom_boomerang_injected_pure_pursuit_example}, }); // Initialize chassis and auton selector chassis.initialize(); ez::as::initialize(); - master.rumble("."); + master.rumble(chassis.drive_imu_calibrated() ? "." : "---"); } /** @@ -94,11 +112,115 @@ void autonomous() { chassis.pid_targets_reset(); // Resets PID targets to 0 chassis.drive_imu_reset(); // Reset gyro position to 0 chassis.drive_sensor_reset(); // Reset drive sensors to 0 + chassis.odom_xyt_set(0_in, 0_in, 0_deg); // Set the current position, you can start at a specific position with this chassis.drive_brake_set(MOTOR_BRAKE_HOLD); // Set motors to hold. This helps autonomous consistency + /* + Odometry and Pure Pursuit are not magic + + It is possible to get perfectly consistent results without tracking wheels, + but it is also possible to have extremely inconsistent results without tracking wheels. + When you don't use tracking wheels, you need to: + - avoid wheel slip + - avoid wheelies + - avoid throwing momentum around (super harsh turns, like in the example below) + You can do cool curved motions, but you have to give your robot the best chance + to be consistent + */ + ez::as::auton_selector.selected_auton_call(); // Calls selected auton from autonomous selector } +/** + * Simplifies printing tracker values to the brain screen + */ +void screen_print_tracker(ez::tracking_wheel *tracker, std::string name, int line) { + std::string tracker_value = "", tracker_width = ""; + // Check if the tracker exists + if (tracker != nullptr) { + tracker_value = name + " tracker: " + util::to_string_with_precision(tracker->get()); // Make text for the tracker value + tracker_width = " width: " + util::to_string_with_precision(tracker->distance_to_center_get()); // Make text for the distance to center + } + ez::screen_print(tracker_value + tracker_width, line); // Print final tracker text +} + +/** + * Ez screen task + * Adding new pages here will let you view them during user control or autonomous + * and will help you debug problems you're having + */ +void ez_screen_task() { + while (true) { + // Only run this when not connected to a competition switch + if (!pros::competition::is_connected()) { + // Blank page for odom debugging + if (chassis.odom_enabled() && !chassis.pid_tuner_enabled()) { + // If we're on the first blank page... + if (ez::as::page_blank_is_on(0)) { + // Display X, Y, and Theta + ez::screen_print("x: " + util::to_string_with_precision(chassis.odom_x_get()) + + "\ny: " + util::to_string_with_precision(chassis.odom_y_get()) + + "\na: " + util::to_string_with_precision(chassis.odom_theta_get()), + 1); // Don't override the top Page line + + // Display all trackers that are being used + screen_print_tracker(chassis.odom_tracker_left, "l", 4); + screen_print_tracker(chassis.odom_tracker_right, "r", 5); + screen_print_tracker(chassis.odom_tracker_back, "b", 6); + screen_print_tracker(chassis.odom_tracker_front, "f", 7); + } + } + } + + // Remove all blank pages when connected to a comp switch + else { + if (ez::as::page_blank_amount() > 0) + ez::as::page_blank_remove_all(); + } + + pros::delay(ez::util::DELAY_TIME); + } +} +pros::Task ezScreenTask(ez_screen_task); + +/** + * Gives you some extras to run in your opcontrol: + * - run your autonomous routine in opcontrol by pressing DOWN and B + * - to prevent this from accidentally happening at a competition, this + * is only enabled when you're not connected to competition control. + * - gives you a GUI to change your PID values live by pressing X + */ +void ez_template_extras() { + // Only run this when not connected to a competition switch + if (!pros::competition::is_connected()) { + // PID Tuner + // - after you find values that you're happy with, you'll have to set them in auton.cpp + + // Enable / Disable PID Tuner + // When enabled: + // * use A and Y to increment / decrement the constants + // * use the arrow keys to navigate the constants + if (master.get_digital_new_press(DIGITAL_X)) + chassis.pid_tuner_toggle(); + + // Trigger the selected autonomous routine + if (master.get_digital(DIGITAL_B) && master.get_digital(DIGITAL_DOWN)) { + pros::motor_brake_mode_e_t preference = chassis.drive_brake_get(); + autonomous(); + chassis.drive_brake_set(preference); + } + + // Allow PID Tuner to iterate + chassis.pid_tuner_iterate(); + } + + // Disable PID Tuner when connected to a comp switch + else { + if (chassis.pid_tuner_enabled()) + chassis.pid_tuner_disable(); + } +} + /** * Runs the operator control code. This function will be started in its own task * with the default priority and stack size whenever the robot is enabled via @@ -114,29 +236,11 @@ void autonomous() { */ void opcontrol() { // This is preference to what you like to drive on - pros::motor_brake_mode_e_t driver_preference_brake = MOTOR_BRAKE_COAST; - - chassis.drive_brake_set(driver_preference_brake); + chassis.drive_brake_set(MOTOR_BRAKE_COAST); while (true) { - // PID Tuner - // After you find values that you're happy with, you'll have to set them in auton.cpp - if (!pros::competition::is_connected()) { - // Enable / Disable PID Tuner - // When enabled: - // * use A and Y to increment / decrement the constants - // * use the arrow keys to navigate the constants - if (master.get_digital_new_press(DIGITAL_X)) - chassis.pid_tuner_toggle(); - - // Trigger the selected autonomous routine - if (master.get_digital(DIGITAL_B) && master.get_digital(DIGITAL_DOWN)) { - autonomous(); - chassis.drive_brake_set(driver_preference_brake); - } - - chassis.pid_tuner_iterate(); // Allow PID Tuner to iterate - } + // Gives you some extras to make EZ-Template ezier + ez_template_extras(); chassis.opcontrol_tank(); // Tank control // chassis.opcontrol_arcade_standard(ez::SPLIT); // Standard split arcade @@ -150,4 +254,4 @@ void opcontrol() { pros::delay(ez::util::DELAY_TIME); // This is used for timer calculations! Keep this ez::util::DELAY_TIME } -} \ No newline at end of file +} diff --git a/EZ-Template@3.1.0.zip b/EZ-Template@3.1.0.zip deleted file mode 100644 index 8185d1fb..00000000 Binary files a/EZ-Template@3.1.0.zip and /dev/null differ diff --git a/EZ-Template@3.2.0.zip b/EZ-Template@3.2.0.zip new file mode 100644 index 00000000..adaecd8f Binary files /dev/null and b/EZ-Template@3.2.0.zip differ diff --git a/Makefile b/Makefile index aaa9bd78..9a55e939 100644 --- a/Makefile +++ b/Makefile @@ -27,7 +27,7 @@ EXCLUDE_COLD_LIBRARIES:= IS_LIBRARY:=1 # TODO: CHANGE THIS! LIBNAME:=EZ-Template -VERSION:=3.1.0 +VERSION:=3.2.0 # EXCLUDE_SRC_FROM_LIB= $(SRCDIR)/unpublishedfile.c # this line excludes opcontrol.c and similar files EXCLUDE_SRC_FROM_LIB+=$(foreach file, $(SRCDIR)/autons $(SRCDIR)/main,$(foreach cext,$(CEXTS),$(file).$(cext)) $(foreach cxxext,$(CXXEXTS),$(file).$(cxxext))) diff --git a/README.md b/README.md index cee22765..6ec8c6c1 100644 --- a/README.md +++ b/README.md @@ -2,41 +2,63 @@ ![](https://github.com/EZ-Robotics/EZ-Template/workflows/Build/badge.svg) [![License: MPL 2.0](https://img.shields.io/badge/License-MPL%202.0-brightgreen.svg)](https://opensource.org/licenses/MPL-2.0) -**EZ-Template is a simple plug-and-play PROS template that handles drive base functions, autonomous selector, input curves, and active brake with PTO support.** +⚡️ **coding made ez** ⚡️ -## Features -- Simple to setup -- PID for driving, turning, swing turns, and arcs -- Easy to use PID tuner -- Speed ramp-up for driving -- Asynchronous PID with blocking functions until settled and until a specific position has come -- Joystick input curves -- Live adjustment of input curves -- Basic autonomous selector -- SD card saving of autonomous selector and joystick curves -- "Tug of war" detection for autonomous -- PID exit conditions for when drive motors overheat -- Tank drive, single stick arcade, and dual stick arcade -- Loading animation during IMU calibration -- 3 wire encoder and rotation sensor support -- Add / remove motors from the drive dynamically to allow for PTO use -- Exposed PID class for use with your other subsystems -- Exposed slew class for gradual speed ramp-up for your other subsystems +💅 **docs and tutorials** 💅 + +💥 **pid, odometry, pure pursuit, boomerang** 💥 + +📚 **focus on consistency** 📚 + +🧐 **out of the box documentation** 🧐 + +🔌 **plug and play example project** 🔌 + +[See a complete playlist of EZ-Template autons here!](https://www.youtube.com/playlist?list=PLyZbi14KopZK70GTSD5NpygoAcM2_ls7T) ## [Discord Server](https://discord.gg/EHjXBcK2Gy) -Need extra assistance using EZ-Template? Feel free to join our [Discord Server](https://discord.gg/EHjXBcK2Gy)! +Need extra assistance using EZ-Template? Feel free to join our [Discord Server](https://discord.gg/EHjXBcK2Gy)! + +## Features +EZ-Template is built with high attention to the user experience. + +* Built with 💜 and [PROS](https://pros.cs.purdue.edu/) + * Powerful open source Development platform for VEX V5 + * Customize and extend with other community PROS libraries +* 🔌 Example project is plug-and-play + * Simple to setup + * Get up and running in minutes +* 👀 Simple to use API + * PID for driving, turning, swing turns, and arcs + * Odometry with Pure Pursuit and Boomerang + * Asynchronous PID with blocking functions + * "Tug of war" detection + * Overheat detection and exiting + * Live PID tuning + * Tracking wheel support +* 📺 Autonomous selector + * Easy to add autons + * SD card saving +* 🎮 Joystick control + * Tank drive, single stick arcade, and dual stick arcade + * Joystick input curves + * Adjust joystick curves live through the controller +* PID for your own subsystems +* Slew for your own subsystems + -## [Installation](https://ez-robotics.github.io/EZ-Template/tutorials/installation) -Installation tutorials can be found [here](https://ez-robotics.github.io/EZ-Template/tutorials/installation). +## Design Principles +* **Quick to get going.** EZ-Template should make it easy to learn and use. Anything is achievable by users, even if it takes them more code and more time to write. +* **Intuitive.** Users will not feel overwhelmed when looking at an EZ-Template project or adding new features. It should look intuitive and easy to build on top of. +* **Sensible Defaults.** Common and popular performance optimizations and configurations will be done for users, but only with the option to override them. -## [Upgrading](https://ez-robotics.github.io/EZ-Template/tutorials/upgrading) -Upgrading tutorials can be found [here](https://ez-robotics.github.io/EZ-Template/tutorials/upgrading). +We believe that, as developers, knowing how a library works helps us become better at using it. We're dedicating effort to creating tutorials and documentation with the hope that reading it will gain the user a deeper understanding of the tool, and become even more proficient in using it. -## [Tutorials](https://ez-robotics.github.io/EZ-Template/category/tutorials) -Tutorials on how to use and install EZ-Template can be found [here](https://ez-robotics.github.io/EZ-Template/category/tutorials). +## [Support me on Patreon!](https://www.patreon.com/roboticsisez) +Supporting me on Patreon will help guarantee that EZ-Template continues to get maintained and allow me to develop products for teams to use. [Click here](https://www.patreon.com/roboticsisez) to see my Patreon! -## [Docs](https://ez-robotics.github.io/EZ-Template/category/docs) -Documentation on how to use EZ-Template functions can be found [here](https://ez-robotics.github.io/EZ-Template/category/docs). +## [Download and Installation](https://ez-robotics.github.io/EZ-Template/tutorials/installation) +Learn how to install and setup EZ-Template [here](https://ez-robotics.github.io/EZ-Template/tutorials/installation)! ## [License](https://opensource.org/licenses/MPL-2.0) -This project is licensed under the Mozilla Public License, version 2.0 - see the [LICENSE](https://opensource.org/licenses/MPL-2.0) file for the full license. +This project is licensed under the Mozilla Public License, version 2.0 - see the [LICENSE](https://opensource.org/licenses/MPL-2.0) file for the full license. \ No newline at end of file diff --git a/firmware/libpros.a b/firmware/libpros.a index 43f4e941..36de8fae 100644 Binary files a/firmware/libpros.a and b/firmware/libpros.a differ diff --git a/include/EZ-Template/PID.hpp b/include/EZ-Template/PID.hpp index 29605362..66d78950 100644 --- a/include/EZ-Template/PID.hpp +++ b/include/EZ-Template/PID.hpp @@ -73,23 +73,23 @@ class PID { * Set's constants for exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. + * sets small_exit_time, timer for to exit within smalL_error * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. + * sets smalL_error, timer will start when error is within this * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. + * sets big_exit_time, timer for to exit within big_error * \param p_big_error - * Sets big_error. Timer will start when error is within this. + * sets big_error, timer will start when error is within this * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. + * sets velocity_exit_time, timer will start when velocity is 0 */ void exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time = 0, double p_big_error = 0, int p_velocity_exit_time = 0, int p_mA_timeout = 0); /** - * Sets target. + * Sets PID target. * * \param target - * Target for PID. + * new target for PID */ void target_set(double input); @@ -97,18 +97,19 @@ class PID { * Computes PID. * * \param current - * Current sensor value. + * current sensor value */ double compute(double current); /** - * Computes PID, but you set the error yourself. This function ignores target. - * Current is only used here for calculative derivative. + * Computes PID, but you compute the error yourself. + * + * Current is only used here for calculative derivative to solve derivative kick. * * \param err - * Error in PID, you need to calculate this yourself. + * error for the PID, you need to calculate this yourself * \param current - * Current sensor value. + * current sensor value */ double compute_error(double err, double current); @@ -122,18 +123,23 @@ class PID { */ Constants constants_get(); + /** + * Returns true if PID constants are set, returns false if they're all 0. + */ + bool constants_set_check(); + /** * Resets all variables to 0. This does not reset constants. */ void variables_reset(); /** - * Constants + * Constants. */ Constants constants; /** - * Exit + * Exit. */ exit_condition_ exit; @@ -141,7 +147,7 @@ class PID { * Updates a secondary sensor for velocity exiting. Ideal use is IMU during normal drive motions. * * \param secondary_sensor - * double for a secondary sensor. + * secondary sensor value */ void velocity_sensor_secondary_set(double secondary_sensor); @@ -154,7 +160,7 @@ class PID { * Boolean for if the secondary sensor will be updated or not. True uses this sensor, false does not. * * \param toggle - * True uses this sensor, false does not. + * true uses this sensor, false does not */ void velocity_sensor_secondary_toggle_set(bool toggle); @@ -164,7 +170,7 @@ class PID { bool velocity_sensor_secondary_toggle_get(); /** - * Sets the threshold that the main sensor will return 0 velocity within + * Sets the threshold that the main sensor will return 0 velocity within. * * \param zero * a small double @@ -172,12 +178,12 @@ class PID { void velocity_sensor_main_exit_set(double zero); /** - * Returns the threshold that the main sensor will return 0 velocity within + * Returns the threshold that the main sensor will return 0 velocity within. */ double velocity_sensor_main_exit_get(); /** - * Sets the threshold that the secondary sensor will return 0 velocity within + * Sets the threshold that the secondary sensor will return 0 velocity within. * * \param zero * a small double @@ -185,7 +191,7 @@ class PID { void velocity_sensor_secondary_exit_set(double zero); /** - * Returns the threshold that the secondary sensor will return 0 velocity within + * Returns the threshold that the secondary sensor will return 0 velocity within. */ double velocity_sensor_secondary_exit_get(); @@ -193,7 +199,7 @@ class PID { * Iterative exit condition for PID. * * \param print = false - * if true, prints when complete. + * if true, prints when complete */ ez::exit_output exit_condition(bool print = false); @@ -201,9 +207,9 @@ class PID { * Iterative exit condition for PID. * * \param sensor - * A pros motor on your mechanism. + * a pros motor on your mechanism * \param print = false - * if true, prints when complete. + * if true, prints when complete */ ez::exit_output exit_condition(pros::Motor sensor, bool print = false); @@ -211,17 +217,27 @@ class PID { * Iterative exit condition for PID. * * \param sensor - * Pros motors on your mechanism. + * pros motors on your mechanism * \param print = false - * if true, prints when complete. + * if true, prints when complete */ ez::exit_output exit_condition(std::vector sensor, bool print = false); + /** + * Iterative exit condition for PID. + * + * \param sensor + * pros motor group on your mechanism + * \param print = false + * if true, prints when complete + */ + ez::exit_output exit_condition(pros::MotorGroup sensor, bool print = false); + /** * Sets the name of the PID that prints during exit conditions. * * \param name - * a string that is the name you want to print + * the name of the mechanism for printing */ void name_set(std::string name); @@ -231,7 +247,9 @@ class PID { std::string name_get(); /** - * Enables / disables i resetting when sgn of error changes. True resets, false doesn't. + * Enables / disables i resetting when sgn of error changes. + * + * True resets, false doesn't. * * \param toggle * true resets, false doesn't @@ -239,7 +257,9 @@ class PID { void i_reset_toggle(bool toggle); /** - * Returns if i will reset when sgn of error changes. True resets, false doesn't. + * Returns if i will reset when sgn of error changes. + * + * True resets, false doesn't. */ bool i_reset_get(); @@ -264,7 +284,7 @@ class PID { private: double velocity_zero_main = 0.05; - double velocity_zero_secondary = 0.1; + double velocity_zero_secondary = 0.075; int i = 0, j = 0, k = 0, l = 0, m = 0; bool is_mA = false; double second_sensor = 0.0; diff --git a/include/EZ-Template/api.hpp b/include/EZ-Template/api.hpp index 63bc6aea..7989cf01 100644 --- a/include/EZ-Template/api.hpp +++ b/include/EZ-Template/api.hpp @@ -13,4 +13,5 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include "EZ-Template/piston.hpp" #include "EZ-Template/sdcard.hpp" #include "EZ-Template/slew.hpp" +#include "EZ-Template/tracking_wheel.hpp" #include "EZ-Template/util.hpp" \ No newline at end of file diff --git a/include/EZ-Template/auton_selector.hpp b/include/EZ-Template/auton_selector.hpp index 10d0c19a..e8f46feb 100644 --- a/include/EZ-Template/auton_selector.hpp +++ b/include/EZ-Template/auton_selector.hpp @@ -17,6 +17,7 @@ class AutonSelector { std::vector Autons; int auton_page_current; int auton_count; + int last_auton_page_current; AutonSelector(); AutonSelector(std::vector autons); void selected_auton_call(); diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index 582792d2..6a4004d0 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/include/EZ-Template/drive/drive.hpp @@ -12,10 +12,12 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include "EZ-Template/PID.hpp" #include "EZ-Template/slew.hpp" +#include "EZ-Template/tracking_wheel.hpp" #include "EZ-Template/util.hpp" #include "okapi/api/units/QAngle.hpp" #include "okapi/api/units/QLength.hpp" #include "okapi/api/units/QTime.hpp" +#include "pros/motor_group.hpp" #include "pros/motors.h" using namespace ez; @@ -24,7 +26,9 @@ namespace ez { class Drive { public: /** - * Joysticks will return 0 when they are within this number. Set with opcontrol_joystick_threshold_set() + * Joysticks will return 0 when they are within this number. + * + * Set with opcontrol_joystick_threshold_set() */ int JOYSTICK_THRESHOLD; @@ -64,37 +68,67 @@ class Drive { pros::Imu imu; /** - * Left tracking wheel. + * Deprecated left tracking wheel. */ pros::adi::Encoder left_tracker; /** - * Right tracking wheel. + * Deprecated right tracking wheel. */ pros::adi::Encoder right_tracker; /** - * Left rotation tracker. + * Deprecated left rotation tracker. */ pros::Rotation left_rotation; /** - * Right rotation tracker. + * Deprecated right rotation tracker. */ pros::Rotation right_rotation; + /** + * Left vertical tracking wheel. + */ + tracking_wheel* odom_tracker_left; + + /** + * Right vertical tracking wheel. + */ + tracking_wheel* odom_tracker_right; + + /** + * Front horizontal tracking wheel. + */ + tracking_wheel* odom_tracker_front; + + /** + * Back horizontal tracking wheel. + */ + tracking_wheel* odom_tracker_back; + /** * PID objects. */ PID headingPID; PID turnPID; - PID forward_drivePID; 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; /** * Slew objects. @@ -109,105 +143,219 @@ class Drive { ez::slew slew_swing; /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for swing movements. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi distance unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_swing_constants_set(okapi::QLength distance, int min_speed); /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for forward swing movements. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi distance unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_swing_constants_forward_set(okapi::QLength distance, int min_speed); /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for backward swing movements. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi distance unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_swing_constants_backward_set(okapi::QLength distance, int min_speed); /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for swing movements. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi angle unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_swing_constants_set(okapi::QAngle distance, int min_speed); /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for swing forward movements. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi angle unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_swing_constants_forward_set(okapi::QAngle distance, int min_speed); /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for swing backward movements. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi angle unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_swing_constants_backward_set(okapi::QAngle distance, int min_speed); /** - * Sets constants for slew for turns. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for turns. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi angle unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_turn_constants_set(okapi::QAngle distance, int min_speed); /** - * Sets constants for slew for driving forward. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for driving forward. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi distance unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_drive_constants_forward_set(okapi::QLength distance, int min_speed); /** - * Sets constants for slew for driving backward. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for driving backward. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi distance unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_drive_constants_backward_set(okapi::QLength distance, int min_speed); /** - * Sets constants for slew for driving. Slew ramps up the speed of the robot until the set distance is traveled. + * Sets constants for slew for driving. + * + * Slew ramps up the speed of the robot until the set distance is traveled. * * \param distance * the distance the robot travels before reaching max speed, an okapi distance unit * \param min_speed - * the starting speed for the movement + * the starting speed for the movement, 0 - 127 */ void slew_drive_constants_set(okapi::QLength distance, int min_speed); + /** + * Sets the default slew for drive forwards and backwards motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_drive_set(bool slew_on); + + /** + * Sets the default slew for drive forward motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_drive_forward_set(bool slew_on); + + /** + * Returns true if slew is enabled for all drive forward movements, false otherwise. + */ + bool slew_drive_forward_get(); + + /** + * Sets the default slew for drive backward motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_drive_backward_set(bool slew_on); + + /** + * Returns true if slew is enabled for all drive backward movements, false otherwise. + */ + bool slew_drive_backward_get(); + + /** + * Sets the default slew for swing forward and backward motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_swing_set(bool slew_on); + + /** + * Sets the default slew for swing forward motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_swing_forward_set(bool slew_on); + + /** + * Returns true if slew is enabled for all swing forward motions, false otherwise. + */ + bool slew_swing_forward_get(); + + /** + * Sets the default slew for swing backward motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_swing_backward_set(bool slew_on); + + /** + * Returns true if slew is enabled for all swing backward motions, false otherwise. + */ + bool slew_swing_backward_get(); + + /** + * Sets the default slew for turn motions, can be overwritten in movement functions. + * + * \param slew_on + * true enables, false disables + */ + void slew_turn_set(bool slew_on); + + /** + * Returns true if slew is enabled for all turn motions, false otherwise. + */ + bool slew_turn_get(); + + /** + * Allows slew to reenable when the new input speed is larger than the current speed during pure pursuits. + * + * \param slew_on + * true enables, false disables + */ + void slew_odom_reenable(bool reenable); + + /** + * Returns if slew will reenable when the new input speed is larger than the current speed during pure pursuits. + */ + bool slew_odom_reenabled(); + /** * Current mode of the drive. */ @@ -215,8 +363,13 @@ class Drive { /** * Sets current mode of drive. + * + * \param p_mode + * the new drive mode + * \param stop_drive + * if the drive will stop when p_mode is DISABLED */ - void drive_mode_set(e_mode p_mode); + void drive_mode_set(e_mode p_mode, bool stop_drive = true); /** * Returns current mode of drive. @@ -237,17 +390,17 @@ class Drive { * Creates a Drive Controller using internal encoders. * * \param left_motor_ports - * Input {1, -2...}. Make ports negative if reversed! + * input {1, -2...}. make ports negative if reversed * \param right_motor_ports - * Input {-3, 4...}. Make ports negative if reversed! + * input {-3, 4...}. make ports negative if reversed * \param imu_port - * Port the IMU is plugged into. + * port the IMU is plugged into * \param wheel_diameter - * Diameter of your drive wheels. Remember 4" is 4.125"! + * diameter of your drive wheels * \param ticks - * Motor cartridge RPM + * motor cartridge RPM * \param ratio - * External gear ratio, wheel gear / motor gear. + * external gear ratio, wheel gear / motor gear */ Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio = 1.0); @@ -255,67 +408,67 @@ class Drive { * Creates a Drive Controller using encoders plugged into the brain. * * \param left_motor_ports - * Input {1, -2...}. Make ports negative if reversed! + * input {1, -2...}. make ports negative if reversed * \param right_motor_ports - * Input {-3, 4...}. Make ports negative if reversed! + * input {-3, 4...}. make ports negative if reversed * \param imu_port - * Port the IMU is plugged into. + * port the IMU is plugged into * \param wheel_diameter - * Diameter of your sensored wheels. Remember 4" is 4.125"! + * diameter of your sensored wheel * \param ticks - * Ticks per revolution of your encoder. + * ticks per revolution of your encoder * \param ratio - * External gear ratio, wheel gear / sensor gear. + * external gear ratio, wheel gear / sensor gear * \param left_tracker_ports - * Input {1, 2}. Make ports negative if reversed! + * input {1, 2}. make ports negative if reversed * \param right_tracker_ports - * Input {3, 4}. Make ports negative if reversed! + * input {3, 4}. make ports negative if reversed */ - Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector left_tracker_ports, std::vector right_tracker_ports); + Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector left_tracker_ports, std::vector right_tracker_ports) __attribute__((deprecated("Use the integrated encoder constructor with odom_tracker_left_set() and odom_tracker_right_set() instead!"))); /** * Creates a Drive Controller using encoders plugged into a 3 wire expander. * * \param left_motor_ports - * Input {1, -2...}. Make ports negative if reversed! + * input {1, -2...}. make ports negative if reversed * \param right_motor_ports - * Input {-3, 4...}. Make ports negative if reversed! + * input {-3, 4...}. make ports negative if reversed * \param imu_port - * Port the IMU is plugged into. + * port the IMU is plugged into * \param wheel_diameter - * Diameter of your sensored wheels. Remember 4" is 4.125"! + * diameter of your sensored wheel * \param ticks - * Ticks per revolution of your encoder. + * ticks per revolution of your encoder * \param ratio - * External gear ratio, wheel gear / sensor gear. + * external gear ratio, wheel gear / sensor gear * \param left_tracker_ports - * Input {1, 2}. Make ports negative if reversed! + * input {1, 2}. make ports negative if reversed * \param right_tracker_ports - * Input {3, 4}. Make ports negative if reversed! + * input {3, 4}. make ports negative if reversed * \param expander_smart_port - * Port the expander is plugged into. + * port the expander is plugged into */ - Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector left_tracker_ports, std::vector right_tracker_ports, int expander_smart_port); + Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector left_tracker_ports, std::vector 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!"))); /** * Creates a Drive Controller using rotation sensors. * * \param left_motor_ports - * Input {1, -2...}. Make ports negative if reversed! + * input {1, -2...}. make ports negative if reversed * \param right_motor_ports - * Input {-3, 4...}. Make ports negative if reversed! + * input {-3, 4...}. make ports negative if reversed * \param imu_port - * Port the IMU is plugged into. + * port the IMU is plugged into * \param wheel_diameter - * Diameter of your sensored wheels. Remember 4" is 4.125"! + * diameter of your sensored wheel * \param ratio - * External gear ratio, wheel gear / sensor gear. + * external gear ratio, wheel gear / sensor gear * \param left_tracker_port - * Make ports negative if reversed! + * make ports negative if reversed * \param right_tracker_port - * Make ports negative if reversed! + * make ports negative if reversed */ - Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ratio, int left_rotation_port, int right_rotation_port); + Drive(std::vector left_motor_ports, std::vector 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!"))); /** * Sets drive defaults. @@ -324,474 +477,1943 @@ class Drive { ///// // - // User Control + // General Odometry // ///// /** - * Sets the chassis to controller joysticks using tank control. Run is usercontrol. - * This passes the controller through the curve functions, but is disabled by default. Use opcontrol_curve_buttons_toggle() to enable it. + * Tasks for tracking. */ - void opcontrol_tank(); + void ez_tracking_task(); /** - * Sets the chassis to controller joysticks using standard arcade control. Run is usercontrol. - * This passes the controller through the curve functions, but is disabled by default. Use opcontrol_curve_buttons_toggle() to enable it. + * Enables / disables tracking. * - * \param stick_type - * ez::SINGLE or ez::SPLIT control + * \param input + * true enables tracking, false disables tracking */ - void opcontrol_arcade_standard(e_type stick_type); + void odom_enable(bool input); /** - * Sets the chassis to controller joysticks using flipped arcade control. Run is usercontrol. - * This passes the controller through the curve functions, but is disabled by default. Use opcontrol_curve_buttons_toggle() to enable it. + * Returns whether the bot is tracking with odometry. * - * \param stick_type - * ez::SINGLE or ez::SPLIT control + * True means tracking is enabled, false means tracking is disabled. */ - void opcontrol_arcade_flipped(e_type stick_type); + bool odom_enabled(); /** - * Initializes left and right curves with the SD card, recommended to run in initialize(). + * Sets the width of the drive. + * + * This is used for tracking. + * + * \param input + * a unit in inches, from center of the wheel to center of the wheel */ - void opcontrol_curve_sd_initialize(); + void drive_width_set(double input); /** - * Sets the default joystick curves. + * Sets the width of the drive. * - * \param left - * Left default curve. - * \param right - * Right default curve. + * This is used for tracking. + * + * \param input + * an okapi unit, from center of the wheel to center of the wheel */ - void opcontrol_curve_default_set(double left, double right = 0); + void drive_width_set(okapi::QLength p_input); /** - * Gets the default joystick curves, in {left, right} + * Returns the width of the drive */ - std::vector opcontrol_curve_default_get(); + double drive_width_get(); /** - * Runs a P loop on the drive when the joysticks are released. + * Sets the current X coordinate of the robot. * - * \param kp - * Constant for the p loop. + * \param x + * new x coordinate in inches */ - void opcontrol_drive_activebrake_set(double kp); + void odom_x_set(double x); /** - * Returns kP for active brake. + * Sets the current X coordinate of the robot. + * + * \param p_x + * new x coordinate as an okapi unit */ - double opcontrol_drive_activebrake_get(); + void odom_x_set(okapi::QLength p_x); /** - * Enables/disables modifying the joystick input curves with the controller. True enables, false disables. - * - * \param input - * bool input + * Returns the current X coordinate of the robot in inches. */ - void opcontrol_curve_buttons_toggle(bool toggle); + double odom_x_get(); /** - * Gets the current state of the toggle. Enables/disables modifying the joystick input curves with the controller. True enables, false disables. + * Sets the current Y coordinate of the robot. + * + * \param y + * new y coordinate in inches */ - bool opcontrol_curve_buttons_toggle_get(); + void odom_y_set(double y); /** - * Sets buttons for modifying the left joystick curve. + * Sets the current Y coordinate of the robot. * - * \param decrease - * a pros button enumerator - * \param increase - * a pros button enumerator + * \param p_y + * new y coordinate as an okapi unit */ - void opcontrol_curve_buttons_left_set(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase); + void odom_y_set(okapi::QLength p_y); /** - * Returns a vector of pros controller buttons user for the left joystick curve, in {decrease, increase} + * Returns the current Y coordinate of the robot in inches. */ - std::vector opcontrol_curve_buttons_left_get(); + double odom_y_get(); /** - * Sets buttons for modifying the right joystick curve. + * Sets the current angle of the robot. * - * \param decrease - * a pros button enumerator - * \param increase - * a pros button enumerator + * \param a + * new angle in degrees */ - void opcontrol_curve_buttons_right_set(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase); + void odom_theta_set(double a); /** - * Returns a vector of pros controller buttons user for the right joystick curve, in {decrease, increase} + * Sets the current Theta of the robot. + * + * \param p_a + * new angle as an okapi unit */ - std::vector opcontrol_curve_buttons_right_get(); + void odom_theta_set(okapi::QAngle p_a); + + /** + * Returns the current Theta of the robot in degrees. + */ + double odom_theta_get(); /** - * Outputs a curve from 5225A In the Zone. This gives more control over the robot at lower speeds. https://www.desmos.com/calculator/rcfjjg83zx + * Sets the current pose of the robot. * - * \param x - * joystick input + * \param itarget + * {x, y, t} units in inches and degrees */ - double opcontrol_curve_left(double x); + void odom_pose_set(pose itarget); /** - * Outputs a curve from 5225A In the Zone. This gives more control over the robot at lower speeds. https://www.desmos.com/calculator/rcfjjg83zx + * Sets the current pose of the robot. * - * \param x - * joystick input + * \param itarget + * {x, y, t} as an okapi unit */ - double opcontrol_curve_right(double x); + void odom_pose_set(united_pose itarget); /** - * Sets a new threshold for the joystick. The joysticks wil not return a value if they are within this. + * Sets the current X and Y coordinate for the robot. * - * \param threshold - * new threshold + * \param x + * new x value, in inches + * \param y + * new y value, in inches */ - void opcontrol_joystick_threshold_set(int threshold); + void odom_xy_set(double x, double y); /** - * Gets a new threshold for the joystick. The joysticks wil not return a value if they are within this. + * Sets the current X and Y coordinate for the robot. + * + * \param p_x + * new x value, okapi unit + * \param p_y + * new y value, okapi unit */ - int opcontrol_joystick_threshold_get(); + void odom_xy_set(okapi::QLength p_x, okapi::QLength p_y); /** - * Resets drive sensors at the start of opcontrol. + * Sets the current X, Y, and Theta values for the robot. + * + * \param x + * new x value, in inches + * \param y + * new y value, in inches + * \param t + * new theta value, in degrees */ - void opcontrol_drive_sensors_reset(); + void odom_xyt_set(double x, double y, double t); /** - * Sets minimum value distance constants. + * Sets the current X, Y, and Theta values for the robot. * - * \param l_stick - * input for left joystick - * \param r_stick - * input for right joystick + * \param p_x + * new x value, okapi unit + * \param p_y + * new y value, okapi unit + * \param p_t + * new theta value, okapi unit */ - void opcontrol_joystick_threshold_iterate(int l_stick, int r_stick); + void odom_xyt_set(okapi::QLength p_x, okapi::QLength p_y, okapi::QAngle p_t); - ///// - // - // PTO - // - ///// + /** + * Returns the current pose of the robot. + */ + pose odom_pose_get(); /** - * Checks if the motor is currently in pto_list. Returns true if it's already in pto_list. - * - * \param check_if_pto - * motor to check. + * Resets xyt to 0. */ - bool pto_check(pros::Motor check_if_pto); + void odom_reset(); /** - * Adds motors to the pto list, removing them from the drive. + * Flips the X axis. * - * \param pto_list - * list of motors to remove from the drive. + * \param flip + * true means left is positive x, false means right is positive x */ - void pto_add(std::vector pto_list); + void odom_x_flip(bool flip = true); /** - * Removes motors from the pto list, adding them to the drive. You cannot use the first index in a pto. + * Checks if X axis is flipped. * - * \param pto_list - * list of motors to add to the drive. + * True means left is positive X, false means right is positive X. */ - void pto_remove(std::vector pto_list); + bool odom_x_direction_get(); /** - * Adds/removes motors from drive. You cannot use the first index in a pto. + * Flips the Y axis. * - * \param pto_list - * list of motors to add/remove from the drive. - * \param toggle - * if true, adds to list. if false, removes from list. + * \param flip + * true means down is positive y, false means up is positive y */ - void pto_toggle(std::vector pto_list, bool toggle); + void odom_y_flip(bool flip = true); - ///// - // - // PROS Wrapers - // - ///// + /** + * Checks if Y axis is flipped. + * + * True means down is positive Y, false means up is positive Y. + */ + bool odom_y_direction_get(); /** - * Sets the chassis to voltage. Disables PID when called. + * Flips the rotation axis. * - * \param left - * voltage for left side, -127 to 127 - * \param right - * voltage for right side, -127 to 127 + * \param flip + * true means counterclockwise is positive, false means clockwise is positive */ - void drive_set(int left, int right); + void odom_theta_flip(bool flip = true); /** - * Gets the chassis to voltage, -127 to 127. Returns {left, right} + * Checks if the rotation axis is flipped. + * + * True means counterclockwise is positive, false means clockwise is positive. */ - std::vector drive_get(); + bool odom_theta_direction_get(); /** - * Changes the way the drive behaves when it is not under active user control + * Sets a new dlead. * - * \param brake_type - * the 'brake mode' of the motor e.g. 'pros::E_MOTOR_BRAKE_COAST' 'pros::E_MOTOR_BRAKE_BRAKE' 'pros::E_MOTOR_BRAKE_HOLD' + * Dlead is a proportional value of how much to make the robot curve during boomerang motions. + * + * \param input + * a value between 0 and 1 */ - void drive_brake_set(pros::motor_brake_mode_e_t brake_type); + void odom_boomerang_dlead_set(double input); /** - * Returns the brake mode of the drive in pros_brake_mode_e_t_ + * Returns the current dlead. */ - pros::motor_brake_mode_e_t drive_brake_get(); + double odom_boomerang_dlead_get(); /** - * Sets the limit for the current on the drive. + * Sets how far away the carrot point can be from the target point. * - * \param mA - * input in milliamps + * \param distance + * distance in inches */ - void drive_current_limit_set(int mA); + void odom_boomerang_distance_set(double distance); /** - * Gets the limit for the current on the drive. + * Sets how far away the carrot point can be from the target point. + * + * \param distance + * distance as an okapi unit */ - int drive_current_limit_get(); + void odom_boomerang_distance_set(okapi::QLength p_distance); /** - * Toggles set drive in autonomous. True enables, false disables. + * Returns how far away the carrot point can be from target. */ - void pid_drive_toggle(bool toggle); + double odom_boomerang_distance_get(); /** - * Gets the current state of the toggle. This toggles set drive in autonomous. True enables, false disables. + * A proportion of how prioritized turning is during odometry motions. + * + * Turning is prioritized so the robot "applies brakes" while turning. Lower number means more braking. + * + * \param bias + * a number between 0 and 1 */ - bool pid_drive_toggle_get(); + void odom_turn_bias_set(double bias); /** - * Toggles printing in autonomous. True enables, false disables. + * Returns the proportion of how prioritized turning is during odometry motions. */ - void pid_print_toggle(bool toggle); + double odom_turn_bias_get(); /** - * Gets the current state of the toggle. This toggles printing in autonomous. True enables, false disables. + * Sets the spacing between points when points get injected into the path. + * + * \param spacing + * a small number in inches */ - bool pid_print_toggle_get(); + void odom_path_spacing_set(double spacing); - ///// - // - // Telemetry - // - ///// + /** + * Sets the spacing between points when points get injected into the path. + * + * \param spacing + * a small number in okapi units + */ + void odom_path_spacing_set(okapi::QLength p_spacing); /** - * The position of the right motor. + * Returns the spacing between points when points get injected into the path. */ - double drive_sensor_right(); + double odom_path_spacing_get(); /** - * The position of the right motor. + * Sets the constants for smoothing out a path. + * + * Path smoothing based on https://medium.com/@jaems33/understanding-robot-motion-path-smoothing-5970c8363bc4 + * + * \param weight_smooth + * how much weight to update the data + * \param weight_data + * how much weight to smooth the coordinates + * \param tolerance + * how much change per iteration is necessary to keep iterating */ - int drive_sensor_right_raw(); + void odom_path_smooth_constants_set(double weight_smooth, double weight_data, double tolerance); /** - * The velocity of the right motor. + * Returns the constants for smoothing out a path. + * + * In order of: + * - weight_smooth + * - weight_data + * - tolerance */ - int drive_velocity_right(); + std::vector odom_path_smooth_constants_get(); /** - * The watts of the right motor. + * Prints the current path the robot is following. */ - double drive_mA_right(); + void odom_path_print(); /** - * Return TRUE when the motor is over current. + * Sets how far away the robot looks in the path during pure pursuits. + * + * \param distance + * how long the "carrot on a stick" is, in inches */ - bool drive_current_right_over(); + void odom_look_ahead_set(double distance); /** - * The position of the left motor. + * Sets how far away the robot looks in the path during pure pursuits. + * + * \param distance + * how long the "carrot on a stick" is, in okapi units */ - double drive_sensor_left(); + void odom_look_ahead_set(okapi::QLength p_distance); /** - * The position of the left motor. + * Returns how far away the robot looks in the path during pure pursuits. */ - int drive_sensor_left_raw(); + double odom_look_ahead_get(); /** - * The velocity of the left motor. + * Sets the parallel left tracking wheel for odometry. + * + * \param input + * an ez::tracking_wheel */ - int drive_velocity_left(); + void odom_tracker_left_set(tracking_wheel* input); /** - * The watts of the left motor. + * Sets the parallel right tracking wheel for odometry. + * + * \param input + * an ez::tracking_wheel */ - double drive_mA_left(); + void odom_tracker_right_set(tracking_wheel* input); /** - * Return TRUE when the motor is over current. + * Sets the perpendicular front tracking wheel for odometry. + * + * \param input + * an ez::tracking_wheel */ - bool drive_current_left_over(); + void odom_tracker_front_set(tracking_wheel* input); /** - * Reset all the chassis motors, recommended to run at the start of your autonomous routine. + * Sets the perpendicular back tracking wheel for odometry. + * + * \param input + * an ez::tracking_wheel */ - void drive_sensor_reset(); + void odom_tracker_back_set(tracking_wheel* input); /** - * Resets the current imu value. Defaults to 0, recommended to run at the start of your autonomous routine. + * Sets the default behavior for turns in odom, swinging, and turning. * - * \param new_heading - * New heading value. + * \param behavior + * ez::shortest, ez::longest, ez::left, ez::right, ez::raw */ - void drive_imu_reset(double new_heading = 0); + void pid_angle_behavior_set(e_angle_behavior behavior); /** - * Returns the current imu rotation value. + * Sets the default behavior for turns in turning movements. + * + * \param behavior + * ez::shortest, ez::longest, ez::left, ez::right, ez::raw */ - double drive_imu_get(); + void pid_turn_behavior_set(e_angle_behavior behavior); + + /** + * Sets the default behavior for turns in swinging movements. + * + * \param behavior + * ez::shortest, ez::longest, ez::left, ez::right, ez::raw + */ + void pid_swing_behavior_set(e_angle_behavior behavior); + + /** + * Sets the default behavior for turns in odom turning movements. + * + * \param behavior + * ez::shortest, ez::longest, ez::left, ez::right, ez::raw + */ + void pid_odom_behavior_set(e_angle_behavior behavior); + + /** + * Returns the turn behavior for turns. + */ + e_angle_behavior pid_turn_behavior_get(); + + /** + * Returns the turn behavior for swings. + */ + e_angle_behavior pid_swing_behavior_get(); + + /** + * Returns the turn behavior for odom turns. + */ + e_angle_behavior pid_odom_behavior_get(); + + /** + * Gives some wiggle room in shortest vs longest, so a 180.1 and 179.9 degree turns have consistent behavior. + * + * \param p_tolerance + * angle wiggle room, an okapi unit + */ + void pid_angle_behavior_tolerance_set(okapi::QAngle p_tolerance); + + /** + * Gives some wiggle room in shortest vs longest, so a 180.1 and 179.9 degree turns have consistent behavior. + * + * \param p_tolerance + * angle wiggle room, in degrees + */ + void pid_angle_behavior_tolerance_set(double tolerance); + + /** + * Returns the wiggle room in shortest vs longest, so a 180.1 and 179.9 degree turns have consistent behavior. + */ + double pid_angle_behavior_tolerance_get(); + + /** + * When a turn is within its tolerance, you can have it bias left or right. + * + * \param behavior + * ez::left or ez::right + */ + void pid_angle_behavior_bias_set(e_angle_behavior behavior); + + /** + * Returns the behavior when a turn is within its tolerance, you can have it bias left or right. + */ + e_angle_behavior pid_angle_behavior_bias_get(); + + ///// + // + // User Control + // + ///// + + /** + * Sets the chassis to controller joysticks using tank control. + * Run in usercontrol. + * + * This passes the controller through the curve functions, but is disabled by default. + * Use opcontrol_curve_buttons_toggle() to enable it. + */ + void opcontrol_tank(); + + /** + * Sets the chassis to controller joysticks using standard arcade control, where left stick is fwd/rev. + * Run in usercontrol. + * + * This passes the controller through the curve functions, but is disabled by default. + * Use opcontrol_curve_buttons_toggle() to enable it. + * + * \param stick_type + * ez::SINGLE or ez::SPLIT control + */ + void opcontrol_arcade_standard(e_type stick_type); + + /** + * Sets the chassis to controller joysticks using flipped arcade control, where right stick is fwd/rev. + * Run in usercontrol. + * + * This passes the controller through the curve functions, but is disabled by default. + * Use opcontrol_curve_buttons_toggle() to enable it. + * + * \param stick_type + * ez::SINGLE or ez::SPLIT control + */ + void opcontrol_arcade_flipped(e_type stick_type); + + /** + * Initializes left and right curves with the SD card, recommended to run in initialize(). + */ + void opcontrol_curve_sd_initialize(); + + /** + * Sets the default joystick curves. + * + * \param left + * left default curve + * \param right + * right default curve + */ + void opcontrol_curve_default_set(double left, double right = 0); + + /** + * Gets the default joystick curves, in {left, right} + */ + std::vector opcontrol_curve_default_get(); + + /** + * Runs a PID loop on the drive when the joysticks are released. + * + * \param kp + * proportional term + * \param ki + * integral term + * \param kd + * derivative term + * \param start_i + * error threshold to start integral + */ + void opcontrol_drive_activebrake_set(double kp, double ki = 0.0, double kd = 0.0, double start_i = 0.0); + + /** + * Returns kP for active brake. + */ + double opcontrol_drive_activebrake_get(); + + /** + * Returns all PID constants for active brake. + */ + PID::Constants opcontrol_drive_activebrake_constants_get(); + + /** + * Enables/disables modifying the joystick input curves with the controller. + * + * \param input + * true enables, false disables + */ + void opcontrol_curve_buttons_toggle(bool toggle); + + /** + * Gets the current state of the toggle. Enables/disables modifying the joystick input curves with the controller. + * + * True enabled, false disabled. + */ + bool opcontrol_curve_buttons_toggle_get(); + + /** + * Sets buttons for modifying the left joystick curve. + * + * \param decrease + * a pros button enumerator + * \param increase + * a pros button enumerator + */ + void opcontrol_curve_buttons_left_set(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase); + + /** + * Returns a vector of pros controller buttons user for the left joystick curve, in {decrease, increase} + */ + std::vector opcontrol_curve_buttons_left_get(); + + /** + * Sets buttons for modifying the right joystick curve. + * + * \param decrease + * a pros button enumerator + * \param increase + * a pros button enumerator + */ + void opcontrol_curve_buttons_right_set(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase); + + /** + * Returns a vector of pros controller buttons user for the right joystick curve, in {decrease, increase} + */ + std::vector opcontrol_curve_buttons_right_get(); + + /** + * Outputs a curve from 5225A In the Zone. + * + * This gives more control over the robot at lower speeds. + * + * \param x + * joystick input + */ + double opcontrol_curve_left(double x); + + /** + * Outputs a curve from 5225A In the Zone. + * + * This gives more control over the robot at lower speeds. + * + * \param x + * joystick input + */ + double opcontrol_curve_right(double x); + + /** + * Sets a new threshold for the joystick. + * + * The joysticks wil not return a value if they are within this. + * + * \param threshold + * new threshold + */ + void opcontrol_joystick_threshold_set(int threshold); + + /** + * Gets the threshold for the joystick. + */ + int opcontrol_joystick_threshold_get(); + + /** + * Resets drive sensors at the start of opcontrol. + */ + void opcontrol_drive_sensors_reset(); + + /** + * Sets minimum value distance constants. + * + * \param l_stick + * input for left joystick + * \param r_stick + * input for right joystick + */ + void opcontrol_joystick_threshold_iterate(int l_stick, int r_stick); + + ///// + // + // PTO + // + ///// + + /** + * Checks if the motor is currently in pto_list. + * + * Returns true if it's already in pto_list. + * + * \param check_if_pto + * motor to check + */ + bool pto_check(pros::Motor check_if_pto); + + /** + * Adds motors to the pto list, removing them from the drive. + * + * You cannot add the first index because it's used for autonomous. + * + * \param pto_list + * list of motors to remove from the drive + */ + void pto_add(std::vector pto_list); + + /** + * Removes motors from the pto list, adding them to the drive. + * + * \param pto_list + * list of motors to add to the drive + */ + void pto_remove(std::vector pto_list); + + /** + * Adds/removes motors from drive. + * + * You cannot add the first index because it's used for autonomous. + * + * \param pto_list + * list of motors to add/remove from the drive + * \param toggle + * list of motors to add/remove from the drive + */ + void pto_toggle(std::vector pto_list, bool toggle); + + ///// + // + // PROS Wrappers + // + ///// + + /** + * Sets the chassis to voltage. + * + * Disables PID when called. + * + * \param left + * voltage for left side, -127 to 127 + * \param right + * voltage for right side, -127 to 127 + */ + void drive_set(int left, int right); + + /** + * Gets the chassis to voltage, -127 to 127. Returns {left, right}. + */ + std::vector drive_get(); + + /** + * Changes the way the drive behaves when it is not under active user control. + * + * \param brake_type + * the 'brake mode' of the motor e.g. 'pros::E_MOTOR_BRAKE_COAST' 'pros::E_MOTOR_BRAKE_BRAKE' 'pros::E_MOTOR_BRAKE_HOLD' + */ + void drive_brake_set(pros::motor_brake_mode_e_t brake_type); + + /** + * Returns the brake mode of the drive in pros_brake_mode_e_t_. + */ + pros::motor_brake_mode_e_t drive_brake_get(); + + /** + * Sets the limit for the current on the drive. + * + * \param mA + * input in milliamps + */ + void drive_current_limit_set(int mA); + + /** + * Gets the limit for the current on the drive. + */ + int drive_current_limit_get(); + + /** + * Toggles set drive in autonomous. + * + * \param toggle + * true enables, false disables + */ + void pid_drive_toggle(bool toggle); + + /** + * Gets the current state of the toggle. + * + * This toggles set drive in autonomous. + * + * True enabled, false disabled. + */ + bool pid_drive_toggle_get(); + + /** + * Toggles printing in autonomous. + * + * \param toggle + * true enables, false disables + */ + void pid_print_toggle(bool toggle); + + /** + * Gets the current state of the toggle. + * + * This toggles printing in autonomous. + * + * True enabled, false disabled. + */ + bool pid_print_toggle_get(); + + ///// + // + // Telemetry + // + ///// + + /** + * The position of the right sensor in inches. + * + * If you have two parallel tracking wheels, this will return tracking wheel position. Otherwise this returns motor position. + */ + double drive_sensor_right(); + + /** + * The position of the right sensor. + * + * If you have two parallel tracking wheels, this will return tracking wheel position. Otherwise this returns motor position. + */ + int drive_sensor_right_raw(); + + /** + * The velocity of the right motor. + */ + int drive_velocity_right(); + + /** + * The watts of the right motor. + */ + double drive_mA_right(); + + /** + * Return true when the motor is over current. + */ + bool drive_current_right_over(); + + /** + * The position of the left sensor in inches. + * + * If you have two parallel tracking wheels, this will return tracking wheel position. Otherwise this returns motor position. + */ + double drive_sensor_left(); + + /** + * The position of the left sensor. + * + * If you have two parallel tracking wheels, this will return tracking wheel position. Otherwise this returns motor position. + */ + int drive_sensor_left_raw(); + + /** + * The velocity of the left motor. + */ + int drive_velocity_left(); + + /** + * The watts of the left motor. + */ + double drive_mA_left(); + + /** + * Return true when the motor is over current. + */ + bool drive_current_left_over(); + + /** + * Reset all the chassis motors and tracking wheels, recommended to run at the start of your autonomous routine. + */ + void drive_sensor_reset(); + + /** + * Resets the current imu value. Defaults to 0, recommended to run at the start of your autonomous routine. + * + * \param new_heading + * new heading value + */ + void drive_imu_reset(double new_heading = 0); + + /** + * Returns the current imu rotation value in degrees. + */ + double drive_imu_get(); /** * Returns the current imu accel x + accel y value. */ - double drive_imu_accel_get(); + double drive_imu_accel_get(); + + /** + * Sets a new imu scaling factor. + * + * This value is multiplied by the imu to change its output. + * + * \param scaler + * factor to scale the imu by + */ + void drive_imu_scaler_set(double scaler); + + /** + * Returns the current imu scaling factor. + */ + double drive_imu_scaler_get(); + + /** + * Calibrates the IMU, recommended to run in initialize(). + * + * \param run_loading_animation + * true runs the animation, false doesn't + */ + bool drive_imu_calibrate(bool run_loading_animation = true); + + /** + * Checks if the imu calibrated successfully or if it took longer than expected. + * + * Returns true if calibrated successfully, and false if unsuccessful. + */ + bool drive_imu_calibrated(); + + /** + * Loading display while the IMU calibrates. + */ + void drive_imu_display_loading(int iter); + + /** + * Practice mode for driver practice that shuts off the drive if you go max speed. + * + * \param toggle + * true enables, false disables + */ + void opcontrol_joystick_practicemode_toggle(bool toggle); + + /** + * Gets current state of the toggle. + * + * True is enabled, false is disabled. + */ + bool opcontrol_joystick_practicemode_toggle_get(); + + /** + * Reversal for drivetrain in opcontrol that flips the left and right side and the direction of the drive. + * + * \param toggle + * true if you want your drivetrain reversed and false if you do not + */ + void opcontrol_drive_reverse_set(bool toggle); + + /** + * Gets current state of the toggle. + * + * Reversal for drivetrain in opcontrol that flips the left and right side and the direction of the drive. + */ + bool opcontrol_drive_reverse_get(); + + ///// + // + // Autonomous Functions + // + ///// + + /** + * Sets the robot to move forward using PID without okapi units, only using slew if globally enabled. + * + * This function is actually odom. + * + * \param target + * target value as a double, unit is inches + * \param speed + * 0 to 127, max speed during motion + */ + void pid_odom_set(double target, int speed); + + /** + * Sets the robot to move forward using PID without okapi units, using slew if enabled for this motion. + * + * This function is actually odom + * + * \param target + * target value as a double, unit is inches + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_set(double target, int speed, bool slew_on); + + /** + * Sets the robot to move forward using PID with okapi units, only using slew if globally enabled. + * + * This function is actually odom + * + * \param target + * target value in inches + * \param speed + * 0 to 127, max speed during motion + */ + void pid_odom_set(okapi::QLength p_target, int speed); + + /** + * Sets the robot to move forward using PID with okapi units, using slew if enabled for this motion. + * + * This function is actually odom + * + * \param target + * target value in inches + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + * \param toggle_heading + * toggle for heading correction. true enables, false disables + */ + void pid_odom_set(okapi::QLength p_target, int speed, bool slew_on); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if globally enabled. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement + */ + void pid_odom_set(odom imovement); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if enabled for this motion. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_set(odom imovement, bool slew_on); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if globally enabled. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement + */ + void pid_odom_ptp_set(odom imovement); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if enabled for this motion. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_ptp_set(odom imovement, bool slew_on); + + /** + * Takes in an odom movement to go to a single point using boomerang. If an angle is set, this will run boomerang. Uses slew if globally enabled. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement + */ + void pid_odom_boomerang_set(odom imovement); + + /** + * Takes in an odom movement to go to a single point using boomerang. If an angle is set, this will run boomerang. Uses slew if enabled for this motion. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_boomerang_set(odom imovement, bool slew_on); + + /** + * Takes in an odom movement to go to a single point using boomerang. If an angle is set, this will run boomerang. Uses slew if globally enabled. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement. values are united here with okapi units + */ + void pid_odom_boomerang_set(united_odom p_imovement); + + /** + * Takes in an odom movement to go to a single point using boomerang. If an angle is set, this will run boomerang. Uses slew if enabled for this motion. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_boomerang_set(united_odom p_imovement, bool slew_on); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if globally enabled. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement. values are united here with okapi units + */ + void pid_odom_ptp_set(united_odom p_imovement); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if enabled for this motion. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_ptp_set(united_odom p_imovement, bool slew_on); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if globally enabled. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement. values are united here with okapi units + */ + void pid_odom_set(united_odom p_imovement); + + /** + * Takes in an odom movement to go to a single point. If an angle is set, this will run boomerang. Uses slew if enabled for this motion. + * + * \param imovement + * {{x, y, t}, fwd/rev, 1-127} an odom movement. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_set(united_odom p_imovement, bool slew_on); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + */ + void pid_odom_set(std::vector imovements); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_set(std::vector imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + */ + void pid_odom_pp_set(std::vector imovements); + + /** + * Takes in odom movements to go through multiple points. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_pp_set(std::vector imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points, will inject into the path. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + */ + void pid_odom_injected_pp_set(std::vector imovements); + + /** + * Takes in odom movements to go through multiple points, will inject into the path. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_injected_pp_set(std::vector imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + */ + void pid_odom_smooth_pp_set(std::vector imovements); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_smooth_pp_set(std::vector imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + */ + void pid_odom_smooth_pp_set(std::vector p_imovements); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_smooth_pp_set(std::vector p_imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points, will inject into the path. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + */ + void pid_odom_injected_pp_set(std::vector p_imovements); + + /** + * Takes in odom movements to go through multiple points, will inject into the path. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_injected_pp_set(std::vector p_imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + */ + void pid_odom_pp_set(std::vector p_imovements); + + /** + * Takes in odom movements to go through multiple points. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_pp_set(std::vector p_imovements, bool slew_on); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if globally enabled. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + */ + void pid_odom_set(std::vector p_imovements); + + /** + * Takes in odom movements to go through multiple points, will inject and smooth the path. If an angle is set, this will run boomerang for that point. Uses slew if enabled for this motion. + * + * \param imovements + * {{{x, y, t}, fwd/rev, 1-127}, {{x, y, t}, fwd/rev, 1-127}} odom movements. values are united here with okapi units + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_odom_set(std::vector p_imovements, bool slew_on); + + /** + * Sets the robot to move forward using PID with okapi units, only using slew if globally enabled. + * + * \param p_target + * target okapi unit + * \param speed + * 0 to 127, max speed during motion + */ + void pid_drive_set(okapi::QLength p_target, int speed); + + /** + * Sets the robot to move forward using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target okapi unit + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + * \param toggle_heading + * toggle for heading correction. true enables, false disables + */ + void pid_drive_set(okapi::QLength p_target, int speed, bool slew_on, bool toggle_heading = true); + + /** + * Sets the robot to move forward using PID without okapi units, only using slew if globally enabled. + * + * \param target + * target value in inches + * \param speed + * 0 to 127, max speed during motion + */ + void pid_drive_set(double target, int speed); + + /** + * Sets the robot to move forward using PID without okapi units, using slew if enabled for this motion. + * + * \param target + * target value in inches + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + * \param toggle_heading + * toggle for heading correction. true enables, false disables + */ + void pid_drive_set(double target, int speed, bool slew_on, bool toggle_heading = true); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face + * \param speed + * 0 to 127, max speed during motion + */ + void pid_turn_set(pose itarget, drive_directions dir, int speed); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(pose itarget, drive_directions dir, int speed, bool slew_on); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_set(pose itarget, drive_directions dir, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(pose itarget, drive_directions dir, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face. this uses okapi units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face. this uses okapi units + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, bool slew_on); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face. this uses okapi units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn face a point using PID and odometry. + * + * \param target + * {x, y} a target point to face. this uses okapi units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn relative to initial heading using PID. + * + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(double target, int speed); + + /** + * Sets the robot to turn relative to initial heading using PID. + * + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_set(double target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn relative to initial heading using PID, using slew if enabled for this motion. + * + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(double target, int speed, bool slew_on); + + /** + * Sets the robot to turn relative to initial heading using PID, using slew if enabled for this motion. + * + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(double target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn relative to initial heading using PID with okapi units. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_turn_set(okapi::QAngle p_target, int speed); + + /** + * Sets the robot to turn relative to initial heading using PID with okapi units. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn relative to initial heading using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(okapi::QAngle p_target, int speed, bool slew_on); + + /** + * Sets the robot to turn relative to initial heading using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn relative to current heading using PID with okapi units, only using slew if globally enabled. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_turn_relative_set(okapi::QAngle p_target, int speed); + + /** + * Sets the robot to turn relative to current heading using PID with okapi units, only using slew if globally enabled. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_relative_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn relative to current heading using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_relative_set(okapi::QAngle p_target, int speed, bool slew_on); + + /** + * Sets the robot to turn relative to current heading using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_relative_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn relative to current heading using PID without okapi units, only using slew if globally enabled. + * + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_turn_relative_set(double target, int speed); + + /** + * Sets the robot to turn relative to current heading using PID without okapi units, only using slew if globally enabled. + * + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_relative_set(double target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn relative to current heading using PID without okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_relative_set(double target, int speed, bool slew_on); + + /** + * Sets the robot to turn relative to current heading using PID without okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_relative_set(double target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, double target, int speed); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, double target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, double target, int speed, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, double target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, double target, int speed, int opposite_speed); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed); + + /** + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior); /** - * Sets a new imu scaling factor. This value is multiplied by the imu to change its output. + * Sets the robot to turn using only the left or right side relative to initial heading with okapi units, using slew if enabled for this motion. * - * \param scaler - * Factor to scale the imu by. + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void drive_imu_scaler_set(double scaler); + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, bool slew_on); /** - * Returns the current imu scaling factor. + * Sets the robot to turn only using the left or right side relative to initial heading using PID with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - double drive_imu_scaler_get(); + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on); /** - * Calibrates the IMU, recommended to run in initialize(). + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, only using slew if globally enabled. * - * \param run_loading_animation - * bool for running loading animation + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion */ - bool drive_imu_calibrate(bool run_loading_animation = true); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed); /** - * Loading display while the IMU calibrates. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion */ - void drive_imu_display_loading(int iter); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior); /** - * Practice mode for driver practice that shuts off the drive if you go max speed. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, using slew if enabled for this motion. * - * @param toggle True if you want this mode enables and False if you want it disabled. + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion */ - void opcontrol_joystick_practicemode_toggle(bool toggle); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, bool slew_on); /** - * Gets current state of the toggle. Practice mode for driver practice that shuts off the drive if you go max speed. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion */ - bool opcontrol_joystick_practicemode_toggle_get(); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on); /** - * Reversal for drivetrain in opcontrol that flips the left and right side and the direction of the drive + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, only using slew if globally enabled. * - * @param toggle True if you want your drivetrain reversed and False if you do not. + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void opcontrol_drive_reverse_set(bool toggle); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed); /** - * Gets current state of the toggle. Reversal for drivetrain in opcontrol that flips the left and right side and the direction of the drive. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - bool opcontrol_drive_reverse_get(); - - ///// - // - // Autonomous Functions - // - ///// + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior); /** - * Sets the robot to move forward using PID with okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, using slew if enabled for this motion. * - * \param target - * target value in inches + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units * \param speed * 0 to 127, max speed during motion - * \param slew_on - * ramp up from a lower speed to your target speed - * \param toggle_heading - * toggle for heading correction + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void pid_drive_set(okapi::QLength p_target, int speed, bool slew_on = false, bool toggle_heading = true); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, bool slew_on); /** - * Sets the robot to move forward using PID without okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, using slew if enabled for this motion. * - * \param target - * target value as a double, unit is inches + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units * \param speed * 0 to 127, max speed during motion - * \param slew_on - * ramp up from a lower speed to your target speed - * \param toggle_heading - * toggle for heading correction + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void pid_drive_set(double target, int speed, bool slew_on = false, bool toggle_heading = true); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on); /** - * Sets the robot to turn using PID. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, only using slew if globally enabled. * - * \param target + * \param type + * L_SWING or R_SWING + * \param p_target * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion - * \param slew_on - * ramp up from a lower speed to your target speed */ - void pid_turn_set(double target, int speed, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed); /** - * Sets the robot to turn using PID with okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, only using slew if globally enabled. * + * \param type + * L_SWING or R_SWING * \param p_target - * target value in degrees + * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion - * \param slew_on - * ramp up from a lower speed to your target speed */ - void pid_turn_set(okapi::QAngle p_target, int speed, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, e_angle_behavior behavior); /** - * Sets the robot to turn relative to current heading using PID with okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, using slew if enabled for this motion. * + * \param type + * L_SWING or R_SWING * \param p_target - * target value in okapi angle units + * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion - * \param slew_on - * ramp up from a lower speed to your target speed */ - void pid_turn_relative_set(okapi::QAngle p_target, int speed, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, bool slew_on); /** - * Sets the robot to turn relative to current heading using PID without okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, using slew if enabled for this motion. * + * \param type + * L_SWING or R_SWING * \param p_target * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion - * \param slew_on - * ramp up from a lower speed to your target speed */ - void pid_turn_relative_set(double target, int speed, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, e_angle_behavior behavior, bool slew_on); /** - * Turn using only the left or right side without okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, only using slew if globally enabled. * * \param type * L_SWING or R_SWING @@ -800,40 +2422,40 @@ class Drive { * \param speed * 0 to 127, max speed during motion * \param opposite_speed - * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void pid_swing_set(e_swing type, double target, int speed, int opposite_speed = 0, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed); /** - * Turn using only the left or right side with okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, only using slew if globally enabled. * * \param type * L_SWING or R_SWING * \param p_target - * target value in degrees + * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion * \param opposite_speed - * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed = 0, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior); /** - * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, using slew if enabled for this motion. * * \param type * L_SWING or R_SWING * \param p_target - * target value in okapi angle units + * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion * \param opposite_speed - * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed = 0, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on); /** - * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, using slew if enabled for this motion. * * \param type * L_SWING or R_SWING @@ -842,9 +2464,9 @@ class Drive { * \param speed * 0 to 127, max speed during motion * \param opposite_speed - * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + * -127 to 127, max speed of the opposite side of the drive during the swing. this is used for arcs, and is defaulted to 0 */ - void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed = 0, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on); /** * Resets all PID targets to 0. @@ -852,12 +2474,12 @@ class Drive { void pid_targets_reset(); /** - * Sets heading of imo and target of PID, okapi angle. + * Sets heading of imu and target of PID, okapi angle. */ void drive_angle_set(okapi::QAngle p_angle); /** - * Sets heading of imo and target of PID, takes double as an angle. + * Sets heading of imu and target of PID, takes double as an angle. */ void drive_angle_set(double angle); @@ -886,40 +2508,97 @@ class Drive { * Lock the code in a while loop until this position has passed for driving without okapi units. * * \param target - * for driving or turning, using a double. degrees for turns/swings, inches for driving. + * for driving or turning, using a double. degrees for turns/swings, inches for driving */ void pid_wait_until(double target); /** * Lock the code in a while loop until the robot has settled. - * Wrapper for pid_wait_until(target), target is your previously input target + * + * Wrapper for pid_wait_until(target), target is your previously input target. */ void pid_wait_quick(); /** * Lock the code in a while loop until the robot has settled. - * This also adds distance to target, and then exits with pid_wait_quick + * + * This also adds distance to target, and then exits with pid_wait_quick. + * * This will exit the motion while carrying momentum into the next motion. */ void pid_wait_quick_chain(); /** - * Autonomous interference detection. Returns true when interfered, and false when nothing happened. + * Lock the code in a while loop until this point has been passed. + * + * \param index + * index of your input points, 0 is the first point in the index + */ + void pid_wait_until_index(int index); + + /** + * Lock the code in a while loop until this point becomes the target. + * + * \param index + * index of your input points, 0 is the first point in the index + */ + void pid_wait_until_index_started(int index); + + /** + * Lock the code in a while loop until this point has been passed. + * + * \param target + * {x, y} pose for the robot to pass through before the while loop is released + */ + void pid_wait_until_point(pose target); + + /** + * Lock the code in a while loop until this point has been passed, with okapi units. + * + * \param target + * {x, y} pose with units for the robot to pass through before the while loop is released + */ + void pid_wait_until_point(united_pose target); + + /** + * Lock the code in a while loop until this point has been passed. + * + * Wrapper for pid_wait_until_point. + * + * \param target + * {x, y} a pose for the robot to pass through before the while loop is released + */ + void pid_wait_until(pose target); + + /** + * Lock the code in a while loop until this point has been passed, with okapi units. + * + * Wrapper for pid_wait_until_point. + * + * \param target + * {x, y} a pose with units for the robot to pass through before the while loop is released + */ + void pid_wait_until(united_pose target); + + /** + * Autonomous interference detection. + * + * Returns true when interfered, and false when nothing happened. */ bool interfered = false; /** - * @brief Set the ratio of the robot + * Set the ratio of the robot. * - * @param ratio + * \param ratio * ratio of the gears */ void drive_ratio_set(double ratio); /** - * @brief Set the cartridge/wheel rpm of the robot + * Set the cartridge/wheel rpm of the robot. * - * @param rpm + * \param rpm * rpm of the cartridge or wheel */ void drive_rpm_set(double rpm); @@ -938,7 +2617,7 @@ class Drive { * Changes max speed during a drive motion. * * \param speed - * new clipped speed + * new clipped speed, between 0 and 127 */ void pid_speed_max_set(int speed); @@ -948,27 +2627,27 @@ class Drive { int pid_speed_max_get(); /** - * @brief Set the turn pid constants object + * Set the turn pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_turn_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. - * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns PID constants with PID::Constants. */ PID::Constants pid_turn_constants_get(); /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This sets turning constants. * * \param input @@ -978,6 +2657,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This sets turning constants. * * \param input @@ -991,67 +2671,67 @@ class Drive { double pid_turn_chain_constant_get(); /** - * @brief Set the swing pid constants object + * Set the swing pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_swing_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. Returns -1 if fwd and rev constants aren't the same! + * Returns PID constants with PID::Constants. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns -1 if fwd and rev constants aren't the same! */ PID::Constants pid_swing_constants_get(); /** - * @brief Set the forward swing pid constants object + * Set the forward swing pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_swing_constants_forward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. - * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns PID constants with PID::Constants. */ PID::Constants pid_swing_constants_forward_get(); /** - * @brief Set the backward swing pid constants object + * Set the backward swing pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_swing_constants_backward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. - * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns PID constants with PID::Constants. */ PID::Constants pid_swing_constants_backward_get(); /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This sets forward and backwards swing constants. * * \param input @@ -1061,6 +2741,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets forward swing constants. * * \param input @@ -1070,6 +2751,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets backward swing constants. * * \param input @@ -1079,6 +2761,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This sets forward and backwards swing constants. * * \param input @@ -1088,6 +2771,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets forward constants. * * \param input @@ -1102,6 +2786,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets backwards swing constants. * * \param input @@ -1115,87 +2800,86 @@ class Drive { double pid_swing_chain_backward_constant_get(); /** - * @brief Set the heading pid constants object + * Set the heading pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_heading_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. - * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns PID constants with PID::Constants. */ PID::Constants pid_heading_constants_get(); /** - * @brief Set the drive pid constants object + * Set the drive pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_drive_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. Returns -1 if fwd and rev constants aren't the same! + * Returns PID constants with PID::Constants. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns -1 if fwd and rev constants aren't the same! */ PID::Constants pid_drive_constants_get(); /** - * @brief Set the forward pid constants object + * Set the forward pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_drive_constants_forward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. - * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns PID constants with PID::Constants. */ PID::Constants pid_drive_constants_forward_get(); /** - * @brief Set the backwards pid constants object + * Set the backwards pid constants object. * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral */ void pid_drive_constants_backward_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); /** - * @brief returns PID constants with PID::Constants. - * - * @param p kP - * @param i kI - * @param d kD - * @param p_start_i start_I + * Returns PID constants with PID::Constants. */ PID::Constants pid_drive_constants_backward_get(); /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This sets forward and backwards driving constants. * * \param input @@ -1205,6 +2889,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets forward driving constants. * * \param input @@ -1214,6 +2899,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets backward driving constants. * * \param input @@ -1223,6 +2909,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This sets forward and backwards driving constants. * * \param input @@ -1232,6 +2919,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets forward driving constants. * * \param input @@ -1246,6 +2934,7 @@ class Drive { /** * Sets the amount that the PID will overshoot target by to maintain momentum into the next motion. + * * This only sets backward driving constants. * * \param input @@ -1284,21 +2973,131 @@ class Drive { */ int pid_turn_min_get(); + /** + * Set the odom angular pid constants object. + * + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral + */ + void pid_odom_angular_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); + + /** + * Set the odom boomerang pid constants object. + * + * \param p + * proportional term + * \param i + * integral term + * \param d + * derivative term + * \param p_start_i + * error threshold to start integral + */ + void pid_odom_boomerang_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0); + + /** + * Set's constants for odom driving exit conditions. + * + * \param p_small_exit_time + * time to exit when within smalL_error, in ms + * \param p_small_error + * small timer will start when error is within this, in inches + * \param p_big_exit_time + * time to exit when within big_error, in ms + * \param p_big_error + * big timer will start when error is within this, in inches + * \param p_velocity_exit_time + * velocity timer will start when velocity is 0, in ms + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, in ms + * \param use_imu + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't + */ + 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); + + /** + * Set's constants for odom turning exit conditions. + * + * \param p_small_exit_time + * time to exit when within smalL_error, in ms + * \param p_small_error + * small timer will start when error is within this, in degrees + * \param p_big_exit_time + * time to exit when within big_error, in ms + * \param p_big_error + * big timer will start when error is within this, in degrees + * \param p_velocity_exit_time + * velocity timer will start when velocity is 0, in ms + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, in ms + * \param use_imu + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't + */ + 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); + + /** + * Set's constants for odom turning exit conditions. + * + * \param p_small_exit_time + * time to exit when within smalL_error, okapi unit + * \param p_small_error + * small timer will start when error is within this, okapi unit + * \param p_big_exit_time + * time to exit when within big_error, okapi unit + * \param p_big_error + * big timer will start when error is within this, okapi unit + * \param p_velocity_exit_time + * velocity timer will start when velocity is 0, okapi unit + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, okapi unit + * \param use_imu + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't + */ + 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); + + /** + * Set's constants for odom driving exit conditions. + * + * \param p_small_exit_time + * time to exit when within smalL_error, okapi unit + * \param p_small_error + * small timer will start when error is within this, okapi unit + * \param p_big_exit_time + * time to exit when within big_error, okapi unit + * \param p_big_error + * big timer will start when error is within this, okapi unit + * \param p_velocity_exit_time + * velocity timer will start when velocity is 0, okapi unit + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, okapi unit + * \param use_imu + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't + */ + 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); + /** * Set's constants for drive exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. In okapi units. + * time to exit when within smalL_error, okapi unit * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. In okapi units. + * small timer will start when error is within this, okapi unit * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. In okapi units. + * time to exit when within big_error, okapi unit * \param p_big_error - * Sets big_error. Timer will start when error is within this. In okapi units. + * big timer will start when error is within this, okapi unit * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. In okapi units. + * velocity timer will start when velocity is 0, okapi unit + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, okapi unit * \param use_imu - * Adds the Imu for velocity calculation in conjunction with the main sensor. + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't */ 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); @@ -1306,17 +3105,19 @@ class Drive { * Set's constants for turn exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. In okapi units. + * time to exit when within smalL_error, okapi unit * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. In okapi units. + * small timer will start when error is within this, okapi unit * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. In okapi units. + * time to exit when within big_error, okapi unit * \param p_big_error - * Sets big_error. Timer will start when error is within this. In okapi units. + * big timer will start when error is within this, okapi unit * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. In okapi units. + * velocity timer will start when velocity is 0, okapi unit + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, okapi unit * \param use_imu - * Adds the Imu for velocity calculation in conjunction with the main sensor. + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't */ 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); @@ -1324,17 +3125,19 @@ class Drive { * Set's constants for swing exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. In okapi units. + * time to exit when within smalL_error, okapi unit * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. In okapi units. + * small timer will start when error is within this, okapi unit * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. In okapi units. + * time to exit when within big_error, okapi unit * \param p_big_error - * Sets big_error. Timer will start when error is within this. In okapi units. + * big timer will start when error is within this, okapi unit * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. In okapi units. + * velocity timer will start when velocity is 0, okapi unit + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, okapi unit * \param use_imu - * Adds the Imu for velocity calculation in conjunction with the main sensor. + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't */ 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); @@ -1342,17 +3145,19 @@ class Drive { * Set's constants for drive exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. + * time to exit when within smalL_error, in ms * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. + * small timer will start when error is within this, in inches * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. + * time to exit when within big_error, in ms * \param p_big_error - * Sets big_error. Timer will start when error is within this. + * big timer will start when error is within this, in inches * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. + * velocity timer will start when velocity is 0, in ms + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, in ms * \param use_imu - * Adds the Imu for velocity calculation in conjunction with the main sensor. + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't */ 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); @@ -1360,17 +3165,19 @@ class Drive { * Set's constants for turn exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. + * time to exit when within smalL_error, in ms * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. + * small timer will start when error is within this, in degrees * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. + * time to exit when within big_error, in ms * \param p_big_error - * Sets big_error. Timer will start when error is within this. + * big timer will start when error is within this, in degrees * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. + * velocity timer will start when velocity is 0, in ms + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, in ms * \param use_imu - * Adds the Imu for velocity calculation in conjunction with the main sensor. + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't */ 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); @@ -1378,57 +3185,61 @@ class Drive { * Set's constants for swing exit conditions. * * \param p_small_exit_time - * Sets small_exit_time. Timer for to exit within smalL_error. + * time to exit when within smalL_error, in ms * \param p_small_error - * Sets smalL_error. Timer will start when error is within this. + * small timer will start when error is within this, in degrees * \param p_big_exit_time - * Sets big_exit_time. Timer for to exit within big_error. + * time to exit when within big_error, in ms * \param p_big_error - * Sets big_error. Timer will start when error is within this. + * big timer will start when error is within this, in degrees * \param p_velocity_exit_time - * Sets velocity_exit_time. Timer will start when velocity is 0. + * velocity timer will start when velocity is 0, in ms + * \param p_mA_timeout + * mA timer will start when the motors are pulling too much current, in ms * \param use_imu - * Adds the Imu for velocity calculation in conjunction with the main sensor. + * true adds the imu for velocity calculation in conjunction with the main sensor, false doesn't */ 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); /** - * Returns current tick_per_inch() + * Returns current tick_per_inch. */ double drive_tick_per_inch(); /** - * Returns current tick_per_inch() + * Iterates modifying controller curves with the controller. */ void opcontrol_curve_buttons_iterate(); /** - * Enables PID Tuner + * Enables PID Tuner. */ void pid_tuner_enable(); /** - * Disables PID Tuner + * Disables PID Tuner. */ void pid_tuner_disable(); /** - * Toggles PID tuner between enabled and disables + * Toggles PID tuner between enabled and disabled. */ void pid_tuner_toggle(); /** - * Checks if PID Tuner is enabled. True is enabled, false is disables. + * Checks if PID Tuner is enabled. + * + * True is enabled, false is disabled. */ bool pid_tuner_enabled(); /** - * Iterates through controller inputs to modify PID constants + * Iterates through controller inputs to modify PID constants. */ void pid_tuner_iterate(); /** - * Toggle for printing the display of the PID Tuner to the brain + * Toggle for printing the display of the PID Tuner to the brain. * * \param input * true prints to brain, false doesn't @@ -1436,7 +3247,7 @@ class Drive { void pid_tuner_print_brain_set(bool input); /** - * Toggle for printing the display of the PID Tuner to the terminal + * Toggle for printing the display of the PID Tuner to the terminal. * * \param input * true prints to terminal, false doesn't @@ -1444,68 +3255,230 @@ class Drive { void pid_tuner_print_terminal_set(bool input); /** - * Returns true if printing to terminal is enabled + * Returns true if printing to terminal is enabled. */ bool pid_tuner_print_terminal_enabled(); /** - * Returns true if printing to brain is enabled + * Returns true if printing to brain is enabled. */ bool pid_tuner_print_brain_enabled(); /** - * Sets the value that PID Tuner increments P + * Sets the value that PID Tuner increments P. * * \param p - * double, p will increase by this + * p will increase by this */ void pid_tuner_increment_p_set(double p); /** - * Sets the value that PID Tuner increments I + * Sets the value that PID Tuner increments I. * * \param p - * double, i will increase by this + * i will increase by this */ void pid_tuner_increment_i_set(double i); /** - * Sets the value that PID Tuner increments D + * Sets the value that PID Tuner increments D. * * \param p - * double, d will increase by this + * d will increase by this */ void pid_tuner_increment_d_set(double d); /** - * Sets the value that PID Tuner increments Start I + * Sets the value that PID Tuner increments Start I. * * \param p - * double, start i will increase by this + * start i will increase by this */ void pid_tuner_increment_start_i_set(double start_i); /** - * Returns the value that PID Tuner increments P + * Returns the value that PID Tuner increments P. */ double pid_tuner_increment_p_get(); /** - * Returns the value that PID Tuner increments I + * Returns the value that PID Tuner increments I. */ double pid_tuner_increment_i_get(); /** - * Returns the value that PID Tuner increments D + * Returns the value that PID Tuner increments D. */ double pid_tuner_increment_d_get(); /** - * Returns the value that PID Tuner increments Start I + * Returns the value that PID Tuner increments Start I. */ double pid_tuner_increment_start_i_get(); - private: // !Auton + /** + * Enables the full PID tuner with unique fwd/rev constants. + * + * \param enable + * true will enable the full PID tuner, false will use the simplified PID tuner + */ + void pid_tuner_full_enable(bool enable); + + /** + * Returns if the full PID tuner with unique fwd/rev constants is enabled. + * + * True means the full PID tuner is enabled, false means the simplified PID tuner is enabled. + */ + bool pid_tuner_full_enabled(); + + struct const_and_name { + std::string name = ""; + PID::Constants* consts; + }; + + /** + * Vector used for a simplified PID Tuner + */ + std::vector 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}}; + + /** + * Vector used for the full PID Tuner + */ + std::vector 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}}; + + /** + * Sets the max speed for user control. + * + * \param int + * the speed limit + */ + void opcontrol_speed_max_set(int speed); + + /** + * Returns the max speed for user control. + */ + int opcontrol_speed_max_get(); + + /** + * Toggles vector scaling for arcade control. True enables, false disables. + * + * \param bool + * true enables, false disables + */ + void opcontrol_arcade_scaling(bool enable); + + /** + * Returns if vector scaling for arcade control is enabled. True enables, false disables. + */ + 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* used_pid_tuner_pids; + double opcontrol_speed_max = 127.0; + bool arcade_vector_scaling = false; + // odom privates + std::vector pp_movements; + std::vector injected_pp_index; + int pp_index = 0; + std::vector smooth_path(std::vector 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 imovements, bool slew_on); + bool ptf1_running = false; + std::vector 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 inject_points(std::vector imovements); + std::vector 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, ez::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 decide_vert_sensor(ez::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 set_odoms_direction(std::vector 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; @@ -1529,11 +3502,6 @@ class Drive { bool slew_swing_using_angle = false; bool pid_tuner_terminal_b = false; bool pid_tuner_lcd_b = true; - struct const_and_name { - std::string name = ""; - PID::Constants *consts; - }; - std::vector constants; void pid_tuner_print(); void pid_tuner_value_modify(float p, float i, float d, float start); void pid_tuner_value_increase(); @@ -1576,11 +3544,6 @@ class Drive { */ bool heading_on = true; - /** - * Active brake kp constant. - */ - double active_brake_kp = 0; - /** * Tick per inch calculation. */ @@ -1604,6 +3567,9 @@ class Drive { void swing_pid_task(); void turn_pid_task(); void ez_auto_task(); + void ptp_task(); + void boomerang_task(); + void pp_task(); /** * Starting value for left/right @@ -1624,6 +3590,7 @@ class Drive { #define DRIVE_INTEGRATED 1 #define DRIVE_ADI_ENCODER 2 #define DRIVE_ROTATION 3 +#define ODOM_TRACKER 4 /** * Is tracking? @@ -1656,7 +3623,7 @@ class Drive { /** * Function for button presses. */ - void button_press(button_ *input_name, int button, std::function changeCurve, std::function save); + void button_press(button_* input_name, int button, std::function changeCurve, std::function save); /** * The left and right curve scalers. @@ -1677,4 +3644,4 @@ class Drive { */ bool is_reversed = false; }; -}; // namespace ez \ No newline at end of file +}; // namespace ez diff --git a/include/EZ-Template/piston.hpp b/include/EZ-Template/piston.hpp index ec091ea8..af11a223 100644 --- a/include/EZ-Template/piston.hpp +++ b/include/EZ-Template/piston.hpp @@ -17,22 +17,26 @@ class Piston { pros::adi::DigitalOut piston; /** - * Piston constructor. This class keeps track of piston state. The starting position of your piston is FALSE. + * Piston constructor. + * + * The starting position of your piston defaults to false. * * \param input_port - * The ports of your pistons. + * the ports of your pistons * \param default_state - * Starting state of your piston. + * starting state of your piston */ Piston(int input_port, bool default_state = false); /** - * Piston constructor in 3 wire expander. The starting position of your piston is FALSE. + * Piston constructor in 3 wire expander. + * + * The starting position of your piston defaults to false. * * \param input_ports - * The ports of your pistons. + * the ports of your pistons * \param default_state - * Starting state of your piston. + * starting state of your piston */ Piston(int input_port, int expander_smart_port, bool default_state = false); @@ -40,7 +44,7 @@ class Piston { * Sets the piston to the input. * * \param input - * True or false. True sets to the opposite of the starting position. + * true sets to the opposite of the starting position */ void set(bool input); @@ -53,7 +57,7 @@ class Piston { * One button toggle for the piston. * * \param toggle - * An input button. + * an input button */ void button_toggle(int toggle); @@ -61,9 +65,9 @@ class Piston { * Two buttons trigger the piston. Active is enabled, deactive is disabled. * * \param active - * Sets piston to true. + * sets piston to true * \param active - * Sets piston to false. + * sets piston to false */ void buttons(int active, int deactive); diff --git a/include/EZ-Template/sdcard.hpp b/include/EZ-Template/sdcard.hpp index ea8d5427..ce1250e8 100644 --- a/include/EZ-Template/sdcard.hpp +++ b/include/EZ-Template/sdcard.hpp @@ -12,6 +12,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. namespace ez { namespace as { extern AutonSelector auton_selector; + /** * Sets sd card to current page. */ @@ -43,7 +44,7 @@ void initialize(); void shutdown(); /** - * Returns true if the auton selector is running + * Returns true if the auton selector is running. */ bool enabled(); @@ -53,8 +54,9 @@ extern bool turn_off; extern pros::adi::DigitalIn* limit_switch_left; extern pros::adi::DigitalIn* limit_switch_right; + /** - * Initialize two limitswithces to change pages on the lcd + * Initialize two limit switches to change pages on the lcd. * * @param left_limit_port * port for the left limit switch @@ -67,5 +69,35 @@ void limit_switch_lcd_initialize(pros::adi::DigitalIn* right_limit, pros::adi::D * pre_auto_task */ void limitSwitchTask(); + +/** + * Returns the current blank page that is on. Negative value means the current page isn't blank. + */ +int page_blank_current(); + +/** + * Checks if this blank page is open. If this page doesn't exist, this will create it. + */ +bool page_blank_is_on(int page); + +/** + * Removes the blank page if it exists, and previous ones. + */ +void page_blank_remove(int page); + +/** + * Removes all blank pages. + */ +void page_blank_remove_all(); + +/** + * Removes the current amount of blank pages. + */ +int page_blank_amount(); + +/** + * Current amount of blank pages. + */ +extern int amount_of_blank_pages; } // namespace as } // namespace ez diff --git a/include/EZ-Template/slew.hpp b/include/EZ-Template/slew.hpp index 53e1fa21..4f33e302 100644 --- a/include/EZ-Template/slew.hpp +++ b/include/EZ-Template/slew.hpp @@ -77,7 +77,7 @@ class slew { double output(); /** - * Sets the max speed the slew can be + * Sets the max speed the slew can be. * * \param speed * maximum speed @@ -85,7 +85,7 @@ class slew { void speed_max_set(double speed); /** - * Returns the max speed the slew can be + * Returns the max speed the slew can be. */ double speed_max_get(); diff --git a/include/EZ-Template/tracking_wheel.hpp b/include/EZ-Template/tracking_wheel.hpp new file mode 100644 index 00000000..05a60b6b --- /dev/null +++ b/include/EZ-Template/tracking_wheel.hpp @@ -0,0 +1,162 @@ +/* +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 "pros/adi.hpp" +#include "pros/rotation.hpp" + +namespace ez { +class tracking_wheel { + public: + pros::adi::Encoder adi_encoder; + pros::Rotation smart_encoder; + + /** + * Creates a new tracking wheel with an ADI Encoder. + * + * \param ports + * {'A', 'B'}. make the encoder negative if reversed + * \param wheel_diameter + * assumed inches, this is the diameter of your wheel + * \param distance_to_center + * the distance to the center of your robot, this is used for tracking + * \param ratio + * the gear ratio of your tracking wheel. if it's not geared, this should be 1.0 + */ + tracking_wheel(std::vector ports, double wheel_diameter, double distance_to_center = 0.0, double ratio = 1.0); + + /** + * Creates a new tracking wheel with an ADI Encoder plugged into a 3-wire expander. + * + * \param smart_port + * the smart port your ADI Expander is plugged into + * \param ports + * {'A', 'B'}. make the encoder negative if reversed + * \param wheel_diameter + * assumed inches, this is the diameter of your wheel + * \param distance_to_center + * the distance to the center of your robot, this is used for tracking + * \param ratio + * the gear ratio of your tracking wheel. if it's not geared, this should be 1.0 + */ + tracking_wheel(int smart_port, std::vector ports, double wheel_diameter, double distance_to_center = 0.0, double ratio = 1.0); + + /** + * Creates a new tracking wheel with a Rotation sensor. + * + * \param port + * the port your Rotation sensor is plugged into, make this negative if reversed + * \param wheel_diameter + * assumed inches, this is the diameter of your wheel + * \param distance_to_center + * the distance to the center of your robot, this is used for tracking + * \param ratio + * the gear ratio of your tracking wheel. if it's not geared, this should be 1.0 + */ + tracking_wheel(int port, double wheel_diameter, double distance_to_center = 0.0, double ratio = 1.0); + + /** + * Returns how far the wheel has traveled in inches. + */ + double get(); + + /** + * Returns the raw sensor value. + */ + double get_raw(); + + /** + * Sets the distance to the center of the robot. + * + * \param input + * distance to the center of your robot in inches + */ + void distance_to_center_set(double input); + + /** + * Returns the distance to the center of the robot. + */ + double distance_to_center_get(); + + /** + * Sets the distance to the center to be flipped to negative or not. + * + * \param input + * flips distance to center to negative. false leaves it alone, true flips it + */ + void distance_to_center_flip_set(bool input); + + /** + * Returns if the distance to center is flipped or not. False is not, true is. + */ + bool distance_to_center_flip_get(); + + /** + * Resets your sensor. + */ + void reset(); + + /** + * Returns the constant for how many ticks is 1 inch. + */ + double ticks_per_inch(); + + /** + * Sets the amount of ticks per revolution of your sensor. + * + * This is useful for custom encoders. + * + * \param input + * ticks per revolution + */ + void ticks_per_rev_set(double input); + + /** + * Returns the amount of ticks per revolution of your sensor. + */ + double ticks_per_rev_get(); + + /** + * Sets the gear ratio for your tracking wheel. + * + * \param input + * gear ratio of tracking wheel + */ + void ratio_set(double input); + + /** + * Returns the gear ratio for your tracking wheel. + */ + double ratio_get(); + + /** + * Sets the diameter of your wheel. + * + * \param input + * wheel diameter + */ + void wheel_diameter_set(double input); + + /** + * Returns the diameter of your wheel. + */ + double wheel_diameter_get(); + + private: +#define DRIVE_ADI_ENCODER 2 +#define DRIVE_ROTATION 3 + int IS_TRACKER = 0; + + bool IS_FLIPPED = false; + + double DISTANCE_TO_CENTER = 0.0; + double WHEEL_DIAMETER = 0.0; + double RATIO = 1.0; + double ENCODER_TICKS_PER_REV = 0.0; + double WHEEL_TICK_PER_REV = 0.0; +}; +}; // namespace ez diff --git a/include/EZ-Template/util.hpp b/include/EZ-Template/util.hpp index 531ab144..94d9d1dc 100644 --- a/include/EZ-Template/util.hpp +++ b/include/EZ-Template/util.hpp @@ -11,6 +11,12 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include #include "api.h" +#include "okapi/api/units/QAngle.hpp" +#include "okapi/api/units/QLength.hpp" +#include "okapi/api/units/QTime.hpp" +#include "util.hpp" + +using namespace okapi::literals; /** * Controller. @@ -20,18 +26,19 @@ extern pros::Controller master; namespace ez { /** - * Prints our branding all over your pros terminal + * Prints our branding all over your pros terminal. */ void ez_template_print(); /** - * Prints to the brain screen in one string. Splits input between lines with - * '\n' or when text longer then 32 characters. + * Prints to the brain screen in one string. + * + * Splits input between lines with '\n' or when text longer then 32 characters. * - * @param text - * Input string. Use '\n' for a new line - * @param line - * Starting line to print on, defaults to 0 + * \param text + * input string + * \param line + * starting line to print on, defaults to 0 */ void screen_print(std::string text, int line = 0); @@ -69,7 +76,78 @@ enum exit_output { RUNNING = 1, enum e_mode { DISABLE = 0, SWING = 1, TURN = 2, - DRIVE = 3 }; + TURN_TO_POINT = 3, + DRIVE = 4, + POINT_TO_POINT = 5, + PURE_PURSUIT = 6 }; + +/** + * Enum for drive directions. + */ +enum drive_directions { FWD = 0, + FORWARD = FWD, + fwd = FWD, + forward = FWD, + REV = 1, + REVERSE = REV, + rev = REV, + reverse = REV }; + +/** + * Enum for turn types. + */ +enum e_angle_behavior { raw = 0, + left_turn = 1, + LEFT_TURN = 1, + counterclockwise = 1, + ccw = 1, + right_turn = 2, + RIGHT_TURN = 2, + clockwise = 2, + cw = 2, + shortest = 3, + longest = 4 }; + +const double ANGLE_NOT_SET = 0.0000000000000000000001; +const okapi::QAngle p_ANGLE_NOT_SET = 0.0000000000000000000001_deg; + +/** + * Struct for coordinates. + */ +typedef struct pose { + double x; + double y; + double theta = ANGLE_NOT_SET; +} pose; + +/** + * Struct for united coordinates. + */ +typedef struct united_pose { + okapi::QLength x; + okapi::QLength y; + okapi::QAngle theta = p_ANGLE_NOT_SET; +} united_pose; + +/** + * Struct for odom movements. + */ +typedef struct odom { + pose target; + drive_directions drive_direction; + int max_xy_speed; + e_angle_behavior turn_behavior = shortest; +} odom; + +/** + * Struct for united odom movements. + */ +typedef struct united_odom { + united_pose target; + drive_directions drive_direction; + int max_xy_speed; + e_angle_behavior turn_behavior = shortest; +} united_odom; /** * Outputs string for exit_condition enum. @@ -80,28 +158,176 @@ namespace util { extern bool AUTON_RAN; /** - * Returns 1 if input is positive and -1 if input is negative + * Returns the amount of places after a decimal, maxing out at 6. + * + * \param input + * your input value with decimals + * \param min + * minimum number of decimal places to print, this is defaulted to 0 + */ +int places_after_decimal(double input, int min = 0); + +/** + * Returns a string with a specific number of decimal points. + * + * \param input + * your input value + * \param n + * the amount of decimals you want to display + */ +std::string to_string_with_precision(double input, int n = 2); + +/** + * Returns 1 if input is positive and -1 if input is negative. + * + * \param input + * your input value */ int sgn(double input); /** - * Returns true if the input is < 0 + * Returns true if the input is < 0. + * + * \param input + * your input value */ bool reversed_active(double input); /** - * Returns input restricted to min-max threshold + * Returns input restricted to min-max threshold. + * + * \param input + * your input value + * \param max + * the maximum input can be + * \param min + * the minimum input can be */ double clamp(double input, double max, double min); +/** + * Returns input restricted to min-max threshold. + * + * The minimum used is negative max. + * + * \param input + * your input value + * \param max + * the absolute value maximum input can be + */ +double clamp(double input, double max); + /** * Is the SD card plugged in? */ const bool SD_CARD_ACTIVE = pros::usd::is_installed(); /** - * Delay time for tasks + * Delay time for tasks, this is set to 10 ms. */ const int DELAY_TIME = 10; + +/** + * Converts radians to degrees. + * + * \param input + * your input radian + */ +double to_deg(double input); + +/** + * Converts degrees to radians. + * + * \param input + * your input degree + */ +double to_rad(double input); + +/** + * Returns the angle between two points. + * + * \param itarget + * target position + * \param icurrent + * current position + */ +double absolute_angle_to_point(pose itarget, pose icurrent); + +/** + * Returns the distance between two points. + * + * \param itarget + * target position + * \param icurrent + * current position + */ +double distance_to_point(pose itarget, pose icurrent); + +/** + * Constrains an angle between 180 and -180. + * + * \param theta + * input angle in degrees + */ +double wrap_angle(double theta); + +/** + * Returns a new pose that is projected off of the current pose. + * + * \param added + * how far to project a new point + * \param icurrent + * point to project off of + */ +pose vector_off_point(double added, pose icurrent); + +/** + * Returns the shortest angle for the robot to turn to in order to get to target. + * + * \param target + * target value in degrees + * \param current + * current value in degrees + * \param print = false + * will print the new value if this is true, defaults to false + */ +double turn_shortest(double target, double current, bool print = false); + +/** + * Returns the farthest away angle for the robot to turn to in order to get to target. + * + * \param target + * target value in degrees + * \param current + * current value in degrees + * \param print = false + * will print the new value if this is true, defaults to false + */ +double turn_longest(double target, double current, bool print = false); + +/** + * Converts pose with okapi units to a pose without okapi units. + * + * \param input + * a pose with units + */ +pose united_pose_to_pose(united_pose input); + +/** + * Converts vector of poses with okapi units to a vector of poses without okapi units. + * + * \param inputs + * poses with units + */ +std::vector united_odoms_to_odoms(std::vector inputs); + +/** + * Converts odom movement with united pose to an odom movement without united pose. + * + * \param input + * odom movement with units + */ +odom united_odom_to_odom(united_odom input); + } // namespace util } // namespace ez diff --git a/include/api.h b/include/api.h index b97e65d6..d56915cc 100644 --- a/include/api.h +++ b/include/api.h @@ -41,9 +41,9 @@ #define PROS_VERSION_MAJOR 4 #define PROS_VERSION_MINOR 1 +#define PROS_VERSION_PATCH 1 +#define PROS_VERSION_STRING "4.1.1" -#define PROS_VERSION_PATCH 0 -#define PROS_VERSION_STRING "4.1.0" #include "pros/adi.h" #include "pros/colors.h" diff --git a/include/autons.hpp b/include/autons.hpp index 654902d8..99ae3ab9 100644 --- a/include/autons.hpp +++ b/include/autons.hpp @@ -1,8 +1,6 @@ #pragma once -#include "EZ-Template/drive/drive.hpp" - -extern Drive chassis; +void default_constants(); void drive_example(); void turn_example(); @@ -12,5 +10,9 @@ void swing_example(); void motion_chaining(); void combining_movements(); void interfered_example(); - -void default_constants(); \ No newline at end of file +void odom_drive_example(); +void odom_pure_pursuit_example(); +void odom_pure_pursuit_wait_until_example(); +void odom_boomerang_example(); +void odom_boomerang_injected_pure_pursuit_example(); +void measure_offsets(); \ No newline at end of file diff --git a/include/main.h b/include/main.h index 987e2f24..08269a2e 100644 --- a/include/main.h +++ b/include/main.h @@ -39,15 +39,14 @@ /** * You should add more #includes here */ -//#include "okapi/api.hpp" -//#include "pros/api_legacy.h" +// #include "okapi/api.hpp" +// #include "pros/api_legacy.h" #include "EZ-Template/api.hpp" // More includes here... #include "autons.hpp" #include "subsystems.hpp" - /** * If you find doing pros::Motor() to be tedious and you'd prefer just to do * Motor, you can use the namespace with the following commented out line. diff --git a/include/pros/optical.h b/include/pros/optical.h index 049c395b..61a2c86d 100644 --- a/include/pros/optical.h +++ b/include/pros/optical.h @@ -460,6 +460,39 @@ int32_t optical_enable_gesture(uint8_t port); */ int32_t optical_disable_gesture(uint8_t port); +/** + * Get integration time (update rate) of the optical sensor in milliseconds, with + * minimum time being + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Optical Sensor + * + * \param port + * The V5 Optical Sensor port number from 1-21 + * \return Integration time in milliseconds if the operation is successful + * or PROS_ERR if the operation failed, setting errno. + */ +double optical_get_integration_time(uint8_t port); + +/** + * Set integration time (update rate) of the optical sensor in milliseconds. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Optical Sensor + * + * \param port + * The V5 Optical Sensor port number from 1-21 + * \param time + * The desired integration time in milliseconds + * \return 1 if the operation is successful or PROS_ERR if the operation failed, + * setting errno. + */ +int32_t optical_set_integration_time(uint8_t port, double time); + ///@} ///@} diff --git a/include/pros/optical.hpp b/include/pros/optical.hpp index 463daa8a..b3e1d4e9 100644 --- a/include/pros/optical.hpp +++ b/include/pros/optical.hpp @@ -398,6 +398,36 @@ class Optical : public Device { */ virtual std::int32_t disable_gesture(); + /** + * Set integration time (update rate) of the optical sensor in milliseconds, with + * minimum time being 3 ms and maximum time being 712 ms. Default is 100 ms, with the + * optical sensor communciating with the V5 brain every 20 ms. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Optical Sensor + * + * \return 1 if the operation is successful or PROS_ERR_F if the operation failed, + * setting errno. + */ + double get_integration_time(); + + /** + * Get integration time (update rate) of the optical sensor in milliseconds. + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Optical Sensor + * + * \param time + * The desired integration time in milliseconds + * \return Integration time in milliseconds if the operation is successful + * or PROS_ERR if the operation failed, setting errno. + */ + std::int32_t set_integration_time(double time); + /** * This is the overload for the << operator for printing to streams * diff --git a/include/subsystems.hpp b/include/subsystems.hpp index 404e2418..294a3a4d 100644 --- a/include/subsystems.hpp +++ b/include/subsystems.hpp @@ -1,7 +1,10 @@ #pragma once +#include "EZ-Template/api.hpp" #include "api.h" +extern Drive chassis; + // Your motors, sensors, etc. should go here. Below are examples // inline pros::Motor intake(1); diff --git a/project.pros b/project.pros index a9016daf..f2653802 100644 --- a/project.pros +++ b/project.pros @@ -5,7 +5,7 @@ "target": "v5", "templates": { "kernel": { - "location": "C:\\Users\\xuzwi\\AppData\\Roaming\\PROS\\templates\\kernel@4.1.0", + "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\kernel@4.1.1", "metadata": { "cold_addr": "58720256", "cold_output": "bin/cold.package.bin", @@ -18,64 +18,66 @@ "py/object": "pros.conductor.templates.local_template.LocalTemplate", "supported_kernels": null, "system_files": [ - "include\\pros\\rtos.hpp", - "firmware\\libpros.a", - "include\\pros\\imu.hpp", + "include/pros/gps.hpp", + "include/pros/motors.h", + "include/pros/error.h", + "include/pros/imu.hpp", + "include/pros/serial.h", + "include/pros/rtos.h", + "include/pros/optical.hpp", + "firmware/libc.a", + "include/pros/distance.hpp", + "include/pros/rotation.h", "common.mk", - "include\\pros\\rotation.hpp", - "include\\pros\\abstract_motor.hpp", - "include\\pros\\serial.h", - "include\\pros\\vision.hpp", - "include\\pros\\colors.h", - "include\\pros\\colors.hpp", - "include\\pros\\device.hpp", - "include\\pros\\misc.h", - "include\\api.h", - "include\\pros\\link.h", - "firmware\\v5-hot.ld", - "include\\pros\\distance.h", - "firmware\\v5-common.ld", - "include\\pros\\imu.h", - "include\\pros\\motor_group.hpp", - "include\\pros\\vision.h", - "include\\pros\\misc.hpp", - "include\\pros\\adi.h", - "include\\pros\\screen.h", - "include\\pros\\motors.h", - "include\\pros\\llemu.h", - "include\\pros\\adi.hpp", - "include\\pros\\device.h", - "include\\pros\\motors.hpp", - "include\\pros\\serial.hpp", - "include\\pros\\ext_adi.h", - "include\\pros\\apix.h", - "include\\pros\\rtos.h", - "include\\pros\\rotation.h", - "include\\pros\\llemu.hpp", - "firmware\\libc.a", - "firmware\\v5.ld", - "include\\pros\\gps.h", - "include\\pros\\optical.hpp", - "include\\pros\\gps.hpp", - "firmware\\libm.a", - "include\\pros\\distance.hpp", - "include\\pros\\link.hpp", - "include\\pros\\optical.h", - "include\\pros\\screen.hpp", - "include\\pros\\error.h" + "include/pros/motors.hpp", + "firmware/v5.ld", + "include/pros/gps.h", + "include/pros/link.hpp", + "include/pros/colors.h", + "include/pros/imu.h", + "include/pros/adi.hpp", + "include/pros/ext_adi.h", + "include/pros/misc.hpp", + "include/pros/link.h", + "include/pros/device.h", + "include/pros/misc.h", + "firmware/v5-common.ld", + "include/pros/adi.h", + "include/pros/abstract_motor.hpp", + "include/api.h", + "firmware/v5-hot.ld", + "firmware/libm.a", + "include/pros/device.hpp", + "include/pros/apix.h", + "include/pros/llemu.h", + "include/pros/vision.hpp", + "include/pros/serial.hpp", + "include/pros/llemu.hpp", + "include/pros/colors.hpp", + "include/pros/rotation.hpp", + "include/pros/distance.h", + "include/pros/rtos.hpp", + "include/pros/vision.h", + "include/pros/screen.h", + "include/pros/optical.h", + "firmware/libpros.a", + "include/pros/motor_group.hpp", + "include/pros/screen.hpp" ], "target": "v5", "user_files": [ - "src\\main.c", - "include\\main.hh", + "src/main.cpp", + "src/main.c", "Makefile", - "include\\main.h", + "src/main.cc", + "include/main.h", ".gitignore", - "src\\main.cc", - "include\\main.hpp", - "src\\main.cpp" + "include/main.hh", + "include/main.hpp", + "src\\main.cpp", + "include\\main.h" ], - "version": "4.1.0" + "version": "4.1.1" }, "liblvgl": { "location": "C:\\Users\\xuzwi\\AppData\\Roaming\\PROS\\templates\\liblvgl@8.3.8", @@ -398,7 +400,7 @@ }, "upload_options": { "compress_bin": true, - "description": "making code ez", + "description": "roboticsisez.com", "remote_name": "EZ-Template", "slot": 1 }, diff --git a/src/EZ-Template/PID.cpp b/src/EZ-Template/PID.cpp index 686e071c..7a633e91 100644 --- a/src/EZ-Template/PID.cpp +++ b/src/EZ-Template/PID.cpp @@ -4,8 +4,8 @@ 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 "main.h" -#include "util.hpp" +#include "EZ-Template/api.hpp" +#include "api.h" using namespace ez; @@ -41,6 +41,12 @@ void PID::constants_set(double p, double i, double d, double p_start_i) { 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; @@ -58,8 +64,7 @@ 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) { - error = target - current; - return compute_error(error, current); + return compute_error(target - current, current); } double PID::compute_error(double err, double current) { @@ -244,4 +249,12 @@ exit_output PID::exit_condition(std::vector sensor, bool print) { } return exit_condition(print); +} + +exit_output PID::exit_condition(pros::MotorGroup sensor, bool print) { + std::vector 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); } \ No newline at end of file diff --git a/src/EZ-Template/auton.cpp b/src/EZ-Template/auton.cpp index c0a70f3e..75b50166 100644 --- a/src/EZ-Template/auton.cpp +++ b/src/EZ-Template/auton.cpp @@ -4,7 +4,7 @@ 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 "main.h" +#include "EZ-Template/api.hpp" Auton::Auton() { Name = ""; diff --git a/src/EZ-Template/auton_selector.cpp b/src/EZ-Template/auton_selector.cpp index a6dee423..ea0bc0c3 100644 --- a/src/EZ-Template/auton_selector.cpp +++ b/src/EZ-Template/auton_selector.cpp @@ -4,7 +4,7 @@ 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 "main.h" +#include "EZ-Template/api.hpp" AutonSelector::AutonSelector() { auton_count = 0; @@ -28,7 +28,7 @@ void AutonSelector::selected_auton_print() { void AutonSelector::selected_auton_call() { if (auton_count == 0) return; - Autons[auton_page_current].auton_call(); + Autons[last_auton_page_current].auton_call(); } void AutonSelector::autons_add(std::vector autons) { diff --git a/src/EZ-Template/drive/drive.cpp b/src/EZ-Template/drive/drive.cpp index d2a6809c..0c659d47 100644 --- a/src/EZ-Template/drive/drive.cpp +++ b/src/EZ-Template/drive/drive.cpp @@ -4,11 +4,9 @@ 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 "drive.hpp" - #include -#include "main.h" +#include "EZ-Template/api.hpp" #include "okapi/api/units/QAngle.hpp" #include "pros/llemu.hpp" #include "pros/screen.hpp" @@ -149,25 +147,44 @@ Drive::Drive(std::vector left_motor_ports, std::vector right_motor_por } void Drive::drive_defaults_set() { + imu.set_data_rate(5); + std::cout << std::fixed; std::cout << std::setprecision(2); // PID Constants - pid_heading_constants_set(11, 0, 20, 0); - pid_drive_constants_set(20, 0, 100, 0); - pid_turn_constants_set(3, 0.05, 20, 15); - pid_swing_constants_set(6, 0, 65); + 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_drive_constants_set(7_in, 80); + 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(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); - pid_swing_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); - pid_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 500_ms); + 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); @@ -188,11 +205,14 @@ void Drive::drive_defaults_set() { } 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 + 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); @@ -216,7 +236,7 @@ void Drive::private_drive_set(int left, int right) { } void Drive::drive_set(int left, int right) { - drive_mode_set(DISABLE); + drive_mode_set(DISABLE, false); private_drive_set(left, right); } @@ -245,8 +265,23 @@ int Drive::drive_current_limit_get() { // 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(); @@ -263,9 +298,15 @@ int Drive::drive_sensor_right_raw() { 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() { return drive_sensor_right_raw() / drive_tick_per_inch(); } +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(); } @@ -275,14 +316,24 @@ int Drive::drive_sensor_left_raw() { 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() { return drive_sensor_left_raw() / drive_tick_per_inch(); } +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); } +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; } @@ -293,34 +344,35 @@ 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; - // Boarder - int boarder = 50; + // Border + int border = 50; - // Create the boarder + // Create the border pros::screen::set_pen(pros::c::COLOR_WHITE); for (int i = 1; i < 3; i++) { - pros::screen::draw_rect(boarder + i, boarder + i, 480 - boarder - i, 240 - boarder - 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 = boarder; + static int last_x1 = border; pros::screen::set_pen(0x00FF6EC7); // EZ Pink - int x1 = (iter * ((480 - (boarder * 2)) / 2000.0)) + boarder; - pros::screen::fill_rect(last_x1, boarder, x1, 240 - boarder); + 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 = boarder; + static int last_x1 = border; pros::screen::set_pen(pros::c::COLOR_RED); - int x1 = ((iter - 2000) * ((480 - (boarder * 2)) / 1000.0)) + boarder; - pros::screen::fill_rect(last_x1, boarder, x1, 240 - boarder); + 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(); @@ -343,15 +395,24 @@ bool Drive::drive_imu_calibrate(bool run_loading_animation) { } 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; @@ -374,62 +435,43 @@ void Drive::initialize() { drive_sensor_reset(); } -void Drive::pid_drive_toggle(bool toggle) { drive_toggle = toggle; } -void Drive::pid_print_toggle(bool toggle) { print_toggle = toggle; } - -bool Drive::pid_drive_toggle_get() { return drive_toggle; } -bool Drive::pid_print_toggle_get() { return print_toggle; } - -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::odom_tracker_left_set(tracking_wheel* input) { + if (input == nullptr) return; -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); -} + odom_tracker_left = input; + odom_tracker_left_enabled = true; -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); -} + // Assume the user input a positive number and set it to a negative number + odom_tracker_left->distance_to_center_flip_set(true); -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); + // 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; -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); -} + odom_tracker_right = input; + odom_tracker_right_enabled = true; -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); + // 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; -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); + odom_tracker_front = input; + odom_tracker_front_enabled = true; } +void Drive::odom_tracker_back_set(tracking_wheel* input) { + if (input == nullptr) return; -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); -} + odom_tracker_back = input; + odom_tracker_back_enabled = true; -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); + // Set the center distance to be negative + odom_tracker_back->distance_to_center_flip_set(true); } - -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); -} \ No newline at end of file diff --git a/src/EZ-Template/drive/exit_conditions.cpp b/src/EZ-Template/drive/exit_conditions.cpp index 51747384..159dc689 100644 --- a/src/EZ-Template/drive/exit_conditions.cpp +++ b/src/EZ-Template/drive/exit_conditions.cpp @@ -4,8 +4,8 @@ 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 "main.h" using namespace ez; @@ -14,6 +14,8 @@ void Drive::pid_drive_exit_condition_set(int p_small_exit_time, double p_small_e 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) { @@ -62,6 +64,40 @@ void Drive::pid_swing_exit_condition_set(okapi::QTime p_small_exit_time, okapi:: 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 @@ -84,8 +120,45 @@ void Drive::pid_wait() { } } + // 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) { + 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()); @@ -120,7 +193,7 @@ void Drive::wait_until_drive(double target) { pros::delay(10); // Make sure mode is correct - if (!(mode == DRIVE)) { + if (!(mode == DRIVE || mode == POINT_TO_POINT || mode == PURE_PURSUIT)) { printf("Mode needs to be drive!\n"); return; } @@ -174,11 +247,14 @@ void Drive::wait_until_drive(double target) { // 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)) { + 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); @@ -192,7 +268,7 @@ void Drive::wait_until_turn_swing(double target) { g_error = target - drive_imu_get(); // If turning... - if (mode == TURN) { + 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) { @@ -247,7 +323,7 @@ void Drive::wait_until_turn_swing(double target) { void Drive::pid_wait_until(okapi::QLength target) { // If robot is driving... - if (mode == DRIVE) { + 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"); @@ -256,7 +332,7 @@ void Drive::pid_wait_until(okapi::QLength target) { void Drive::pid_wait_until(okapi::QAngle target) { // If robot is driving... - if (mode == TURN || mode == SWING) { + 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"); @@ -265,20 +341,101 @@ void Drive::pid_wait_until(okapi::QAngle target) { void Drive::pid_wait_until(double target) { // If driving... - if (mode == DRIVE) { + if (mode == DRIVE || mode == POINT_TO_POINT || mode == PURE_PURSUIT) { wait_until_drive(target); } // If turning or swinging... - else if (mode == TURN || mode == SWING) { + 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 == DRIVE || mode == TURN || mode == SWING)) { + 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; } @@ -329,18 +486,46 @@ void Drive::pid_wait_quick_chain() { 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 valid drive mode!\n"); + printf("Not in a supported drive mode!\n"); return; } diff --git a/src/EZ-Template/drive/pid_tasks.cpp b/src/EZ-Template/drive/pid_tasks.cpp index 2456051c..f2a888a7 100644 --- a/src/EZ-Template/drive/pid_tasks.cpp +++ b/src/EZ-Template/drive/pid_tasks.cpp @@ -4,33 +4,44 @@ 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/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: + 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(util::DELAY_TIME); + pros::delay(ez::util::DELAY_TIME); } } @@ -51,11 +62,11 @@ void Drive::drive_pid_task() { double r_drive_out = rightPID.output; // Scale leftPID and rightPID to slew (if slew is disabled, it returns max_speed) - double max_slew_out = fmin(slew_left.output(), slew_right.output()); + 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 = l_drive_out * (max_slew_out / faster_side); - r_drive_out = r_drive_out * (max_slew_out / faster_side); + l_drive_out *= (max_slew_out / faster_side); + r_drive_out *= (max_slew_out / faster_side); } // Toggle heading @@ -65,12 +76,12 @@ void Drive::drive_pid_task() { double l_out = l_drive_out + imu_out; double r_out = r_drive_out - imu_out; - // Vector scaling when combining drive and imo - max_slew_out = fmin(slew_left.output(), slew_right.output()); + // 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 = l_out * (max_slew_out / faster_side); - r_out = r_out * (max_slew_out / faster_side); + l_out *= (max_slew_out / faster_side); + r_out *= (max_slew_out / faster_side); } // Set motors @@ -80,8 +91,17 @@ void Drive::drive_pid_task() { // Turn PID task void Drive::turn_pid_task() { - // Compute PID - turnPID.compute(drive_imu_get()); + // 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()); @@ -133,4 +153,119 @@ void Drive::swing_pid_task() { 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(); + } } \ No newline at end of file diff --git a/src/EZ-Template/drive/pid_tuner.cpp b/src/EZ-Template/drive/pid_tuner.cpp index 092518f3..5b808810 100644 --- a/src/EZ-Template/drive/pid_tuner.cpp +++ b/src/EZ-Template/drive/pid_tuner.cpp @@ -4,14 +4,20 @@ 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/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; } @@ -45,17 +51,13 @@ bool Drive::pid_tuner_print_brain_enabled() { return pid_tuner_lcd_b; } // Enable PID Tuner void Drive::pid_tuner_enable() { - // Set the constants - constants = { - {"Drive Forward PID Constants", &forward_drivePID.constants}, - {"Drive Backward PID Constants", &backward_drivePID.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}}; - 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); @@ -88,18 +90,23 @@ void Drive::pid_tuner_toggle() { void Drive::pid_tuner_print() { if (!pid_tuner_on) return; - std::string name = constants[column].name + "\n"; - std::string kp = "kp: " + std::to_string(constants[column].consts->kp); - std::string ki = "ki: " + std::to_string(constants[column].consts->ki); - std::string kd = "kd: " + std::to_string(constants[column].consts->kd); - std::string starti = "start i: " + std::to_string(constants[column].consts->start_i); + 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)); - kp = row == 0 ? kp + arrow : kp + "\n"; - ki = row == 1 ? ki + arrow : ki + "\n"; - kd = row == 2 ? kd + arrow : kd + "\n"; - starti = row == 3 ? starti + arrow : starti + "\n"; + 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 = name + "\n" + kp + ki + kd + starti + "\n"; + complete_pid_tuner_output = sname + "\n" + skp + ski + skd + sstarti + "\n"; pid_tuner_print_brain(); pid_tuner_print_terminal(); @@ -121,16 +128,24 @@ void Drive::pid_tuner_value_modify(float p, float i, float d, float start) { switch (row) { case 0: - constants[column].consts->kp += p; + 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: - constants[column].consts->ki += i; + 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: - constants[column].consts->kd += d; + 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: - constants[column].consts->start_i += start; + 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; @@ -163,13 +178,13 @@ void Drive::pid_tuner_iterate() { // Up / Down for Rows if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_RIGHT)) { column++; - if (column > constants.size() - 1) + 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 = constants.size() - 1; + column = used_pid_tuner_pids->size() - 1; pid_tuner_print(); } diff --git a/src/EZ-Template/drive/pto.cpp b/src/EZ-Template/drive/pto.cpp index 6aa24fdc..40c11485 100644 --- a/src/EZ-Template/drive/pto.cpp +++ b/src/EZ-Template/drive/pto.cpp @@ -7,8 +7,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include #include -#include "drive.hpp" -#include "main.h" +#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()); diff --git a/src/EZ-Template/drive/purepursuit_math.cpp b/src/EZ-Template/drive/purepursuit_math.cpp new file mode 100644 index 00000000..60a34c06 --- /dev/null +++ b/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 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 Drive::inject_points(std::vector imovements) { + injected_pp_index.clear(); + bool first_point_added = false; + + // Create new vector that includes the starting point + std::vector 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 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 Drive::smooth_path(std::vector ipath, double weight_smooth, double weight_data, double tolerance) { + double path[500][3]; + double new_path[500][3]; + std::vector 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 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; +} diff --git a/src/EZ-Template/drive/set_pid.cpp b/src/EZ-Template/drive/set_pid.cpp deleted file mode 100644 index 8ed3d259..00000000 --- a/src/EZ-Template/drive/set_pid.cpp +++ /dev/null @@ -1,312 +0,0 @@ -/* -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/util.hpp" -#include "main.h" -#include "okapi/api/units/QAngle.hpp" - -// Set PID constants -void Drive::pid_drive_constants_set(double p, double i, double d, double p_start_i) { - pid_drive_constants_forward_set(p, i, d, p_start_i); - pid_drive_constants_backward_set(p, i, d, p_start_i); -} - -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; -} - -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); -} - -PID::Constants Drive::pid_drive_constants_forward_get() { - return forward_drivePID.constants_get(); -} - -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); -} - -PID::Constants Drive::pid_drive_constants_backward_get() { - return backward_drivePID.constants_get(); -} - -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_swing_constants_set(double p, double i, double d, double p_start_i) { - pid_swing_constants_forward_set(p, i, d, p_start_i); - pid_swing_constants_backward_set(p, i, d, p_start_i); -} - -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; -} - -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); -} - -PID::Constants Drive::pid_swing_constants_forward_get() { - return forward_swingPID.constants_get(); -} - -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_backward_get() { - return backward_swingPID.constants_get(); -} - -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); -} - -PID::Constants Drive::pid_heading_constants_get() { - return headingPID.constants_get(); -} - -// 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; -} - -void Drive::pid_targets_reset() { - headingPID.target_set(0); - leftPID.target_set(0); - rightPID.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_angle_set(double angle) { - headingPID.target_set(angle); - drive_imu_reset(angle); -} - -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); -} - -void Drive::drive_mode_set(e_mode p_mode) { mode = p_mode; } -e_mode Drive::drive_mode_get() { return mode; } - -void Drive::pid_turn_min_set(int min) { turn_min = abs(min); } -int Drive::pid_turn_min_get() { return turn_min; } - -void Drive::pid_swing_min_set(int min) { swing_min = abs(min); } -int Drive::pid_swing_min_get() { return swing_min; } - -// 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::Constants pid_consts; - 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) { - pid_consts = backward_drivePID.constants_get(); - slew_consts = slew_backward.constants_get(); - motion_chain_backward = true; - } else { - pid_consts = forward_drivePID.constants_get(); - slew_consts = slew_forward.constants_get(); - motion_chain_backward = false; - } - leftPID.constants_set(pid_consts.kp, pid_consts.ki, pid_consts.kd, pid_consts.start_i); - rightPID.constants_set(pid_consts.kp, pid_consts.ki, pid_consts.kd, pid_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()); - - // Run task - drive_mode_set(DRIVE); -} - -// 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); -} - -// Raw Set Turn PID -void Drive::pid_turn_set(double target, int speed, bool slew_on) { - turnPID.timers_reset(); - - // 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); - - // Run task - drive_mode_set(TURN); -} - -// Set turn PID -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, slew_on); -} - -// Set relative turn PID -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, slew_on); -} - -// Set relative turn PID -void Drive::pid_turn_relative_set(double target, int 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_turn_set(absolute_target, speed, slew_on); -} - -// Raw Set Swing PID -void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on) { - swingPID.timers_reset(); - - // Print targets - if (print_toggle) printf("Swing Started... Target Value: %.2f\n", target); - current_swing = type; - chain_sensor_start = drive_imu_get(); - chain_target_start = target; - used_motion_chain_scale = 0.0; - - // 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::Constants pid_consts; - PID::Constants pid_swing_consts; - slew::Constants slew_consts; - - if (direction == -1) { - pid_consts = backward_drivePID.constants_get(); - pid_swing_consts = backward_swingPID.constants_get(); - slew_consts = slew_swing_backward.constants_get(); - slew_swing_using_angle = slew_swing_rev_using_angle; - motion_chain_backward = true; - - } else { - pid_consts = forward_drivePID.constants_get(); - pid_swing_consts = forward_swingPID.constants_get(); - slew_consts = slew_swing_forward.constants_get(); - slew_swing_using_angle = slew_swing_fwd_using_angle; - motion_chain_backward = false; - } - - // Set targets for the side that isn't moving - swingPID.constants_set(pid_swing_consts.kp, pid_swing_consts.ki, pid_swing_consts.kd, pid_swing_consts.start_i); - leftPID.constants_set(pid_consts.kp, pid_consts.ki, pid_consts.kd, pid_consts.start_i); - rightPID.constants_set(pid_consts.kp, pid_consts.ki, pid_consts.kd, pid_consts.start_i); - leftPID.target_set(drive_sensor_left()); - rightPID.target_set(drive_sensor_right()); - slew_swing.constants_set(slew_consts.distance_to_travel, slew_consts.min_speed); - - // 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 slew_tar = slew_swing_using_angle ? target : direction * 100; - double current = slew_swing_using_angle ? chain_sensor_start : (current_swing == LEFT_SWING ? drive_sensor_left() : drive_sensor_right()); - slew_swing.initialize(slew_on, max_speed, slew_tar, current); - - // Run task - drive_mode_set(SWING); -} - -// Set swing PID -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); -} - -// Set relative swing PID -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); -} - -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, slew_on); -} \ No newline at end of file diff --git a/src/EZ-Template/drive/set_pid/set_drive_pid.cpp b/src/EZ-Template/drive/set_pid/set_drive_pid.cpp new file mode 100644 index 00000000..42b553df --- /dev/null +++ b/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); +} \ No newline at end of file diff --git a/src/EZ-Template/drive/set_pid/set_odom_pid.cpp b/src/EZ-Template/drive/set_pid/set_odom_pid.cpp new file mode 100644 index 00000000..10a002a0 --- /dev/null +++ b/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 Drive::set_odoms_direction(std::vector inputs) { + std::vector 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 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 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 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 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 p_imovements) { + std::vector imovements = util::united_odoms_to_odoms(p_imovements); + pid_odom_set(imovements); +} +void Drive::pid_odom_set(std::vector p_imovements, bool slew_on) { + std::vector 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 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 p_imovements) { + std::vector imovements = util::united_odoms_to_odoms(p_imovements); + pid_odom_pp_set(imovements); +} +void Drive::pid_odom_pp_set(std::vector p_imovements, bool slew_on) { + std::vector 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 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 imovements, bool slew_on) { + xyPID.timers_reset(); + current_a_odomPID.timers_reset(); + + if (print_toggle) printf("Injected "); + std::vector 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 p_imovements) { + std::vector imovements = util::united_odoms_to_odoms(p_imovements); + pid_odom_injected_pp_set(imovements); +} +void Drive::pid_odom_injected_pp_set(std::vector p_imovements, bool slew_on) { + std::vector 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 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 imovements, bool slew_on) { + xyPID.timers_reset(); + current_a_odomPID.timers_reset(); + + if (print_toggle) printf("Smooth Injected "); + std::vector 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 p_imovements) { + std::vector imovements = util::united_odoms_to_odoms(p_imovements); + pid_odom_smooth_pp_set(imovements); +} +void Drive::pid_odom_smooth_pp_set(std::vector p_imovements, bool slew_on) { + std::vector 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 imovements, bool slew_on) { + xyPID.timers_reset(); + current_a_odomPID.timers_reset(); + + std::vector 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 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; +} \ No newline at end of file diff --git a/src/EZ-Template/drive/set_pid/set_pid.cpp b/src/EZ-Template/drive/set_pid/set_pid.cpp new file mode 100644 index 00000000..633cdea3 --- /dev/null +++ b/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; } diff --git a/src/EZ-Template/drive/set_pid/set_swing_pid.cpp b/src/EZ-Template/drive/set_pid/set_swing_pid.cpp new file mode 100644 index 00000000..a5826d52 --- /dev/null +++ b/src/EZ-Template/drive/set_pid/set_swing_pid.cpp @@ -0,0 +1,349 @@ +/* +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); + current_swing = type; + chain_sensor_start = drive_imu_get(); + chain_target_start = target; + used_motion_chain_scale = 0.0; + + // 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); +} diff --git a/src/EZ-Template/drive/set_pid/set_turn_pid.cpp b/src/EZ-Template/drive/set_pid/set_turn_pid.cpp new file mode 100644 index 00000000..8a7a222a --- /dev/null +++ b/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); +} diff --git a/src/EZ-Template/drive/tracking.cpp b/src/EZ-Template/drive/tracking.cpp new file mode 100644 index 00000000..c5ed77f9 --- /dev/null +++ b/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 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 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 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 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); +} diff --git a/src/EZ-Template/drive/user_input.cpp b/src/EZ-Template/drive/user_input.cpp index f9761aab..bd4e9e6e 100644 --- a/src/EZ-Template/drive/user_input.cpp +++ b/src/EZ-Template/drive/user_input.cpp @@ -4,7 +4,12 @@ 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 "main.h" +#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) { @@ -167,12 +172,8 @@ void Drive::opcontrol_curve_buttons_iterate() { button_press(&r_decrease_, master.get_digital(r_decrease_.button), ([this] { this->r_decrease(); }), ([this] { this->save_r_curve_sd(); })); } - std::ostringstream short_sl, short_sr; - short_sl << std::fixed << std::setprecision(1) << left_curve_scale; - short_sr << std::fixed << std::setprecision(1) << right_curve_scale; - - auto sr = short_sr.str(); - auto sl = short_sl.str(); + 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 @@ -202,23 +203,30 @@ double Drive::opcontrol_curve_right(double x) { } // Set active brake constant -void Drive::opcontrol_drive_activebrake_set(double kp) { - active_brake_kp = kp; - drive_sensor_reset(); +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 constant -double Drive::opcontrol_drive_activebrake_get() { - return active_brake_kp; -} +// 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) { - drive_sensor_reset(); + opcontrol_drive_activebrake_targets_set(); util::AUTON_RAN = false; } } @@ -230,22 +238,54 @@ 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 (practice_mode_is_on && (abs(l_stick) > 120 || abs(r_stick) > 120)) - drive_set(0, 0); - else if (is_reversed == true) - drive_set(-r_stick, -l_stick); - else - drive_set(l_stick, r_stick); - if (active_brake_kp != 0) drive_sensor_reset(); + 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 { - drive_set((0 - drive_sensor_left()) * active_brake_kp, (0 - drive_sensor_right()) * active_brake_kp); + 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; } @@ -257,12 +297,12 @@ void Drive::opcontrol_tank() { // Toggle for controller curve opcontrol_curve_buttons_iterate(); - auto analog_left_value = master.get_analog(ANALOG_LEFT_Y); - auto analog_right_value = master.get_analog(ANALOG_RIGHT_Y); + 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(ANALOG_LEFT_Y))); - int r_stick = opcontrol_curve_left(clipped_joystick(master.get_analog(ANALOG_RIGHT_Y))); + 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); @@ -280,12 +320,12 @@ void Drive::opcontrol_arcade_standard(e_type stick_type) { // 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(ANALOG_LEFT_Y))); - turn_stick = opcontrol_curve_right(clipped_joystick(master.get_analog(ANALOG_RIGHT_X))); + 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(ANALOG_LEFT_Y))); - turn_stick = opcontrol_curve_right(clipped_joystick(master.get_analog(ANALOG_LEFT_X))); + 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 @@ -304,12 +344,12 @@ void Drive::opcontrol_arcade_flipped(e_type stick_type) { // 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(ANALOG_RIGHT_Y))); - turn_stick = opcontrol_curve_left(clipped_joystick(master.get_analog(ANALOG_LEFT_X))); + 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(ANALOG_RIGHT_Y))); - turn_stick = opcontrol_curve_left(clipped_joystick(master.get_analog(ANALOG_RIGHT_X))); + 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 diff --git a/src/EZ-Template/sdcard.cpp b/src/EZ-Template/sdcard.cpp index 9edea78e..63435cf8 100644 --- a/src/EZ-Template/sdcard.cpp +++ b/src/EZ-Template/sdcard.cpp @@ -9,6 +9,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include #include "auton_selector.hpp" +#include "liblvgl/llemu.hpp" #include "pros/llemu.hpp" #include "util.hpp" @@ -50,22 +51,69 @@ void auton_selector_initialize() { } } +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++; - auto_sd_update(); - auton_selector.selected_auton_print(); + 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--; - auto_sd_update(); - auton_selector.selected_auton_print(); + 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() { @@ -74,7 +122,7 @@ void initialize() { ez::as::auton_selector_initialize(); // Callbacks for auto selector - ez::as::auton_selector.selected_auton_print(); + print_page(); pros::lcd::register_btn0_cb(ez::as::page_down); pros::lcd::register_btn2_cb(ez::as::page_up); diff --git a/src/EZ-Template/tracking_wheel.cpp b/src/EZ-Template/tracking_wheel.cpp new file mode 100644 index 00000000..8ec56fad --- /dev/null +++ b/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 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 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; + } +} diff --git a/src/EZ-Template/util.cpp b/src/EZ-Template/util.cpp index bbc3e0e0..e56fcd1b 100644 --- a/src/EZ-Template/util.cpp +++ b/src/EZ-Template/util.cpp @@ -4,7 +4,10 @@ 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 "main.h" +#include "EZ-Template/api.hpp" +#include "liblvgl/llemu.hpp" +#include "pros/llemu.hpp" +#include "pros/misc.h" pros::Controller master(pros::E_CONTROLLER_MASTER); @@ -62,9 +65,9 @@ void screen_print(std::string text, int line) { texts.push_back(temp); temp = text[i]; } else { - int size = last_word.length(); + int size = last_word.length(); - auto rest_of_word = get_rest_of_the_word(text, i); + 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; @@ -74,7 +77,6 @@ void screen_print(std::string text, int line) { texts.push_back(temp); break; } - } } if (i >= text.length() - 1) { @@ -124,6 +126,27 @@ std::string exit_to_string(exit_output input) { 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; @@ -145,5 +168,118 @@ double clamp(double input, double max, double 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 united_odoms_to_odoms(std::vector inputs) { + std::vector 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 diff --git a/src/autons.cpp b/src/autons.cpp index 2ccdb230..548c4b8d 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -8,26 +8,44 @@ // These are out of 127 const int DRIVE_SPEED = 110; const int TURN_SPEED = 90; -const int SWING_SPEED = 90; +const int SWING_SPEED = 110; /// // Constants /// void default_constants() { - chassis.pid_heading_constants_set(11, 0, 20); - chassis.pid_drive_constants_set(20, 0, 100); - chassis.pid_turn_constants_set(3, 0.05, 20, 15); - chassis.pid_swing_constants_set(6, 0, 65); - - chassis.pid_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); - chassis.pid_swing_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); - chassis.pid_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 500_ms); - + // P, I, D, and Start I + chassis.pid_drive_constants_set(20.0, 0.0, 100.0); // Fwd/rev constants, used for odom and non odom motions + chassis.pid_heading_constants_set(11.0, 0.0, 20.0); // Holds the robot straight while going forward without odom + chassis.pid_turn_constants_set(3.0, 0.05, 20.0, 15.0); // Turn in place constants + chassis.pid_swing_constants_set(6.0, 0.0, 65.0); // Swing constants + chassis.pid_odom_angular_constants_set(6.5, 0.0, 52.5); // Angular control for odom motions + chassis.pid_odom_boomerang_constants_set(5.8, 0.0, 32.5); // Angular control for boomerang motions + + // Exit conditions + chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); + chassis.pid_swing_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 250_ms, 3_in, 500_ms, 500_ms); + chassis.pid_odom_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms); + chassis.pid_odom_drive_exit_condition_set(90_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms); chassis.pid_turn_chain_constant_set(3_deg); chassis.pid_swing_chain_constant_set(5_deg); chassis.pid_drive_chain_constant_set(3_in); - chassis.slew_drive_constants_set(7_in, 80); + // Slew constants + chassis.slew_turn_constants_set(3_deg, 70); + chassis.slew_drive_constants_set(3_in, 70); + chassis.slew_swing_constants_set(3_in, 80); + + // The amount that turns are prioritized over driving in odom motions + // - if you have tracking wheels, you can run this higher. 1.0 is the max + chassis.odom_turn_bias_set(0.9); + + chassis.odom_look_ahead_set(7_in); // This is how far ahead in the path the robot looks at + chassis.odom_boomerang_distance_set(16_in); // This sets the maximum distance away from target that the carrot point can be + chassis.odom_boomerang_dlead_set(0.625); // This handles how aggressive the end of boomerang motions are + + chassis.pid_angle_behavior_set(ez::shortest); // Changes the default behavior for turning, this defaults it to the shortest path there } /// @@ -218,6 +236,143 @@ void interfered_example() { chassis.pid_wait(); } +/// +// Odom Drive PID +/// +void odom_drive_example() { + // This works the same as pid_drive_set, but it uses odom instead! + // You can replace pid_drive_set with pid_odom_set and your robot will + // have better error correction. + + chassis.pid_odom_set(24_in, DRIVE_SPEED, true); + chassis.pid_wait(); + + chassis.pid_odom_set(-12_in, DRIVE_SPEED); + chassis.pid_wait(); + + chassis.pid_odom_set(-12_in, DRIVE_SPEED); + chassis.pid_wait(); +} + +/// +// Odom Pure Pursuit +/// +void odom_pure_pursuit_example() { + // Drive to 0, 30 and pass through 6, 10 and 0, 20 on the way, with slew + chassis.pid_odom_set({{{6_in, 10_in}, fwd, DRIVE_SPEED}, + {{0_in, 20_in}, fwd, DRIVE_SPEED}, + {{0_in, 30_in}, fwd, DRIVE_SPEED}}, + true); + chassis.pid_wait(); + + // Drive to 0, 0 backwards + chassis.pid_odom_set({{0_in, 0_in}, rev, DRIVE_SPEED}, + true); + chassis.pid_wait(); +} + +/// +// Odom Pure Pursuit Wait Until +/// +void odom_pure_pursuit_wait_until_example() { + chassis.pid_odom_set({{{0_in, 24_in}, fwd, DRIVE_SPEED}, + {{12_in, 24_in}, fwd, DRIVE_SPEED}, + {{24_in, 24_in}, fwd, DRIVE_SPEED}}, + true); + chassis.pid_wait_until_index(1); // Waits until the robot passes 12, 24 + // Intake.move(127); // Set your intake to start moving once it passes through the second point in the index + chassis.pid_wait(); + // Intake.move(0); // Turn the intake off +} + +/// +// Odom Boomerang +/// +void odom_boomerang_example() { + chassis.pid_odom_set({{0_in, 24_in, 45_deg}, fwd, DRIVE_SPEED}, + true); + chassis.pid_wait(); + + chassis.pid_odom_set({{0_in, 0_in, 0_deg}, rev, DRIVE_SPEED}, + true); + chassis.pid_wait(); +} + +/// +// Odom Boomerang Injected Pure Pursuit +/// +void odom_boomerang_injected_pure_pursuit_example() { + chassis.pid_odom_set({{{0_in, 24_in, 45_deg}, fwd, DRIVE_SPEED}, + {{12_in, 24_in}, fwd, DRIVE_SPEED}, + {{24_in, 24_in}, fwd, DRIVE_SPEED}}, + true); + chassis.pid_wait(); + + chassis.pid_odom_set({{0_in, 0_in, 0_deg}, rev, DRIVE_SPEED}, + true); + chassis.pid_wait(); +} + +/// +// Calculate the offsets of your tracking wheels +/// +void measure_offsets() { + // Number of times to test + int iterations = 10; + + // Our final offsets + double l_offset = 0.0, r_offset = 0.0, b_offset = 0.0, f_offset = 0.0; + + // Reset all trackers if they exist + if (chassis.odom_tracker_left != nullptr) chassis.odom_tracker_left->reset(); + if (chassis.odom_tracker_right != nullptr) chassis.odom_tracker_right->reset(); + if (chassis.odom_tracker_back != nullptr) chassis.odom_tracker_back->reset(); + if (chassis.odom_tracker_front != nullptr) chassis.odom_tracker_front->reset(); + + for (int i = 0; i < iterations; i++) { + // Reset pid targets and get ready for running an auton + chassis.pid_targets_reset(); + chassis.drive_imu_reset(); + chassis.drive_sensor_reset(); + chassis.drive_brake_set(MOTOR_BRAKE_HOLD); + chassis.odom_xyt_set(0_in, 0_in, 0_deg); + double imu_start = chassis.odom_theta_get(); + double target = i % 2 == 0 ? 90 : 270; // Switch the turn target every run from 270 to 90 + + // Turn to target at half power + chassis.pid_turn_set(target, 63, ez::raw); + chassis.pid_wait(); + pros::delay(250); + + // Calculate delta in angle + double t_delta = util::to_rad(fabs(util::wrap_angle(chassis.odom_theta_get() - imu_start))); + + // Calculate delta in sensor values that exist + double l_delta = chassis.odom_tracker_left != nullptr ? chassis.odom_tracker_left->get() : 0.0; + double r_delta = chassis.odom_tracker_right != nullptr ? chassis.odom_tracker_right->get() : 0.0; + double b_delta = chassis.odom_tracker_back != nullptr ? chassis.odom_tracker_back->get() : 0.0; + double f_delta = chassis.odom_tracker_front != nullptr ? chassis.odom_tracker_front->get() : 0.0; + + // Calculate the radius that the robot traveled + l_offset += l_delta / t_delta; + r_offset += r_delta / t_delta; + b_offset += b_delta / t_delta; + f_offset += f_delta / t_delta; + } + + // Average all offsets + l_offset /= iterations; + r_offset /= iterations; + b_offset /= iterations; + f_offset /= iterations; + + // Set new offsets to trackers that exist + if (chassis.odom_tracker_left != nullptr) chassis.odom_tracker_left->distance_to_center_set(l_offset); + if (chassis.odom_tracker_right != nullptr) chassis.odom_tracker_right->distance_to_center_set(r_offset); + if (chassis.odom_tracker_back != nullptr) chassis.odom_tracker_back->distance_to_center_set(b_offset); + if (chassis.odom_tracker_front != nullptr) chassis.odom_tracker_front->distance_to_center_set(f_offset); +} + // . . . // Make your own autonomous functions here! // . . . \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 162f2391..36291630 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -11,9 +11,16 @@ ez::Drive chassis( {-5, -6, -7, -8}, // Left Chassis Ports (negative port will reverse it!) {11, 15, 16, 17}, // Right Chassis Ports (negative port will reverse it!) - 21, // IMU Port - 4.125, // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!) - 420); // Wheel RPM + 21, // IMU Port + 4.125, // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!) + 420.0); // Wheel RPM = cartridge * (motor gear / wheel gear) + +// Are you using tracking wheels? Comment out which ones you're using here! +// `2.75` is the wheel diameter +// `4.0` is the distance from the center of the wheel to the center of the robot +// ez::tracking_wheel right_tracker({-'A', -'B'}, 2.75, 4.0); // ADI Encoders +// ez::tracking_wheel left_tracker(1, {'C', 'D'}, 2.75, 4.0); // ADI Encoders plugged into a Smart port +// ez::tracking_wheel horiz_tracker(1, 2.75, 4.0); // Rotation sensors /** * Runs initialization code. This occurs as soon as the program is started. @@ -27,10 +34,15 @@ void initialize() { pros::delay(500); // Stop the user from doing anything while legacy ports configure + // Are you using tracking wheels? Comment out which ones you're using here! + // chassis.odom_tracker_right_set(&right_tracker); + // chassis.odom_tracker_left_set(&left_tracker); + // chassis.odom_tracker_back_set(&horiz_tracker); // Replace `back` to `front` if your tracker is in the front! + // Configure your chassis controls - chassis.opcontrol_curve_buttons_toggle(true); // Enables modifying the controller curve with buttons on the joysticks - chassis.opcontrol_drive_activebrake_set(0); // Sets the active brake kP. We recommend ~2. 0 will disable. - chassis.opcontrol_curve_default_set(0, 0); // Defaults for curve. If using tank, only the first parameter is used. (Comment this line out if you have an SD card!) + chassis.opcontrol_curve_buttons_toggle(true); // Enables modifying the controller curve with buttons on the joysticks + chassis.opcontrol_drive_activebrake_set(0.0); // Sets the active brake kP. We recommend ~2. 0 will disable. + chassis.opcontrol_curve_default_set(0.0, 0.0); // Defaults for curve. If using tank, only the first parameter is used. (Comment this line out if you have an SD card!) // Set the drive to your own constants from autons.cpp! default_constants(); @@ -41,20 +53,26 @@ void initialize() { // Autonomous Selector using LLEMU ez::as::auton_selector.autons_add({ - Auton("Example Drive\n\nDrive forward and come back.", drive_example), - Auton("Example Turn\n\nTurn 3 times.", turn_example), - Auton("Drive and Turn\n\nDrive forward, turn, come back. ", drive_and_turn), - Auton("Drive and Turn\n\nSlow down during drive.", wait_until_change_speed), - Auton("Swing Example\n\nSwing in an 'S' curve", swing_example), - Auton("Motion Chaining\n\nDrive forward, turn, and come back, but blend everything together :D", motion_chaining), - Auton("Combine all 3 movements", combining_movements), - Auton("Interference\n\nAfter driving forward, robot performs differently if interfered or not.", interfered_example), + {"Drive\n\nDrive forward and come back", drive_example}, + {"Turn\n\nTurn 3 times.", turn_example}, + {"Drive and Turn\n\nDrive forward, turn, come back", drive_and_turn}, + {"Drive and Turn\n\nSlow down during drive", wait_until_change_speed}, + {"Swing Turn\n\nSwing in an 'S' curve", swing_example}, + {"Motion Chaining\n\nDrive forward, turn, and come back, but blend everything together :D", motion_chaining}, + {"Combine all 3 movements", combining_movements}, + {"Interference\n\nAfter driving forward, robot performs differently if interfered or not", interfered_example}, + {"Simple Odom\n\nThis is the same as the drive example, but it uses odom instead!", odom_drive_example}, + {"Pure Pursuit\n\nGo to (0, 30) and pass through (6, 10) on the way. Come back to (0, 0)", odom_pure_pursuit_example}, + {"Pure Pursuit Wait Until\n\nGo to (24, 24) but start running an intake once the robot passes (12, 24)", odom_pure_pursuit_wait_until_example}, + {"Boomerang\n\nGo to (0, 24, 45) then come back to (0, 0, 0)", odom_boomerang_example}, + {"Boomerang Pure Pursuit\n\nGo to (0, 24, 45) on the way to (24, 24) then come back to (0, 0, 0)", odom_boomerang_injected_pure_pursuit_example}, + {"Measure Offsets\n\nThis will turn the robot a bunch of times and calculate your offsets for your tracking wheels.", odom_boomerang_injected_pure_pursuit_example}, }); // Initialize chassis and auton selector chassis.initialize(); ez::as::initialize(); - master.rumble("."); + master.rumble(chassis.drive_imu_calibrated() ? "." : "---"); } /** @@ -94,11 +112,115 @@ void autonomous() { chassis.pid_targets_reset(); // Resets PID targets to 0 chassis.drive_imu_reset(); // Reset gyro position to 0 chassis.drive_sensor_reset(); // Reset drive sensors to 0 + chassis.odom_xyt_set(0_in, 0_in, 0_deg); // Set the current position, you can start at a specific position with this chassis.drive_brake_set(MOTOR_BRAKE_HOLD); // Set motors to hold. This helps autonomous consistency + /* + Odometry and Pure Pursuit are not magic + + It is possible to get perfectly consistent results without tracking wheels, + but it is also possible to have extremely inconsistent results without tracking wheels. + When you don't use tracking wheels, you need to: + - avoid wheel slip + - avoid wheelies + - avoid throwing momentum around (super harsh turns, like in the example below) + You can do cool curved motions, but you have to give your robot the best chance + to be consistent + */ + ez::as::auton_selector.selected_auton_call(); // Calls selected auton from autonomous selector } +/** + * Simplifies printing tracker values to the brain screen + */ +void screen_print_tracker(ez::tracking_wheel *tracker, std::string name, int line) { + std::string tracker_value = "", tracker_width = ""; + // Check if the tracker exists + if (tracker != nullptr) { + tracker_value = name + " tracker: " + util::to_string_with_precision(tracker->get()); // Make text for the tracker value + tracker_width = " width: " + util::to_string_with_precision(tracker->distance_to_center_get()); // Make text for the distance to center + } + ez::screen_print(tracker_value + tracker_width, line); // Print final tracker text +} + +/** + * Ez screen task + * Adding new pages here will let you view them during user control or autonomous + * and will help you debug problems you're having + */ +void ez_screen_task() { + while (true) { + // Only run this when not connected to a competition switch + if (!pros::competition::is_connected()) { + // Blank page for odom debugging + if (chassis.odom_enabled() && !chassis.pid_tuner_enabled()) { + // If we're on the first blank page... + if (ez::as::page_blank_is_on(0)) { + // Display X, Y, and Theta + ez::screen_print("x: " + util::to_string_with_precision(chassis.odom_x_get()) + + "\ny: " + util::to_string_with_precision(chassis.odom_y_get()) + + "\na: " + util::to_string_with_precision(chassis.odom_theta_get()), + 1); // Don't override the top Page line + + // Display all trackers that are being used + screen_print_tracker(chassis.odom_tracker_left, "l", 4); + screen_print_tracker(chassis.odom_tracker_right, "r", 5); + screen_print_tracker(chassis.odom_tracker_back, "b", 6); + screen_print_tracker(chassis.odom_tracker_front, "f", 7); + } + } + } + + // Remove all blank pages when connected to a comp switch + else { + if (ez::as::page_blank_amount() > 0) + ez::as::page_blank_remove_all(); + } + + pros::delay(ez::util::DELAY_TIME); + } +} +pros::Task ezScreenTask(ez_screen_task); + +/** + * Gives you some extras to run in your opcontrol: + * - run your autonomous routine in opcontrol by pressing DOWN and B + * - to prevent this from accidentally happening at a competition, this + * is only enabled when you're not connected to competition control. + * - gives you a GUI to change your PID values live by pressing X + */ +void ez_template_extras() { + // Only run this when not connected to a competition switch + if (!pros::competition::is_connected()) { + // PID Tuner + // - after you find values that you're happy with, you'll have to set them in auton.cpp + + // Enable / Disable PID Tuner + // When enabled: + // * use A and Y to increment / decrement the constants + // * use the arrow keys to navigate the constants + if (master.get_digital_new_press(DIGITAL_X)) + chassis.pid_tuner_toggle(); + + // Trigger the selected autonomous routine + if (master.get_digital(DIGITAL_B) && master.get_digital(DIGITAL_DOWN)) { + pros::motor_brake_mode_e_t preference = chassis.drive_brake_get(); + autonomous(); + chassis.drive_brake_set(preference); + } + + // Allow PID Tuner to iterate + chassis.pid_tuner_iterate(); + } + + // Disable PID Tuner when connected to a comp switch + else { + if (chassis.pid_tuner_enabled()) + chassis.pid_tuner_disable(); + } +} + /** * Runs the operator control code. This function will be started in its own task * with the default priority and stack size whenever the robot is enabled via @@ -114,32 +236,14 @@ void autonomous() { */ void opcontrol() { // This is preference to what you like to drive on - pros::motor_brake_mode_e_t driver_preference_brake = MOTOR_BRAKE_COAST; - - chassis.drive_brake_set(driver_preference_brake); + chassis.drive_brake_set(MOTOR_BRAKE_COAST); while (true) { - // PID Tuner - // After you find values that you're happy with, you'll have to set them in auton.cpp - if (!pros::competition::is_connected()) { - // Enable / Disable PID Tuner - // When enabled: - // * use A and Y to increment / decrement the constants - // * use the arrow keys to navigate the constants - if (master.get_digital_new_press(DIGITAL_X)) - chassis.pid_tuner_toggle(); - - // Trigger the selected autonomous routine - if (master.get_digital(DIGITAL_B) && master.get_digital(DIGITAL_DOWN)) { - autonomous(); - chassis.drive_brake_set(driver_preference_brake); - } + // Gives you some extras to make EZ-Template ezier + ez_template_extras(); - chassis.pid_tuner_iterate(); // Allow PID Tuner to iterate - } - - // chassis.opcontrol_tank(); // Tank control - chassis.opcontrol_arcade_standard(ez::SPLIT); // Standard split arcade + chassis.opcontrol_tank(); // Tank control + // chassis.opcontrol_arcade_standard(ez::SPLIT); // Standard split arcade // chassis.opcontrol_arcade_standard(ez::SINGLE); // Standard single arcade // chassis.opcontrol_arcade_flipped(ez::SPLIT); // Flipped split arcade // chassis.opcontrol_arcade_flipped(ez::SINGLE); // Flipped single arcade @@ -150,4 +254,4 @@ void opcontrol() { pros::delay(ez::util::DELAY_TIME); // This is used for timer calculations! Keep this ez::util::DELAY_TIME } -} \ No newline at end of file +} diff --git a/version b/version index a0cd9f0c..a4f52a5d 100644 --- a/version +++ b/version @@ -1 +1 @@ -3.1.0 \ No newline at end of file +3.2.0 \ No newline at end of file