From 65efa0b7afd948dd1ae2462019eb582dade2d39b Mon Sep 17 00:00:00 2001 From: Jess Zarchi Date: Wed, 7 Feb 2024 11:02:55 -0800 Subject: [PATCH] Fixed small bugs, refactoring #89, #90, #91, #92) --- include/EZ-Template/drive/drive.hpp | 25 ++++++++----------- src/EZ-Template/PID.cpp | 2 +- src/EZ-Template/drive/exit_conditions.cpp | 10 ++++---- src/EZ-Template/drive/pid_tasks.cpp | 28 ++++++++++++---------- src/EZ-Template/drive/pid_tuner.cpp | 29 +++++++++++------------ src/EZ-Template/drive/set_pid.cpp | 2 -- src/EZ-Template/slew.cpp | 2 +- src/main.cpp | 3 +++ 8 files changed, 49 insertions(+), 52 deletions(-) diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index 6b122d3c..0fddb2eb 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/include/EZ-Template/drive/drive.hpp @@ -722,7 +722,7 @@ class Drive { * \param toggle_heading * toggle for heading correction */ - void pid_drive_set(double target, int speed, bool slew_on, bool toggle_heading = true); + void pid_drive_set(double target, int speed, bool slew_on = false, bool toggle_heading = true); /** * Sets the robot to turn using PID. @@ -782,7 +782,7 @@ class Drive { * \param speed * 0 to 127, max speed during motion * \param opposite_speed - * 0 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); @@ -796,7 +796,7 @@ class Drive { * \param speed * 0 to 127, max speed during motion * \param opposite_speed - * 0 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); @@ -810,7 +810,7 @@ class Drive { * \param speed * 0 to 127, max speed during motion * \param opposite_speed - * 0 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); @@ -824,7 +824,7 @@ class Drive { * \param speed * 0 to 127, max speed during motion * \param opposite_speed - * 0 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); @@ -1305,14 +1305,13 @@ 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(double p, double i, double d, double start); + void pid_tuner_value_modify(float p, float i, float d, float start); void pid_tuner_value_increase(); void pid_tuner_value_decrease(); void pid_tuner_print_brain(); @@ -1320,16 +1319,12 @@ class Drive { void pid_tuner_brain_init(); int column = 0; int row = 0; - int column_max = 0; - const int row_max = 3; - std::string name, kp, ki, kd, starti; std::string arrow = " <--\n"; - std::string newline = "\n"; - bool last_controller_curve_state; - bool last_auton_selector_state; + bool last_controller_curve_state = false; + bool last_auton_selector_state = false; bool pid_tuner_on = false; - std::string complete_pid_tuner_output; - double p_increment = 0.1, i_increment = 0.001, d_increment = 0.25, start_i_increment = 1.0; + std::string complete_pid_tuner_output = ""; + float p_increment = 0.1, i_increment = 0.001, d_increment = 0.25, start_i_increment = 1.0; /** * Private wait until for drive diff --git a/src/EZ-Template/PID.cpp b/src/EZ-Template/PID.cpp index dcc9e440..977c85a6 100644 --- a/src/EZ-Template/PID.cpp +++ b/src/EZ-Template/PID.cpp @@ -99,7 +99,7 @@ void PID::exit_condition_print(ez::exit_output exit_type) { exit_output PID::exit_condition(bool print) { // If this function is called while all exit constants are 0, print an error - if (!(exit.small_error && exit.small_exit_time && exit.big_error && exit.big_exit_time && exit.velocity_exit_time && exit.mA_timeout)) { + if (exit.small_error == 0 && exit.small_exit_time == 0 && exit.big_error == 0 && exit.big_exit_time == 0 && exit.velocity_exit_time == 0 && exit.mA_timeout == 0) { exit_condition_print(ERROR_NO_CONSTANTS); return ERROR_NO_CONSTANTS; } diff --git a/src/EZ-Template/drive/exit_conditions.cpp b/src/EZ-Template/drive/exit_conditions.cpp index 6297b7ab..460f8229 100644 --- a/src/EZ-Template/drive/exit_conditions.cpp +++ b/src/EZ-Template/drive/exit_conditions.cpp @@ -118,10 +118,10 @@ void Drive::wait_until_drive(double target) { } // Calculate error between current and target (target needs to be an in between position) - int l_tar = l_start + target; - int r_tar = r_start + target; - int l_error = l_tar - drive_sensor_left(); - int r_error = r_tar - drive_sensor_right(); + double l_tar = l_start + target; + double r_tar = r_start + target; + double l_error = l_tar - drive_sensor_left(); + double r_error = r_tar - drive_sensor_right(); int l_sgn = util::sgn(l_error); int r_sgn = util::sgn(r_error); @@ -166,7 +166,7 @@ void Drive::wait_until_turn_swing(double target) { } // Calculate error between current and target (target needs to be an in between position) - int g_error = target - drive_imu_get(); + double g_error = target - drive_imu_get(); int g_sgn = util::sgn(g_error); exit_output turn_exit = RUNNING; diff --git a/src/EZ-Template/drive/pid_tasks.cpp b/src/EZ-Template/drive/pid_tasks.cpp index 17267a16..a9ecbdee 100644 --- a/src/EZ-Template/drive/pid_tasks.cpp +++ b/src/EZ-Template/drive/pid_tasks.cpp @@ -12,22 +12,24 @@ using namespace ez; void Drive::ez_auto_task() { while (true) { // Autonomous PID - if (drive_mode_get() == DRIVE) - drive_pid_task(); - else if (drive_mode_get() == TURN) - turn_pid_task(); - else if (drive_mode_get() == SWING) - swing_pid_task(); + switch (drive_mode_get()) { + case DRIVE: + drive_pid_task(); + break; + case TURN: + turn_pid_task(); + break; + case SWING: + swing_pid_task(); + break; + case DISABLE: + break; + default: + break; + } util::AUTON_RAN = drive_mode_get() != DISABLE ? true : false; - /* - if (pros::competition::is_autonomous() && !util::AUTON_RAN) - util::AUTON_RAN = true; - else if (!pros::competition::is_autonomous()) - drive_mode_set(DISABLE); - */ - pros::delay(util::DELAY_TIME); } } diff --git a/src/EZ-Template/drive/pid_tuner.cpp b/src/EZ-Template/drive/pid_tuner.cpp index 6da71160..77fa4f44 100644 --- a/src/EZ-Template/drive/pid_tuner.cpp +++ b/src/EZ-Template/drive/pid_tuner.cpp @@ -52,7 +52,6 @@ void Drive::pid_tuner_enable() { {"Turn PID Constants", &turnPID.constants}, {"Swing Forward PID Constants", &forward_swingPID.constants}, {"Swing Backward PID Constants", &backward_swingPID.constants}}; - column_max = constants.size() - 1; pid_tuner_brain_init(); @@ -88,16 +87,16 @@ void Drive::pid_tuner_toggle() { void Drive::pid_tuner_print() { if (!pid_tuner_on) return; - name = constants[column].name + "\n"; - kp = "kp: " + std::to_string(constants[column].consts->kp); - ki = "ki: " + std::to_string(constants[column].consts->ki); - kd = "kd: " + std::to_string(constants[column].consts->kd); - starti = "start i: " + std::to_string(constants[column].consts->start_i); + 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); - kp = row == 0 ? kp + arrow : kp + newline; - ki = row == 1 ? ki + arrow : ki + newline; - kd = row == 2 ? kd + arrow : kd + newline; - starti = row == 3 ? starti + arrow : starti + newline; + 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"; complete_pid_tuner_output = name + "\n" + kp + ki + kd + starti + "\n"; @@ -116,7 +115,7 @@ void Drive::pid_tuner_print_terminal() { } // Modify constants -void Drive::pid_tuner_value_modify(double p, double i, double d, double start) { +void Drive::pid_tuner_value_modify(float p, float i, float d, float start) { if (!pid_tuner_on) return; switch (row) { @@ -163,26 +162,26 @@ void Drive::pid_tuner_iterate() { // Up / Down for Rows if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_RIGHT)) { column++; - if (column > column_max) + if (column > constants.size() - 1) column = 0; pid_tuner_print(); } else if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_LEFT)) { column--; if (column < 0) - column = column_max; + column = constants.size() - 1; pid_tuner_print(); } // Left / Right for Columns if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_DOWN)) { row++; - if (row > row_max) + if (row > 3) row = 0; pid_tuner_print(); } else if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_UP)) { row--; if (row < 0) - row = row_max; + row = 3; pid_tuner_print(); } diff --git a/src/EZ-Template/drive/set_pid.cpp b/src/EZ-Template/drive/set_pid.cpp index 7195d1f9..524df281 100644 --- a/src/EZ-Template/drive/set_pid.cpp +++ b/src/EZ-Template/drive/set_pid.cpp @@ -194,8 +194,6 @@ void Drive::pid_turn_set(double target, int speed, bool slew_on) { // Initialize slew slew_turn.initialize(slew_on, max_speed, target, drive_imu_get()); - printf("%.2f %.2f\n\n", slew_turn.constants.distance_to_travel, slew_turn.constants.min_speed); - // Run task drive_mode_set(TURN); } diff --git a/src/EZ-Template/slew.cpp b/src/EZ-Template/slew.cpp index 079e3052..f75debed 100644 --- a/src/EZ-Template/slew.cpp +++ b/src/EZ-Template/slew.cpp @@ -26,7 +26,7 @@ slew::Constants slew::constants_get() { return constants; } // Get constants // Initialize for the movement void slew::initialize(bool enabled, double maximum_speed, double target, double current) { - is_enabled = enabled; + is_enabled = maximum_speed < constants.min_speed ? false : enabled; max_speed = maximum_speed; sign = util::sgn(target - current); diff --git a/src/main.cpp b/src/main.cpp index 5d60753c..5aa501bd 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -145,6 +145,9 @@ void opcontrol() { // 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();