From 89d2d566e1dd82fe88939b186bd3d8c66491fc9a Mon Sep 17 00:00:00 2001 From: Jess Zarchi Date: Wed, 1 Jan 2025 21:02:12 -0800 Subject: [PATCH] Cleaned formatting Ran clangd Fixed typos Removed multiple imu support from deprecated constructors Added "get" to multiple scalers --- include/EZ-Template/drive/drive.hpp | 112 ++++----------- src/EZ-Template/drive/drive.cpp | 187 +++----------------------- src/EZ-Template/drive/maintenance.cpp | 61 ++++----- 3 files changed, 79 insertions(+), 281 deletions(-) diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index 78979f4..f9e89c9 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/include/EZ-Template/drive/drive.hpp @@ -6,11 +6,12 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #pragma once +#include #include #include -#include #include -#include +#include + #include "EZ-Template/PID.hpp" #include "EZ-Template/slew.hpp" #include "EZ-Template/tracking_wheel.hpp" @@ -69,15 +70,15 @@ class Drive { pros::Imu* imu; /** - * All good imus, for redunadncy + * All good imus, for redundancy. */ std::deque good_imus; - + /** - * All good imus, for redunadncy + * All good imus, for redundancy. */ std::deque> bad_imus; - + /** * Deprecated left tracking wheel. */ @@ -392,10 +393,6 @@ class Drive { */ void initialize(); - /* - * Set all scalings for all imu's - */ - void set_all_imu_scaling(std::vector scales); /** * Tasks for autonomous. */ @@ -485,17 +482,15 @@ class Drive { */ 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!"))); - //here is the swithc!!!! - /** - * Creates a Drive Controller using internal encoders. + * Creates a Drive Controller using internal encoders with redundant IMUs. * * \param left_motor_ports * input {1, -2...}. make ports negative if reversed * \param right_motor_ports * input {-3, 4...}. make ports negative if reversed * \param imu_port - * port the IMU is plugged into + * input {5, 6...}. multiple IMU ports * \param wheel_diameter * diameter of your drive wheels * \param ticks @@ -505,74 +500,7 @@ class Drive { */ Drive(std::vector left_motor_ports, std::vector right_motor_ports, std::vector imu_ports, double wheel_diameter, double ticks, double ratio = 1.0); - /** - * Creates a Drive Controller using encoders plugged into the brain. - * - * \param left_motor_ports - * input {1, -2...}. make ports negative if reversed - * \param right_motor_ports - * input {-3, 4...}. make ports negative if reversed - * \param imu_port - * port the IMU is plugged into - * \param wheel_diameter - * diameter of your sensored wheel - * \param ticks - * ticks per revolution of your encoder - * \param ratio - * external gear ratio, wheel gear / sensor gear - * \param left_tracker_ports - * input {1, 2}. make ports negative if reversed - * \param right_tracker_ports - * input {3, 4}. make ports negative if reversed - */ - Drive(std::vector left_motor_ports, std::vector right_motor_ports, std::vector imu_ports, 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 - * \param right_motor_ports - * input {-3, 4...}. make ports negative if reversed - * \param imu_port - * port the IMU is plugged into - * \param wheel_diameter - * diameter of your sensored wheel - * \param ticks - * ticks per revolution of your encoder - * \param ratio - * external gear ratio, wheel gear / sensor gear - * \param left_tracker_ports - * input {1, 2}. make ports negative if reversed - * \param right_tracker_ports - * input {3, 4}. make ports negative if reversed - * \param expander_smart_port - * port the expander is plugged into - */ - Drive(std::vector left_motor_ports, std::vector right_motor_ports, std::vector imu_ports, 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 - * \param right_motor_ports - * input {-3, 4...}. make ports negative if reversed - * \param imu_ports - * port the IMU is plugged into - * \param wheel_diameter - * diameter of your sensored wheel - * \param ratio - * external gear ratio, wheel gear / sensor gear - * \param left_tracker_port - * make ports negative if reversed - * \param right_tracker_port - * make ports negative if reversed - */ - Drive(std::vector left_motor_ports, std::vector right_motor_ports, std::vector imu_ports, 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!"))); - - - //Deconstructor + // Deconstructor ~Drive(); /** @@ -1453,6 +1381,21 @@ class Drive { */ double drive_imu_scaler_get(); + /* + * Sets a new IMU scaling factor for all IMUs. + * + * This value is multiplied by the imu to change its output. + * + * \param scales + * input {0.99, 1.01...} + */ + void drive_imus_scalers_set(std::vector scales); + + /* + * Returns the scaling factor for all IMUs. + */ + std::map drive_imus_scalers_get(); + /** * Calibrates the IMU, recommended to run in initialize(). * @@ -3619,7 +3562,6 @@ class Drive { double used_motion_chain_scale = 0.0; bool motion_chain_backward = false; - bool drive_toggle = true; bool print_toggle = true; int swing_min = 0; @@ -3709,8 +3651,8 @@ class Drive { /** * Current position of imu - */ - + */ + /** * Enable/disable modifying controller curve with controller. */ diff --git a/src/EZ-Template/drive/drive.cpp b/src/EZ-Template/drive/drive.cpp index b965bb9..b22c960 100644 --- a/src/EZ-Template/drive/drive.cpp +++ b/src/EZ-Template/drive/drive.cpp @@ -47,6 +47,7 @@ Drive::Drive(std::vector left_motor_ports, std::vector right_motor_por drive_defaults_set(); } +// Constructor for integrated encoders with redundant imu support Drive::Drive(std::vector left_motor_ports, std::vector right_motor_ports, std::vector imu_ports, double wheel_diameter, double ticks, double ratio) : imu(new pros::Imu(imu_ports[0])), @@ -68,20 +69,19 @@ Drive::Drive(std::vector left_motor_ports, std::vector right_motor_por temp.set_reversed(util::reversed_active(i)); right_motors.push_back(temp); } - + std::vector imu_scale_values = {}; - + good_imus.push_back(imu); imu_scale_values.push_back(1); - //set all imus - for (int i = 1; i < imu_ports.size(); i--) - { + // set all imus + for (int i = 1; i < imu_ports.size(); i--) { pros::Imu* temp = new pros::Imu(imu_ports[i]); good_imus.push_back(temp); imu_scale_values.push_back(1); } - - set_all_imu_scaling(imu_scale_values); + + drive_imus_scalers_set(imu_scale_values); // Set constants for tick_per_inch calculation WHEEL_DIAMETER = wheel_diameter; RATIO = ratio; @@ -90,6 +90,7 @@ Drive::Drive(std::vector left_motor_ports, std::vector right_motor_por drive_defaults_set(); } + // Constructor for tracking wheels plugged into the brain Drive::Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, @@ -125,53 +126,6 @@ Drive::Drive(std::vector left_motor_ports, std::vector right_motor_por drive_defaults_set(); } -// Constructor for tracking wheels plugged into the brain and multiple imus -Drive::Drive(std::vector left_motor_ports, std::vector right_motor_ports, - std::vector imu_ports, double wheel_diameter, double ticks, double ratio, - std::vector left_tracker_ports, std::vector right_tracker_ports) - : imu(new pros::Imu(imu_ports[0])), - left_tracker(abs(left_tracker_ports[0]), abs(left_tracker_ports[1]), util::reversed_active(left_tracker_ports[0])), - right_tracker(abs(right_tracker_ports[0]), abs(right_tracker_ports[1]), util::reversed_active(right_tracker_ports[0])), - left_rotation(-1), - right_rotation(-1), - ez_auto([this] { this->ez_auto_task(); }) { - is_tracker = DRIVE_ADI_ENCODER; - - // Set ports to a global vector - for (auto i : left_motor_ports) { - pros::Motor temp(abs(i)); - temp.set_reversed(util::reversed_active(i)); - left_motors.push_back(temp); - } - for (auto i : right_motor_ports) { - pros::Motor temp(abs(i)); - temp.set_reversed(util::reversed_active(i)); - right_motors.push_back(temp); - } - - std::vector imu_scale_values = {}; - - good_imus.push_back(imu); - imu_scale_values.push_back(1); - //set all imus - for (int i = 1; i < imu_ports.size(); i--) - { - pros::Imu* temp = new pros::Imu(imu_ports[i]); - good_imus.push_back(temp); - imu_scale_values.push_back(1); - } - - set_all_imu_scaling(imu_scale_values); - - // Set constants for tick_per_inch calculation - WHEEL_DIAMETER = wheel_diameter; - RATIO = ratio; - CARTRIDGE = ticks; - TICK_PER_INCH = drive_tick_per_inch(); - - drive_defaults_set(); -} - // Constructor for tracking wheels plugged into a 3 wire expander Drive::Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, @@ -207,53 +161,6 @@ Drive::Drive(std::vector left_motor_ports, std::vector right_motor_por drive_defaults_set(); } -// Constructor for tracking wheels plugged into a 3 wire expander with multiple imu support -Drive::Drive(std::vector left_motor_ports, std::vector right_motor_ports, - std::vector imu_ports, double wheel_diameter, double ticks, double ratio, - std::vector left_tracker_ports, std::vector right_tracker_ports, int expander_smart_port) - : imu(new pros::Imu(imu_ports[0])), - left_tracker({expander_smart_port, abs(left_tracker_ports[0]), abs(left_tracker_ports[1])}, util::reversed_active(left_tracker_ports[0])), - right_tracker({expander_smart_port, abs(right_tracker_ports[0]), abs(right_tracker_ports[1])}, util::reversed_active(right_tracker_ports[0])), - left_rotation(-1), - right_rotation(-1), - ez_auto([this] { this->ez_auto_task(); }) { - is_tracker = DRIVE_ADI_ENCODER; - - // Set ports to a global vector - for (auto i : left_motor_ports) { - pros::Motor temp(abs(i)); - temp.set_reversed(util::reversed_active(i)); - left_motors.push_back(temp); - } - for (auto i : right_motor_ports) { - pros::Motor temp(abs(i)); - temp.set_reversed(util::reversed_active(i)); - right_motors.push_back(temp); - } - - std::vector imu_scale_values = {}; - - good_imus.push_back(imu); - imu_scale_values.push_back(1); - //set all imus - for (int i = 1; i < imu_ports.size(); i--) - { - pros::Imu* temp = new pros::Imu(imu_ports[i]); - good_imus.push_back(temp); - imu_scale_values.push_back(1); - } - - set_all_imu_scaling(imu_scale_values); - - // Set constants for tick_per_inch calculation - WHEEL_DIAMETER = wheel_diameter; - RATIO = ratio; - CARTRIDGE = ticks; - TICK_PER_INCH = drive_tick_per_inch(); - - drive_defaults_set(); -} - // Constructor for rotation sensors Drive::Drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double ratio, @@ -291,74 +198,22 @@ Drive::Drive(std::vector left_motor_ports, std::vector right_motor_por drive_defaults_set(); } -// Constructor for rotation sensors with multiple imu support -Drive::Drive(std::vector left_motor_ports, std::vector right_motor_ports, - std::vector imu_ports, double wheel_diameter, double ratio, - int left_rotation_port, int right_rotation_port) - : imu(new pros::Imu(imu_ports[0])), - left_tracker(-1, -1, false), // Default value - right_tracker(-1, -1, false), // Default value - left_rotation(abs(left_rotation_port)), - right_rotation(abs(right_rotation_port)), - ez_auto([this] { this->ez_auto_task(); }) { - is_tracker = DRIVE_ROTATION; - left_rotation.set_reversed(util::reversed_active(left_rotation_port)); - right_rotation.set_reversed(util::reversed_active(right_rotation_port)); - - // Set ports to a global vector - for (auto i : left_motor_ports) { - pros::Motor temp(abs(i)); - temp.set_reversed(util::reversed_active(i)); - left_motors.push_back(temp); - } - for (auto i : right_motor_ports) { - pros::Motor temp(abs(i)); - temp.set_reversed(util::reversed_active(i)); - right_motors.push_back(temp); - } - - std::vector imu_scale_values = {}; - - good_imus.push_back(imu); - imu_scale_values.push_back(1); - //set all imus - for (int i = 1; i < imu_ports.size(); i--) - { - pros::Imu* temp = new pros::Imu(imu_ports[i]); - good_imus.push_back(temp); - imu_scale_values.push_back(1); - } - - set_all_imu_scaling(imu_scale_values); - // Set constants for tick_per_inch calculation - WHEEL_DIAMETER = wheel_diameter; - RATIO = ratio; - CARTRIDGE = 36000; - TICK_PER_INCH = drive_tick_per_inch(); - - drive_defaults_set(); -} - -Drive::~Drive() -{ +Drive::~Drive() { delete imu; - - while(!good_imus.empty()) - { - delete good_imus.front(); + + while (!good_imus.empty()) { + delete good_imus.front(); good_imus.pop_front(); } - while(!bad_imus.empty()) - { + while (!bad_imus.empty()) { auto ptr = std::get(bad_imus.back()); delete ptr; bad_imus.pop_back(); } - } -//set defaults +// set defaults void Drive::drive_defaults_set() { imu->set_data_rate(5); @@ -417,13 +272,6 @@ void Drive::drive_defaults_set() { as::limit_switch_lcd_initialize(nullptr, nullptr); } -void Drive::set_all_imu_scaling(std::vector scales) -{ - for(int i = 0; i < std::min(good_imus.size(), scales.size()); i++) - { - imu_scale_map[good_imus[i]->get_port()] = scales[i]; - } -} double Drive::drive_tick_per_inch() { if (is_tracker == ODOM_TRACKER) return odom_tracker_right->ticks_per_inch(); @@ -560,6 +408,13 @@ double Drive::drive_imu_accel_get() { return imu->get_accel().x + imu->get_accel void Drive::drive_imu_scaler_set(double scaler) { imu_scale_map[imu->get_port()] = scaler; } double Drive::drive_imu_scaler_get() { return imu_scale_map[imu->get_port()]; } +void Drive::drive_imus_scalers_set(std::vector scales) { + for (int i = 0; i < std::min(good_imus.size(), scales.size()); i++) { + imu_scale_map[good_imus[i]->get_port()] = scales[i]; + } +} +std::map Drive::drive_imus_scalers_get() { return imu_scale_map; } + 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; diff --git a/src/EZ-Template/drive/maintenance.cpp b/src/EZ-Template/drive/maintenance.cpp index c0951f4..79e15a8 100644 --- a/src/EZ-Template/drive/maintenance.cpp +++ b/src/EZ-Template/drive/maintenance.cpp @@ -1,39 +1,40 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + #include "EZ-Template/drive/drive.hpp" #include "EZ-Template/util.hpp" +#include "api.h" using namespace ez; -void Drive::check_imu_task() -{ - std::vector deleteIndexes = {}; - for(int i = 0; i < bad_imus.size(); i++) +void Drive::check_imu_task() { + std::vector deleteIndexes = {}; + for (int i = 0; i < bad_imus.size(); i++) { + auto& [imuPtr, start_time] = bad_imus[i]; + if (pros::millis() - start_time > util::DISCONNECT_THRESHOLD) { + delete imuPtr; // always invalid + deleteIndexes.push_back(i); + } else if (imuPtr->get_status() != pros::ImuStatus::error && errno != PROS_ERR) // not sure if errno is needed yet. I think it is?? { - auto& [imuPtr, start_time] = bad_imus[i]; - if(pros::millis() - start_time > util::DISCONNECT_THRESHOLD) - { - delete imuPtr; //always invalid - deleteIndexes.push_back(i); - } - else if(imuPtr->get_status() != pros::ImuStatus::error && errno != PROS_ERR)//not sure if errno is needed yet. I think it is?? - { - good_imus.push_front(imuPtr); - deleteIndexes.push_back(i); - imu = imuPtr; - } + good_imus.push_front(imuPtr); + deleteIndexes.push_back(i); + imu = imuPtr; } - for(auto i : deleteIndexes) - { - bad_imus.erase(bad_imus.begin() + i); + } + for (auto i : deleteIndexes) { + bad_imus.erase(bad_imus.begin() + i); + } + + if ((imu->get_status() == pros::ImuStatus::error || errno == PROS_ERR) && !good_imus.empty()) // not sure if errno is needed yet. I think it is?? + { + // switch due to an error + bad_imus.push_front(std::make_tuple(good_imus.front(), pros::millis())); + good_imus.pop_front(); + if (!good_imus.empty()) { + imu = good_imus.front(); } - - if((imu->get_status() == pros::ImuStatus::error || errno == PROS_ERR) && !good_imus.empty())//not sure if errno is needed yet. I think it is?? - { - //switch due to an error - bad_imus.push_front(std::make_tuple(good_imus.front(), pros::millis())); - good_imus.pop_front(); - if (!good_imus.empty()) - { - imu = good_imus.front(); - } - } + } } \ No newline at end of file