From a3549b282513fffb210f37ddce8127ed1e3d090e Mon Sep 17 00:00:00 2001 From: Jess Zarchi Date: Fri, 19 Nov 2021 20:10:58 -0700 Subject: [PATCH] #21 Added tracking wheel support tick_per_inch now changes how it calculates depending on if trakcing wheels are used or not. there are 3 drive constructors, one for internal encoders, one for tracking wheels in brain, and one for tracking wheels in an expander. left_sensor and right_sensor output motor/tracking wheel depending on the constructor used. --- include/EZ-Template/drive/drive.hpp | 79 +++++++++++++++++-- src/EZ-Template/drive/drive.cpp | 118 +++++++++++++++++++++++++--- src/main.cpp | 30 +++++-- 3 files changed, 203 insertions(+), 24 deletions(-) diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index b2631665..0d979883 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/include/EZ-Template/drive/drive.hpp @@ -46,6 +46,9 @@ class drive { */ pros::Controller master; + pros::ADIEncoder left_tracker; + pros::ADIEncoder right_tracker; + /** * PID objects. */ @@ -65,12 +68,74 @@ class drive { /** - * Creates a controller. - * - * Give Sensor Ports, Motor Ports (give a negative port if motor is reversed), Drive Measurements - * Set PID Constants + * Creates a Drive Controller using internal encoders. + * + * \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 drive wheels. Remember 4" is 4.125"! + * \param ticks + * Motor cartidge RPM + * \param ratio + * 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); + + /** + * 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 wheels. Remember 4" is 4.125"! + * \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, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector left_tracker_ports, std::vector right_tracker_ports); + + + /** + * 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 wheels. Remember 4" is 4.125"! + * \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, int imu_port, double wheel_diameter, double motor_cartridge, double ratio); + 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); + + /** + * Sets drive defaults. + */ + void set_defaults(); @@ -419,7 +484,6 @@ class drive { void set_exit_condition(exit_condition_ &type, int p_small_exit_time, int p_small_error, int p_big_exit_time, int p_big_error, int p_velocity_exit_timeint, int p_mA_timeout); - private: // !Auton @@ -486,6 +550,9 @@ class drive { // Is tank drive running? bool is_tank; + // Is tracker? + bool is_tracker = false; + // Save input curve to SD card void save_l_curve_sd(); void save_r_curve_sd(); diff --git a/src/EZ-Template/drive/drive.cpp b/src/EZ-Template/drive/drive.cpp index d6776807..fd4c3e54 100644 --- a/src/EZ-Template/drive/drive.cpp +++ b/src/EZ-Template/drive/drive.cpp @@ -10,12 +10,18 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. using namespace ez; -drive::drive(std::vector left_motor_ports, std::vector right_motor_ports, int imu_port, double wheel_diameter, double motor_cartridge, double ratio) - : - imu (imu_port), - master(pros::E_CONTROLLER_MASTER), - ez_auto([this]{ this->ez_auto_task(); }) +// Constructor for integrated encoders +drive::drive(std::vector left_motor_ports, std::vector right_motor_ports, + int imu_port, double wheel_diameter, double ticks, double ratio) + : + imu (imu_port), + master(pros::E_CONTROLLER_MASTER), + left_tracker (-1, -1, false), // Default value + right_tracker (-1, -1, false), // Default value + ez_auto([this]{ this->ez_auto_task(); }) { + is_tracker = false; + // Set ports to a global vector for(auto i : left_motor_ports) { @@ -31,9 +37,81 @@ drive::drive(std::vector left_motor_ports, std::vector right_motor_por // Set constants for tick_per_inch caluclation WHEEL_DIAMETER = wheel_diameter; RATIO = ratio; - CARTRIDGE = motor_cartridge; + CARTRIDGE = ticks; TICK_PER_INCH = get_tick_per_inch(); + set_defaults(); +} + +// 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, + std::vector left_tracker_ports, std::vector right_tracker_ports) + : + imu (imu_port), + master(pros::E_CONTROLLER_MASTER), + left_tracker (abs(left_tracker_ports[0]), abs(left_tracker_ports[1]), util::is_reversed(left_tracker_ports[0])), + right_tracker(abs(right_tracker_ports[0]), abs(right_tracker_ports[1]), util::is_reversed(right_tracker_ports[0])), + ez_auto([this]{ this->ez_auto_task(); }) +{ + is_tracker = true; + + // Set ports to a global vector + for(auto i : left_motor_ports) + { + pros::Motor temp(abs(i), util::is_reversed(i)); + left_motors.push_back(temp); + } + for(auto i : right_motor_ports) + { + pros::Motor temp(abs(i), util::is_reversed(i)); + right_motors.push_back(temp); + } + + // Set constants for tick_per_inch caluclation + WHEEL_DIAMETER = wheel_diameter; + RATIO = ratio; + CARTRIDGE = ticks; + TICK_PER_INCH = get_tick_per_inch(); + + set_defaults(); +} + +// 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, + std::vector left_tracker_ports, std::vector right_tracker_ports, int expander_smart_port) + : + imu (imu_port), + master(pros::E_CONTROLLER_MASTER), + left_tracker ({ expander_smart_port, abs(left_tracker_ports[0]), abs(left_tracker_ports[1]) }, util::is_reversed(left_tracker_ports[0])), + right_tracker({ expander_smart_port, abs(right_tracker_ports[0]), abs(right_tracker_ports[1])}, util::is_reversed(right_tracker_ports[0])), + ez_auto([this]{ this->ez_auto_task(); }) +{ + is_tracker = true; + + // Set ports to a global vector + for(auto i : left_motor_ports) + { + pros::Motor temp(abs(i), util::is_reversed(i)); + left_motors.push_back(temp); + } + for(auto i : right_motor_ports) + { + pros::Motor temp(abs(i), util::is_reversed(i)); + right_motors.push_back(temp); + } + + // Set constants for tick_per_inch caluclation + WHEEL_DIAMETER = wheel_diameter; + RATIO = ratio; + CARTRIDGE = ticks; + TICK_PER_INCH = get_tick_per_inch(); + + set_defaults(); +} + +void drive::set_defaults() { // PID Constants headingPID = {11, 0, 20, 0}; forward_drivePID = {0.45, 0, 5, 0}; @@ -61,8 +139,13 @@ drive::drive(std::vector left_motor_ports, std::vector right_motor_por } double drive::get_tick_per_inch() { - TICK_PER_REV = (50.0*(3600.0/CARTRIDGE)) * RATIO; // with no cart, the encoder reads 50 counts per rotation - CIRCUMFERENCE = WHEEL_DIAMETER*M_PI; + CIRCUMFERENCE = WHEEL_DIAMETER*M_PI; + + if (is_tracker) + TICK_PER_REV = CARTRIDGE * RATIO; + else + 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); return TICK_PER_INCH; } @@ -86,16 +169,29 @@ void drive::set_tank(int left, int right) { // Motor telemetry void drive::reset_drive_sensor() { - left_motors.front().tare_position(); + if (is_tracker) { + left_tracker. reset(); + right_tracker.reset(); + return; + } + left_motors.front(). tare_position(); right_motors.front().tare_position(); } -int drive::right_sensor() { return right_motors.front().get_position(); } +int drive::right_sensor() { + if (is_tracker) + return right_tracker.get_value(); + return right_motors.front().get_position(); +} int drive::right_velocity() { return right_motors.front().get_actual_velocity(); } double drive::right_mA() { return right_motors.front().get_current_draw(); } bool drive::right_over_current() { return right_motors.front().is_over_current(); } -int drive::left_sensor() { return left_motors.front().get_position(); } +int drive::left_sensor() { + if (is_tracker) + return left_tracker.get_value(); + return left_motors.front().get_position(); +} int drive::left_velocity() { return left_motors.front().get_actual_velocity(); } double drive::left_mA() { return left_motors.front().get_current_draw(); } bool drive::left_over_current() { return left_motors.front().is_over_current(); } diff --git a/src/main.cpp b/src/main.cpp index 353c6124..37201dcb 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -8,22 +8,38 @@ // Chassis constructor drive chassis ( // Left Chassis Ports (negative port will reverse it!) - {-11, -5, -7}, + {-11, -5, -7} // Right Chassis Ports (negative port will reverse it!) - {3, 2, 17}, + ,{3, 2, 17} // IMU Port - 18, + ,18 // Wheel Diameter (Remember, 4" wheels are actually 4.125!) - 3.25, + // (or tracking wheel diameter) + ,3.25 // Cartridge RPM - 600, + // (pr tick per rotation if using tracking wheels) + ,600 - // External Gear Ratio - 1.66666666667 + // External Gear Ratio + // (or gear ratio of tracking wheel) + ,1.66666666667 + + // Uncomment if using tracking wheels + /* + // Left Tracking Wheel Ports + ,{1, 2} + + // Right Tracking Wheel Ports + ,{3, 4} + */ + + // Uncomment if tracking wheels are plugged into a 3 wire expander + // 3 Wire Port Expander Smart Port + // ,1 );