From 3de6367de7a3d8d51d17ac5ffeb9ce9bc80782a9 Mon Sep 17 00:00:00 2001 From: Jess Zarchi Date: Thu, 6 Jan 2022 11:52:26 -0800 Subject: [PATCH] New exit conditions implemented into chassis (#33) Still needs wiki page for #33. Fixed issue with #12 Fixed issue with #35 --- docs/Docs/auton_functions.md | 30 --- include/EZ-Template/PID.hpp | 103 ++++++++- include/EZ-Template/drive/drive.hpp | 50 ++--- include/EZ-Template/util.hpp | 8 + src/EZ-Template/PID.cpp | 40 +++- src/EZ-Template/drive/drive.cpp | 18 +- src/EZ-Template/drive/exit_conditions.cpp | 254 ++++++++++------------ src/EZ-Template/drive/slew.cpp | 2 +- src/EZ-Template/util.cpp | 22 +- src/autons.cpp | 25 ++- src/main.cpp | 27 +-- 11 files changed, 311 insertions(+), 268 deletions(-) diff --git a/docs/Docs/auton_functions.md b/docs/Docs/auton_functions.md index e0b83d3e..f5e008dd 100644 --- a/docs/Docs/auton_functions.md +++ b/docs/Docs/auton_functions.md @@ -202,36 +202,6 @@ void autonomous() { --- -## exit_condition() -Iterative bool that checks if the robot has settled. This is used in `wait_drive()` and `wait_until()`. -`targets` is a tuple of PID objects. -`exitConditions` is either `swing_exit`, `drive_exit`, or `turn_exit`. -`wait_until` is a bool that changes the printfs. -**Prototype** -```cpp -bool exit_condition(std::tuple> targets, exit_condition_ exitConditions, bool wait_until = false); -``` - -**Example** -```cpp -void autonomous() { - chassis.set_drive_pid(24, DRIVE_SPEED, true); - while (chassis.exit_condition(tuple{chassis.leftPID.get_target(), chassis.rightPID.get_target()}, chassis.drive_exit)) { - printf("Left Error: %f Right Error: %f\n", chassis.leftPID.error, chassis.rightPID.error); - pros::delay(ez::util::DELAY_TIME); - } - - chassis.set_turn_pid(90, TURN_SPEED); - while (chassis.exit_condition(tuple{chassis.swingPID.get_target(), std::nullopt}, chassis.swing_exit)) { - printf("Turn Error: %f", chassis.turnPID.error); - pros::delay(util::DELAY_TIME); - } -} -``` - - ---- - ## set_max_speed() Sets the max speed of the drive. diff --git a/include/EZ-Template/PID.hpp b/include/EZ-Template/PID.hpp index 8f60b966..67c39157 100644 --- a/include/EZ-Template/PID.hpp +++ b/include/EZ-Template/PID.hpp @@ -11,12 +11,39 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. class PID { public: + /** + * Default constructor. + */ + PID(); + + /** + * Constructor with constants. + * + * \param p + * kP + * \param i + * ki + * \param d + * kD + * \param p_start_i + * error value that i starts within + */ + PID(double p, double i, double d, double start_i = 0); + void set_constants(double p, double i, double d, double p_start_i = 0); + + /** + * Struct for constants. + */ struct Constants { double kp; double ki; double kd; double start_i; }; + + /** + * Struct for exit condition. + */ struct exit_condition_ { int small_exit_time = 0; double small_error = 0; @@ -26,9 +53,6 @@ class PID { int mA_timeout = 0; }; - PID(); - PID(double p, double i, double d, double start_i = 0); - void set_constants(double p, double i, double d, double p_start_i = 0); /** * Set's constants for exit conditions. * @@ -45,22 +69,80 @@ class PID { */ void set_exit_condition(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); - int i = 0, j = 0, k = 0, l = 0; - bool is_mA = false; - void reset_timers(); - + /** + * Set's target. + * + * \param target + * Target for PID. + */ void set_target(double input); + + /** + * Computes PID. + * + * \param current + * Current sensor library. + */ double compute(double current); + + /** + * Returns target value. + */ double get_target(); + + /** + * Returns constants. + */ Constants get_constants(); + + /** + * Resets all variables to 0. This does not reset constants. + */ void reset_variables(); + + /** + * Constants + */ Constants constants; + + /** + * Exit + */ exit_condition_ exit; - ez::exit_output exit_condition(std::vector, bool print = false); + /** + * Iterative exit condition for PID. + * + * \param print = false + * if true, prints when complete. + */ ez::exit_output exit_condition(bool print = false); + /** + * Iterative exit condition for PID. + * + * \param sensor + * A pros motor on your mechanism. + * \param print = false + * if true, prints when complete. + */ + ez::exit_output exit_condition(pros::Motor sensor, bool print = false); + + /** + * Iterative exit condition for PID. + * + * \param sensor + * Pros motors on your mechanism. + * \param print = false + * if true, prints when complete. + */ + ez::exit_output exit_condition(std::vector sensor, bool print = false); + + /** + * PID variables. + */ double output; + double cur; double error; double target; double prev_error; @@ -68,4 +150,9 @@ class PID { double derivative; long time; long prev_time; + + private: + int i = 0, j = 0, k = 0, l = 0; + bool is_mA = false; + void reset_timers(); }; diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index f2898034..cad24949 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/include/EZ-Template/drive/drive.hpp @@ -534,33 +534,6 @@ class Drive { */ void set_slew_distance(int fwd, int rev); - /** - * Exit condition struct. - */ - struct exit_condition_ { - int small_exit_time = 0; - double small_error = 0; - int big_exit_time = 0; - double big_error = 0; - int velocity_exit_time = 0; - int mA_timeout = 0; - }; - - /** - * Exit condition for turning. - */ - exit_condition_ turn_exit; - - /** - * Exit condition for swinging. - */ - exit_condition_ swing_exit; - - /** - * Exit condition for driving. - */ - exit_condition_ drive_exit; - /** * Set's constants for exit conditions. * @@ -577,19 +550,22 @@ class Drive { * \param p_velocity_exit_time * Sets velocity_exit_time. Timer will start when velocity is 0. */ - void set_exit_condition(exit_condition_ &type, 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); + void set_exit_condition(int type, 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); /** - * Iterative exit condition. - * - * \param targets - * leftPID, rightPID, turnPID or swingPID - * \param exitConditions - * turn_exit, swing_exit, or drive_exit - * \param wait_until = false - * changes print statements + * Exit condition for turning. + */ + const int turn_exit = 1; + + /** + * Exit condition for swinging. + */ + const int swing_exit = 2; + + /** + * Exit condition for driving. */ - bool exit_condition(std::tuple> targets, exit_condition_ exitConditions, bool wait_until = false); + const int drive_exit = 3; private: // !Auton diff --git a/include/EZ-Template/util.hpp b/include/EZ-Template/util.hpp index 903094c3..85a4f231 100644 --- a/include/EZ-Template/util.hpp +++ b/include/EZ-Template/util.hpp @@ -69,6 +69,11 @@ enum exit_output { RUNNING = 1, VELOCITY_EXIT = 4, mA_EXIT = 5 }; +/** + * Outputs string for exit_condition enum. + */ +std::string exit_to_string(exit_output input); + namespace util { extern bool AUTON_RAN; @@ -76,14 +81,17 @@ extern bool AUTON_RAN; * Returns 1 if input is positive and -1 if input is negative */ int sgn(double input); + /** * Returns true if the input is < 0 */ bool is_reversed(double input); + /** * Returns input restricted to min-max threshold */ double clip_num(double input, double min, double max); + /** * Is the SD card plugged in? */ diff --git a/src/EZ-Template/PID.cpp b/src/EZ-Template/PID.cpp index 3965ad95..e809c80b 100644 --- a/src/EZ-Template/PID.cpp +++ b/src/EZ-Template/PID.cpp @@ -82,16 +82,18 @@ void PID::reset_timers() { exit_output PID::exit_condition(bool print) { // If the robot gets within the target, make sure it's there for small_timeout amount of time - if (abs(error) < exit.small_error) { - j += util::DELAY_TIME; - i = 0; // While this is running, don't run big thresh - if (j > exit.small_exit_time) { - reset_timers(); - if (print) printf(" Small Exit\n"); - return SMALL_EXIT; + if (exit.small_error != 0) { + if (abs(error) < exit.small_error) { + j += util::DELAY_TIME; + i = 0; // While this is running, don't run big thresh + if (j > exit.small_exit_time) { + reset_timers(); + if (print) printf(" Small Exit\n"); + return SMALL_EXIT; + } + } else { + j = 0; } - } else { - j = 0; } // If the robot is close to the target, start a timer. If the robot doesn't get closer within @@ -111,7 +113,7 @@ exit_output PID::exit_condition(bool print) { // If the motor velocity is 0,the code will timeout and set interfered to true. if (exit.velocity_exit_time != 0) { // Check if this condition is enabled - if (derivative == 0) { + if (abs(derivative) <= 0.05) { k += util::DELAY_TIME; if (k > exit.velocity_exit_time) { reset_timers(); @@ -126,6 +128,24 @@ exit_output PID::exit_condition(bool print) { return RUNNING; } +exit_output PID::exit_condition(pros::Motor sensor, bool print) { + // If the motors are pulling too many mA, the code will timeout and set interfered to true. + if (exit.mA_timeout != 0) { // Check if this condition is enabled + if (sensor.is_over_current()) { + l += util::DELAY_TIME; + if (l > exit.mA_timeout) { + reset_timers(); + if (print) printf(" mA Exit\n"); + return mA_EXIT; + } + } else { + l = 0; + } + } + + return exit_condition(print); +} + exit_output PID::exit_condition(std::vector sensor, bool print) { // If the motors are pulling too many mA, the code will timeout and set interfered to true. if (exit.mA_timeout != 0) { // Check if this condition is enabled diff --git a/src/EZ-Template/drive/drive.cpp b/src/EZ-Template/drive/drive.cpp index b331d922..7dff074a 100644 --- a/src/EZ-Template/drive/drive.cpp +++ b/src/EZ-Template/drive/drive.cpp @@ -186,12 +186,10 @@ void Drive::set_tank(int left, int right) { if (pros::millis() < 1500) return; for (auto i : left_motors) { - if (pto_check(i)) break; // If the motor is in the pto list, don't do anything to the motor. - i.move_voltage(left * (12000.0 / 127.0)); + if (!pto_check(i)) i.move_voltage(left * (12000.0 / 127.0)); // If the motor is in the pto list, don't do anything to the motor. } for (auto i : right_motors) { - if (pto_check(i)) break; // If the motor is in the pto list, don't do anything to the motor. - i.move_voltage(right * (12000.0 / 127.0)); + if (!pto_check(i)) i.move_voltage(right * (12000.0 / 127.0)); // If the motor is in the pto list, don't do anything to the motor. } } @@ -201,12 +199,10 @@ void Drive::set_drive_current_limit(int mA) { } CURRENT_MA = mA; for (auto i : left_motors) { - if (pto_check(i)) break; // If the motor is in the pto list, don't do anything to the motor. - i.set_current_limit(abs(mA)); + if (!pto_check(i)) i.set_current_limit(abs(mA)); // If the motor is in the pto list, don't do anything to the motor. } for (auto i : right_motors) { - if (pto_check(i)) break; // If the motor is in the pto list, don't do anything to the motor. - i.set_current_limit(abs(mA)); + if (!pto_check(i)) i.set_current_limit(abs(mA)); // If the motor is in the pto list, don't do anything to the motor. } } @@ -306,12 +302,10 @@ bool Drive::imu_calibrate() { void Drive::set_drive_brake(pros::motor_brake_mode_e_t brake_type) { CURRENT_BRAKE = brake_type; for (auto i : left_motors) { - if (pto_check(i)) break; // If the motor is in the pto list, don't do anything to the motor. - i.set_brake_mode(brake_type); + if (!pto_check(i)) i.set_brake_mode(brake_type); // If the motor is in the pto list, don't do anything to the motor. } for (auto i : right_motors) { - if (pto_check(i)) break; // If the motor is in the pto list, don't do anything to the motor. - i.set_brake_mode(brake_type); + if (!pto_check(i)) i.set_brake_mode(brake_type); // If the motor is in the pto list, don't do anything to the motor. } } diff --git a/src/EZ-Template/drive/exit_conditions.cpp b/src/EZ-Template/drive/exit_conditions.cpp index e6556812..fa5020f3 100644 --- a/src/EZ-Template/drive/exit_conditions.cpp +++ b/src/EZ-Template/drive/exit_conditions.cpp @@ -4,146 +4,74 @@ 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" using namespace ez; // Set exit condition timeouts -void Drive::set_exit_condition(exit_condition_ &type, 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) { - type.small_exit_time = p_small_exit_time; - type.small_error = p_small_error; - type.big_exit_time = p_big_exit_time; - type.big_error = p_big_error; - type.velocity_exit_time = p_velocity_exit_time; - type.mA_timeout = p_mA_timeout; -} - -// Exit condition -bool Drive::exit_condition(std::tuple> targets, exit_condition_ input_struct, bool wait_until) { - static int i = 0, j = 0, k = 0, l = 0; - bool is_drive = std::get<1>(targets).has_value(); - - // If the robot gets within the target, make sure it's there for small_timeout amount of time - if (fabs(std::get<0>(targets) - (is_drive ? left_sensor() : get_gyro())) < input_struct.small_error) { - if (!is_drive || fabs(*std::get<1>(targets) - right_sensor()) < input_struct.small_error) { - j += util::DELAY_TIME; - i = 0; // While this is running, don't run big thresh - if (j > input_struct.small_exit_time) { - if (!wait_until) - printf(" Timed Out"); - else - printf(" Wait Until Timed Out"); - printf(" - Small Thresh\n"); - - l = 0; - i = 0; - k = 0; - j = 0; - return false; - } - } - } else { - j = 0; +void Drive::set_exit_condition(int type, 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) { + if (type == drive_exit) { + leftPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); + rightPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); } - // If the robot is close to the target, start a timer. If the robot doesn't get closer within - // a certain amount of time, exit and continue. This does not run while small_timeout is running - if (fabs(std::get<0>(targets) - (is_drive ? left_sensor() : get_gyro())) < input_struct.big_error) { - if (!is_drive || fabs(*std::get<1>(targets) - right_sensor()) < input_struct.big_error) { - i += util::DELAY_TIME; - if (i > input_struct.big_exit_time) { - if (!wait_until) - printf(" Timed Out"); - else - printf(" Wait Until Timed Out"); - printf(" - Big Thresh\n"); - - l = 0; - i = 0; - k = 0; - j = 0; - return false; - } - } - } else { - i = 0; + if (type == turn_exit) { + turnPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); } - // If the motors are pulling too many mA, the code will timeout and set interfered to true. - if (right_over_current() || left_over_current()) { - l += util::DELAY_TIME; - if (l > input_struct.mA_timeout) { - if (!wait_until) - printf(" Timed Out"); - else - printf(" Wait Until Timed Out"); - printf(" - mA\n"); - - l = 0; - i = 0; - k = 0; - j = 0; - interfered = true; - return false; - } - } else { - l = 0; + if (type == swing_exit) { + swingPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout); } - - if (right_velocity() == 0 && left_velocity() == 0) { - k += util::DELAY_TIME; - if (k > input_struct.velocity_exit_time) { - if (!wait_until) - printf(" Timed Out"); - else - printf(" Wait Until Timed Out"); - printf(" - Velocity\n"); - - l = 0; - i = 0; - k = 0; - j = 0; - interfered = true; - return false; - } - } else { - k = 0; - } - - interfered = false; - return true; } // User wrapper for exit condition void Drive::wait_drive() { + // Let the PID run at least 1 iteration pros::delay(util::DELAY_TIME); -/* + if (mode == DRIVE) { - while (exit_condition(tuple{leftPID.get_target(), rightPID.get_target()}, drive_exit)) { + exit_output left_exit = RUNNING; + exit_output right_exit = RUNNING; + while (left_exit == RUNNING || right_exit == RUNNING) { + left_exit = left_exit != RUNNING ? left_exit : leftPID.exit_condition(left_motors[0]); + right_exit = right_exit != RUNNING ? right_exit : rightPID.exit_condition(right_motors[0]); pros::delay(util::DELAY_TIME); } - } else if (mode == TURN) { - while (exit_condition(tuple{turnPID.get_target(), std::nullopt}, turn_exit)) { - pros::delay(util::DELAY_TIME); - } - } else if (mode == SWING) { - while (exit_condition(tuple{swingPID.get_target(), std::nullopt}, swing_exit)) { - pros::delay(util::DELAY_TIME); + std::cout << " Left: " << exit_to_string(left_exit) << " Exit. Right: " << exit_to_string(right_exit) << " Exit.\n"; + + if (left_exit == mA_EXIT || left_exit == VELOCITY_EXIT || right_exit == mA_EXIT || right_exit == VELOCITY_EXIT) { + interfered = true; } } - */ - if (mode == DRIVE) { - while (exit_condition(tuple{leftPID.get_target(), rightPID.get_target()}, drive_exit)) { + + // Turn Exit + else if (mode == TURN) { + exit_output turn_exit = RUNNING; + while (turn_exit == RUNNING) { + turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]}); pros::delay(util::DELAY_TIME); } - } else if (mode == TURN) { - while (exit_condition(tuple{turnPID.get_target(), std::nullopt}, turn_exit)) { - pros::delay(util::DELAY_TIME); + std::cout << " Turn: " << exit_to_string(turn_exit) << " Exit.\n"; + + if (turn_exit == mA_EXIT || turn_exit == VELOCITY_EXIT) { + interfered = true; } - } else if (mode == SWING) { - while (exit_condition(tuple{swingPID.get_target(), std::nullopt}, swing_exit)) { + } + + // Swing Exit + else if (mode == SWING) { + exit_output swing_exit = RUNNING; + pros::Motor& sensor = current_swing == ez::LEFT_SWING ? left_motors[0] : right_motors[0]; + while (swing_exit == RUNNING) { + swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor); pros::delay(util::DELAY_TIME); } + std::cout << " Swing: " << exit_to_string(swing_exit) << " Exit.\n"; + + if (swing_exit == mA_EXIT || swing_exit == VELOCITY_EXIT) { + interfered = true; + } } } @@ -151,8 +79,7 @@ void Drive::wait_drive() { void Drive::wait_until(double target) { // If robot is driving... if (mode == DRIVE) { - // If robot is driving... - // Calculate error between current and target (target needs to be an inbetween position) + // Calculate error between current and target (target needs to be an in between position) int l_tar = l_start + (target * TICK_PER_INCH); int r_tar = r_start + (target * TICK_PER_INCH); int l_error = l_tar - left_sensor(); @@ -160,16 +87,31 @@ void Drive::wait_until(double target) { int l_sgn = util::sgn(l_error); int r_sgn = util::sgn(r_error); + exit_output left_exit = RUNNING; + exit_output right_exit = RUNNING; + while (true) { l_error = l_tar - left_sensor(); r_error = r_tar - right_sensor(); - // Break the loop once target is passed - if (util::sgn(l_error) == l_sgn && util::sgn(r_error) == r_sgn) { - // this makes sure that the following else if is rnu after the sgn is flipped - } else if (util::sgn(l_error) != l_sgn && util::sgn(r_error) != r_sgn) { - return; - } else if (!exit_condition(tuple{l_tar, r_tar}, drive_exit, true)) { + // Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop + if (util::sgn(l_error) == l_sgn || util::sgn(r_error) == r_sgn) { + if (left_exit == RUNNING || right_exit == RUNNING) { + left_exit = left_exit != RUNNING ? left_exit : leftPID.exit_condition(left_motors[0]); + right_exit = right_exit != RUNNING ? right_exit : rightPID.exit_condition(right_motors[0]); + pros::delay(util::DELAY_TIME); + } else { + std::cout << " Left: " << exit_to_string(left_exit) << " Wait Until Exit. Right: " << exit_to_string(right_exit) << " Wait Until Exit.\n"; + + if (left_exit == mA_EXIT || left_exit == VELOCITY_EXIT || right_exit == mA_EXIT || right_exit == VELOCITY_EXIT) { + interfered = true; + } + return; + } + } + // Once we've past target, return + else if (util::sgn(l_error) != l_sgn || util::sgn(r_error) != r_sgn) { + std::cout << " Drive Wait Until Exit.\n"; return; } @@ -177,30 +119,64 @@ void Drive::wait_until(double target) { } } - // If robot is turning... + // If robot is turning or swinging... else if (mode == TURN || mode == SWING) { - // Calculate error between current and target (target needs to be an inbetween position) + // Calculate error between current and target (target needs to be an in between position) int g_error = target - get_gyro(); int g_sgn = util::sgn(g_error); - bool run = true; - // Change exit condition constants from turn to swing - exit_condition_ current_exit; - if (mode == TURN) - current_exit = turn_exit; - else - current_exit = swing_exit; + exit_output turn_exit = RUNNING; + exit_output swing_exit = RUNNING; + + pros::Motor& sensor = current_swing == ez::LEFT_SWING ? left_motors[0] : right_motors[0]; while (true) { g_error = target - get_gyro(); - // Break the loop once target is passed - if (util::sgn(g_error) == g_sgn) { - // this makes sure that the following else if is rnu after the sgn is flipped - } else if (util::sgn(g_error) != g_sgn) { - return; - } else if (!exit_condition(tuple{target, std::nullopt}, current_exit, true)) { - return; + // If turning... + if (mode == TURN) { + // 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) { + turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]}); + pros::delay(util::DELAY_TIME); + } else { + std::cout << " Turn: " << exit_to_string(turn_exit) << " Wait Until Exit.\n"; + + if (turn_exit == mA_EXIT || turn_exit == VELOCITY_EXIT) { + interfered = true; + } + return; + } + } + // Once we've past target, return + else if (util::sgn(g_error) != g_sgn) { + std::cout << " Turn Wait Until Exit.\n"; + return; + } + } + + // If swinging... + else { + // Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop + if (util::sgn(g_error) == g_sgn) { + if (swing_exit == RUNNING) { + swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor); + pros::delay(util::DELAY_TIME); + } else { + std::cout << " Swing: " << exit_to_string(swing_exit) << " Wait Until Exit.\n"; + + if (swing_exit == mA_EXIT || swing_exit == VELOCITY_EXIT) { + interfered = true; + } + return; + } + } + // Once we've past target, return + else if (util::sgn(g_error) != g_sgn) { + std::cout << " Swing Wait Until Exit.\n"; + return; + } } pros::delay(util::DELAY_TIME); diff --git a/src/EZ-Template/drive/slew.cpp b/src/EZ-Template/drive/slew.cpp index 3f800176..bc4a820a 100644 --- a/src/EZ-Template/drive/slew.cpp +++ b/src/EZ-Template/drive/slew.cpp @@ -47,5 +47,5 @@ double Drive::slew_calculate(slew_ &input, double current) { return (input.slope * input.error) + input.y_intercept; } // When slew is completed, return max speed - return input.max_speed; + return max_speed; } diff --git a/src/EZ-Template/util.cpp b/src/EZ-Template/util.cpp index 0d5e299d..3339d40b 100644 --- a/src/EZ-Template/util.cpp +++ b/src/EZ-Template/util.cpp @@ -82,8 +82,7 @@ void print_to_screen(std::string text, int line) { texts.push_back(temp); temp = ""; break; - } - else if (text[i] == '\n') { + } else if (text[i] == '\n') { texts.push_back(temp); temp = ""; } else { @@ -101,6 +100,25 @@ void print_to_screen(std::string text, int line) { CurrAutoLine++; } } + +std::string exit_to_string(exit_output input) { + switch ((int)input) { + case RUNNING: + return "Running"; + case SMALL_EXIT: + return "Small"; + case BIG_EXIT: + return "Big"; + case VELOCITY_EXIT: + return "Velocity"; + case mA_EXIT: + return "mA"; + default: + return "Error: Out of bounds!"; + } + + return "Error: Out of bounds!"; +} namespace util { bool AUTON_RAN = true; diff --git a/src/autons.cpp b/src/autons.cpp index c473d5c4..1530e655 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -1,5 +1,12 @@ #include "main.h" + +///// +// For instalattion, upgrading, documentations and tutorials, check out website! +// https://ez-robotics.github.io/EZ-Template/ +///// + + const int DRIVE_SPEED = 110; // This is 110/127 (around 87% of max speed). We don't suggest making this 127. // If this is 127 and the robot tries to heading correct, it's only correcting by // making one side slower. When this is 87%, it's correcting by making one side @@ -47,6 +54,14 @@ void two_mogo_constants() { } +void modified_exit_condition() { + chassis.set_exit_condition(chassis.turn_exit, 100, 3, 500, 7, 500, 500); + chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 500, 500); + chassis.set_exit_condition(chassis.drive_exit, 80, 50, 300, 150, 500, 500); +} + + + /// // Drive Example @@ -119,10 +134,10 @@ void wait_until_change_speed() { // wait_until will wait until the robot gets to a desired position - // When the robot gets to 12 inches, the robot will travel the remaining distance at a max speed of 40 + // When the robot gets to 6 inches, the robot will travel the remaining distance at a max speed of 40 chassis.set_drive_pid(24, DRIVE_SPEED, true); chassis.wait_until(6); - chassis.set_max_speed(40); // After driving 12 inches at DRIVE_SPEED, the robot will go the remaining distance at 40 speed + chassis.set_max_speed(40); // After driving 6 inches at DRIVE_SPEED, the robot will go the remaining distance at 40 speed chassis.wait_drive(); chassis.set_turn_pid(45, TURN_SPEED); @@ -134,10 +149,10 @@ void wait_until_change_speed() { chassis.set_turn_pid(0, TURN_SPEED); chassis.wait_drive(); - // When the robot gets to -12 inches, the robot will travel the remaining distance at a max speed of 40 + // When the robot gets to -6 inches, the robot will travel the remaining distance at a max speed of 40 chassis.set_drive_pid(-24, DRIVE_SPEED, true); chassis.wait_until(-6); - chassis.set_max_speed(40); // After driving 12 inches at DRIVE_SPEED, the robot will go the remaining distance at 40 speed + chassis.set_max_speed(40); // After driving 6 inches at DRIVE_SPEED, the robot will go the remaining distance at 40 speed chassis.wait_drive(); } @@ -202,7 +217,7 @@ void tug (int attempts) { chassis.set_drive_pid(-2, 20); pros::delay(1000); } - // If robot succesfully drove back, return + // If robot successfully drove back, return else { return; } diff --git a/src/main.cpp b/src/main.cpp index e1b5fa3a..eacf234e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -9,7 +9,7 @@ Drive chassis ( // Right Chassis Ports (negative port will reverse it!) // the first port is the sensored port (when trackers are not used!) - ,{1, 2} + ,{6, 5} // IMU Port ,20 @@ -132,13 +132,7 @@ void autonomous() { ez::as::auton_selector.call_selected_auton(); // Calls selected auton from autonomous selector. } -pros::Motor leftm(6); -pros::Motor rightm(5); -PID intakePID(0.4, 0, 5); -void set_intake(int input) { - leftm = input; - rightm = input; -} + /** * Runs the operator control code. This function will be started in its own task @@ -157,24 +151,9 @@ void opcontrol() { // This is preference to what you like to drive on. chassis.set_drive_brake(MOTOR_BRAKE_COAST); - intakePID.set_exit_condition(80, 50, 300, 150, 500, 500); - intakePID.set_target(1000); - while (intakePID.exit_condition(true) == ez::RUNNING) { - set_intake( intakePID.compute(leftm.get_position()) ); - pros::delay(ez::util::DELAY_TIME); - } - - intakePID.set_target(0); - while (intakePID.exit_condition({leftm}, true) == ez::RUNNING) { - set_intake( intakePID.compute(leftm.get_position()) ); - pros::delay(ez::util::DELAY_TIME); - } - - set_intake(0); - while (true) { - // chassis.tank(); // Tank control + chassis.tank(); // Tank control // chassis.arcade_standard(ez::SPLIT); // Standard split arcade // chassis.arcade_standard(ez::SINGLE); // Standard single arcade // chassis.arcade_flipped(ez::SPLIT); // Flipped split arcade