From 25a3eaf7c08f4c0d26a92e4d64fc5a620108d880 Mon Sep 17 00:00:00 2001 From: Jess Zarchi Date: Thu, 30 Dec 2021 20:16:34 +0000 Subject: [PATCH] Added pto support #12 --- include/EZ-Template/drive/drive.hpp | 56 +++++++++++++++++++++++++++++ src/EZ-Template/drive/drive.cpp | 32 ++++++++++------- src/EZ-Template/drive/pto.cpp | 54 ++++++++++++++++++++++++++++ src/main.cpp | 37 ++----------------- 4 files changed, 133 insertions(+), 46 deletions(-) create mode 100644 src/EZ-Template/drive/pto.cpp diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index 5d6565f1..5b0b334c 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/include/EZ-Template/drive/drive.hpp @@ -12,6 +12,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include "EZ-Template/PID.hpp" #include "EZ-Template/util.hpp" +#include "pros/motors.h" using namespace ez; @@ -22,6 +23,16 @@ class Drive { */ int JOYSTICK_THRESHOLD; + /** + * Global current brake mode. + */ + pros::motor_brake_mode_e_t CURRENT_BRAKE = pros::E_MOTOR_BRAKE_COAST; + + /** + * Global current mA. + */ + int CURRENT_MA = 2500; + /** * Current swing type. */ @@ -37,6 +48,11 @@ class Drive { */ std::vector right_motors; + /** + * Vector of pros motors that are disconnected from the drive. + */ + std::vector pto_active; + /** * Inertial sensor. */ @@ -278,6 +294,46 @@ class Drive { */ void set_joystick_threshold(int threshold); + ///// + // + // 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. + * + * \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. You cannot use the first index in a pto. + * + * \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 use the first index in a pto. + * + * \param pto_list + * list of motors to add/remove from the drive. + * \param toggle + * if true, adds to list. if false, removes from list. + */ + void pto_toggle(std::vector pto_list, bool toggle); + ///// // // PROS Wrapers diff --git a/src/EZ-Template/drive/drive.cpp b/src/EZ-Template/drive/drive.cpp index 272aad62..7a9f8562 100644 --- a/src/EZ-Template/drive/drive.cpp +++ b/src/EZ-Template/drive/drive.cpp @@ -4,9 +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 "drive.hpp" + #include -#include "drive.hpp" #include "main.h" #include "pros/llemu.hpp" #include "pros/screen.hpp" @@ -185,9 +186,11 @@ 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)); } 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)); } } @@ -196,10 +199,13 @@ void Drive::set_drive_current_limit(int mA) { if (abs(mA) > 2500) { mA = 2500; } + 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)); } 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)); } } @@ -212,8 +218,7 @@ void Drive::reset_drive_sensor() { left_tracker.reset(); right_tracker.reset(); return; - } - else if (is_tracker == DRIVE_ROTATION) { + } else if (is_tracker == DRIVE_ROTATION) { left_rotation.reset(); right_rotation.reset(); return; @@ -254,24 +259,24 @@ void Drive::imu_loading_display(int iter) { // Create the boarder pros::screen::set_pen(COLOR_WHITE); - for (int i = 1; i<3;i++) { - pros::screen::draw_rect(boarder+i, boarder+i, 480-boarder-i, 240-boarder-i); + for (int i = 1; i < 3; i++) { + pros::screen::draw_rect(boarder + i, boarder + i, 480 - boarder - i, 240 - boarder - i); } // While IMU is loading if (iter < 1440) { static int last_x1 = boarder; - pros::screen::set_pen(0x00FF6EC7); // EZ Pink - int x1 = (iter*((480-(boarder*2))/1440.0)) + boarder; - pros::screen::fill_rect(last_x1, boarder, x1, 240-boarder); + pros::screen::set_pen(0x00FF6EC7); // EZ Pink + int x1 = (iter * ((480 - (boarder * 2)) / 1440.0)) + boarder; + pros::screen::fill_rect(last_x1, boarder, x1, 240 - boarder); last_x1 = x1; } // Failsafe time else { static int last_x1 = boarder; - pros::screen::set_pen(COLOR_RED); - int x1 = ((iter-1440)*((480-(boarder*2))/1560.0)) + boarder; - pros::screen::fill_rect(last_x1, boarder, x1, 240-boarder); + pros::screen::set_pen(COLOR_RED); + int x1 = ((iter - 1440) * ((480 - (boarder * 2)) / 1560.0)) + boarder; + pros::screen::fill_rect(last_x1, boarder, x1, 240 - boarder); last_x1 = x1; } } @@ -299,10 +304,13 @@ bool Drive::imu_calibrate() { // Brake modes 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); } 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); } } @@ -311,4 +319,4 @@ void Drive::initialize() { init_curve_sd(); imu_calibrate(); -} +} \ No newline at end of file diff --git a/src/EZ-Template/drive/pto.cpp b/src/EZ-Template/drive/pto.cpp new file mode 100644 index 00000000..6aa24fdc --- /dev/null +++ b/src/EZ-Template/drive/pto.cpp @@ -0,0 +1,54 @@ +/* +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 +#include + +#include "drive.hpp" +#include "main.h" + +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()); + if (does_exist != pto_active.end()) + return true; // Motor is in the list + return false; // Motor isn't in the list +} + +void Drive::pto_add(std::vector pto_list) { + for (auto i : pto_list) { + // Return if the motor is already in the list + if (pto_check(i)) return; + + // Return if the first index was used (this motor is used for velocity) + if (i.get_port() == left_motors[0].get_port() || i.get_port() == right_motors[0].get_port()) { + printf("You cannot PTO the first index!\n"); + return; + } + + pto_active.push_back(i.get_port()); + } +} + +void Drive::pto_remove(std::vector pto_list) { + for (auto i : pto_list) { + auto does_exist = std::find(pto_active.begin(), pto_active.end(), i.get_port()); + // Return if the motor isn't in the list + if (does_exist == pto_active.end()) return; + + // Find index of motor + int index = std::distance(pto_active.begin(), does_exist); + pto_active.erase(pto_active.begin() + index); + i.set_brake_mode(CURRENT_BRAKE); // Set the motor to the brake type of the drive + i.set_current_limit(CURRENT_MA); // Set the motor to the mA of the drive + } +} + +void Drive::pto_toggle(std::vector pto_list, bool toggle) { + if (toggle) + pto_add(pto_list); + else + pto_remove(pto_list); +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 71a82550..a3fa8863 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,11 +5,11 @@ Drive chassis ( // Left Chassis Ports (negative port will reverse it!) // the first port is the sensored port (when trackers are not used!) - {-15, -16} + {-15, -16, 5, 6} // Right Chassis Ports (negative port will reverse it!) // the first port is the sensored port (when trackers are not used!) - ,{5, 6} + ,{12, 13, 11} // IMU Port ,20 @@ -42,37 +42,6 @@ Drive chassis ( // 3 Wire Port Expander Smart Port // ,1 ); -/* -// Chassis constructor with rotation sensor -Drive chassis ( - // Left Chassis Ports (negative port will reverse it!) - // the first port is the sensored port (when trackers are not used!) - {1, 2} - - // Right Chassis Ports (negative port will reverse it!) - // the first port is the sensored port (when trackers are not used!) - ,{-3, -4} - - // IMU Port - ,5 - - // Wheel Diameter (Remember, 4" wheels are actually 4.125!) - // (or tracking wheel diameter) - ,2.75 - - // External Gear Ratio (MUST BE DECIMAL) - // (or gear ratio of tracking wheel) - // eg. if your drive is 84:36 where the 36t is powered, your RATIO would be 2.333. - // eg. if your drive is 36:60 where the 60t is powered, your RATIO would be 0.6. - ,1 - - // Left Rotation Port (negative port will reverse it!) - ,-6 - - // Left Rotation Port (negative port will reverse it!) - ,7 -); -*/ @@ -181,7 +150,7 @@ void opcontrol() { chassis.set_drive_brake(MOTOR_BRAKE_COAST); while (true) { - + chassis.tank(); // Tank control // chassis.arcade_standard(ez::SPLIT); // Standard split arcade // chassis.arcade_standard(ez::SINGLE); // Standard single arcade