From 25bfb85c67661e317d657ff0a9947f3befa30f96 Mon Sep 17 00:00:00 2001 From: OliverCieslak Date: Sun, 16 Mar 2025 15:48:47 -0500 Subject: [PATCH] Add more functions to Motors and MotorGroups --- include/hardware/Motor/Motor.hpp | 58 ++++++++++++++++++ include/hardware/Motor/MotorGroup.hpp | 87 ++++++++++++++++++++++++--- src/hardware/Motor/Motor.cpp | 53 ++++++++++++++++ src/hardware/Motor/MotorGroup.cpp | 38 ++++++++++++ 4 files changed, 228 insertions(+), 8 deletions(-) diff --git a/include/hardware/Motor/Motor.hpp b/include/hardware/Motor/Motor.hpp index cd24bb8..ca0d4aa 100644 --- a/include/hardware/Motor/Motor.hpp +++ b/include/hardware/Motor/Motor.hpp @@ -472,6 +472,32 @@ class Motor : public Encoder { * @endcode */ Temperature getTemperature() const; + /** + * @brief Get the current of the motor + * + * This function uses the following values of errno when an error state is reached: + * + * ENODEV: the port cannot be configured as a motor + * + * @return Current the current of the motor + * @return INFINITY on failure, setting errno + * + * @b Example: + * @code {.cpp} + * void initialize() { + * lemlib::Motor motor(1, 200_rpm); + * + * // output motor current to the console + * Current current = motor.getCurrent(); + * if (to_amp(current) == INFINITY) { + * std::cout << "Error getting motor current" << std::endl; + * } else { + * std::cout << "Motor Current: " << to_amp(current) << std::endl; + * } + * } + * @endcode + */ + Current getCurrent() const; /** * @brief set the output velocity of the motor * @@ -504,6 +530,38 @@ class Motor : public Encoder { * @endcode */ AngularVelocity getOutputVelocity() const; + /** + * @brief Get the cartridge of the motor + * + * @return AngularVelocity + * + * @b Example: + * @code {.cpp} + * void initialize() { + * lemlib::Motor motor(1, 360_rpm); + * motor.move(.5); + * // get the motor output + * std::cout << motor.getMotorCartridge() << std::endl; // output: 600_rpm + * } + * @endcode + */ + AngularVelocity getMotorCartridge() const; + /** + * @brief Get the current velocity of the motor + * + * @return AngularVelocity + * + * @b Example: + * @code {.cpp} + * void initialize() { + * lemlib::Motor motor(1, 360_rpm); + * motor.move(.5); + * // get the motor output + * std::cout << motor.getActualVelocity() << std::endl; // output: 180_rpm + * } + * @endcode + */ + AngularVelocity getActualVelocity() const; private: mutable pros::Mutex m_mutex; AngularVelocity m_outputVelocity; diff --git a/include/hardware/Motor/MotorGroup.hpp b/include/hardware/Motor/MotorGroup.hpp index 34e6cf5..7341a84 100644 --- a/include/hardware/Motor/MotorGroup.hpp +++ b/include/hardware/Motor/MotorGroup.hpp @@ -347,6 +347,62 @@ class MotorGroup : public Encoder { * @endcode */ std::vector getTemperatures() const; + /** + * @brief Get the currents of the motors in the motor group + * + * This function uses the following values of errno when an error state is reached: + * + * ENODEV: the port cannot be configured as a motor + * + * @return vector vector of the temperatures of the motors + * @return INFINITY on failure, setting errno + * + * @b Example: + * @code {.cpp} + * void initialize() { + * lemlib::MotorGroup motorGroup({1, -2, 3}, 360_rpm); + * + * // output motor currents to the console + * std::vector currents = motorGroup.getCurrents(); + * for (units::Current c : currents) { + * if (to_amp(c) == INFINITY) { + * std::cout << "Error getting motor current" << std::endl; + * } else { + * std::cout << "Motor Current: " << to_amp(c) << " amps" << std::endl; + * } + * } + * } + * @endcode + */ + std::vector getCurrents() const; + /** + * @brief Get the cartridges of the motors in the motor group + * + * This function uses the following values of errno when an error state is reached: + * + * ENODEV: the port cannot be configured as a motor + * + * @return vector vector of the RPMs of the motors catridges + * @return INFINITY on failure, setting errno + * + * @b Example: + * @code {.cpp} + * void initialize() { + * lemlib::MotorGroup motorGroup({1, -2, 3}, 360_rpm); + * + * // output motor currents to the console + * std::vector cartridges = motorGroup.getMotorCartridges(); + * for (AngularVelocity c : cartridges) { + * if (to_rpm(c) == INFINITY) { + * std::cout << "Error getting motor cartridge" << std::endl; + * } else { + * std::cout << "Motor Cartridge: " << to_rpm(c) << " rpm" << std::endl; + * } + * } + * } + * @endcode + */ + std::vector getMotorCartridges() const; /** * @brief set the output velocity of the motors * @@ -378,6 +434,21 @@ class MotorGroup : public Encoder { * @endcode */ AngularVelocity getOutputVelocity() const; + /** + * @brief Get the actual velocity of the motor group + * + * @return AngularVelocity the actual velocity of the motor group + * + * @b Example: + * @code {.cpp} + * void initialize() { + * lemlib::MotorGroup motorGroup({1, -2, 3}, 360_rpm); + * motorGroup.move(0.5); + * std::cout << "output velocity: " << motorGroup.getActualVelocity() << std::endl; // outputs 180 rpm + * } + * @endcode + */ + AngularVelocity getActualVelocity() const; /** * @brief Get the number of connected motors in the group * @@ -506,6 +577,14 @@ class MotorGroup : public Encoder { * @endcode */ void removeMotor(Motor motor); + /** + * @brief Get motors in the motor group as a vector of lemlib::Motor objects + * + * This function exists to simplify logic in the MotorGroup source code. + * + * @return const std::vector vector of lemlib::Motor objects + */ + const std::vector getMotors() const; private: struct MotorInfo { ReversibleSmartPort port; @@ -531,14 +610,6 @@ class MotorGroup : public Encoder { * @return INT_MAX on failure, setting errno */ Angle configureMotor(ReversibleSmartPort port) const; - /** - * @brief Get motors in the motor group as a vector of lemlib::Motor objects - * - * This function exists to simplify logic in the MotorGroup source code. - * - * @return const std::vector vector of lemlib::Motor objects - */ - const std::vector getMotors() const; /** * @brief Get the Motor Infos * diff --git a/src/hardware/Motor/Motor.cpp b/src/hardware/Motor/Motor.cpp index 8ecdaf2..316ab8a 100644 --- a/src/hardware/Motor/Motor.cpp +++ b/src/hardware/Motor/Motor.cpp @@ -3,6 +3,7 @@ #include "hardware/util.hpp" #include "units/Angle.hpp" #include "pros/device.h" +#include "pros/error.h" #include "pros/motors.h" #include "units/Temperature.hpp" #include "units/units.hpp" @@ -201,6 +202,12 @@ Temperature Motor::getTemperature() const { return result; } +Current Motor::getCurrent() const { + const Current result = from_amp(pros::c::motor_get_current_draw(m_port) / 1000.0); + if (result.internal() == INFINITY) return result; // error checking + return result; +} + // Always returns 0 because the velocity setter is not dependent on hardware and should never fail int32_t Motor::setOutputVelocity(AngularVelocity outputVelocity) { std::lock_guard lock(m_mutex); @@ -214,4 +221,50 @@ AngularVelocity Motor::getOutputVelocity() const { std::lock_guard lock(m_mutex); return m_outputVelocity; } + +AngularVelocity Motor::getMotorCartridge() const { + std::lock_guard lock(m_mutex); + const pros::motor_gearset_e_t mode = pros::c::motor_get_gearing(m_port); + switch (mode) { + case (pros::E_MOTOR_GEARSET_06): { + return 600_rpm; + break; + } + case (pros::E_MOTOR_GEARSET_18): { + return 200_rpm; + break; + } + case (pros::E_MOTOR_GEARSET_36): { + return 100_rpm; + break; + } + default: return from_rpm(INFINITY); + } +} + +AngularVelocity Motor::getActualVelocity() const { + std::lock_guard lock(m_mutex); + const double v = pros::c::motor_get_actual_velocity(m_port); + if (v == PROS_ERR_F) return from_rpm(INFINITY); + const pros::motor_gearset_e_t mode = pros::c::motor_get_gearing(m_port); + AngularVelocity cartridgeSpeed = 0_rpm; + switch (mode) { + case (pros::E_MOTOR_GEARSET_06): { + cartridgeSpeed = 600_rpm; + break; + } + case (pros::E_MOTOR_GEARSET_18): { + cartridgeSpeed = 200_rpm; + break; + } + case (pros::E_MOTOR_GEARSET_36): { + cartridgeSpeed = 100_rpm; + break; + } + default: return from_rpm(INFINITY); + } + + return from_rpm(v) * (m_outputVelocity / cartridgeSpeed); +} + } // namespace lemlib \ No newline at end of file diff --git a/src/hardware/Motor/MotorGroup.cpp b/src/hardware/Motor/MotorGroup.cpp index 5223e46..9b37ece 100644 --- a/src/hardware/Motor/MotorGroup.cpp +++ b/src/hardware/Motor/MotorGroup.cpp @@ -167,6 +167,44 @@ std::vector MotorGroup::getTemperatures() const { return temperatures; } +std::vector MotorGroup::getCurrents() const { + std::lock_guard lock(m_mutex); + std::vector motors = getMotors(); + std::vector currents; + for (const Motor motor : motors) { currents.push_back(motor.getCurrent()); } + return currents; +} + +std::vector MotorGroup::getMotorCartridges() const { + std::lock_guard lock(m_mutex); + std::vector motors = getMotors(); + std::vector cartridges; + for (const Motor motor : motors) { cartridges.push_back(motor.getMotorCartridge()); } + return cartridges; +} + +AngularVelocity MotorGroup::getActualVelocity() const { + std::lock_guard lock(m_mutex); + const std::vector motors = getMotors(); + // get the average angle of all motors in the group + AngularVelocity av = 0_rpm; + int errors = 0; + for (Motor motor : motors) { + // get angle + const AngularVelocity result = motor.getActualVelocity(); + if (result == from_rpm(INFINITY)) { + errors++; + continue; + }; + // add to sum + av += result; + } + // if no motors are connected, return INFINITY + if (errors == motors.size()) return from_rpm(INFINITY); + // otherwise, return the average angle + return av / (motors.size() - errors); +} + // Always returns 0 because the velocity setter is not dependent on hardware and should never fail int32_t MotorGroup::setOutputVelocity(AngularVelocity outputVelocity) { std::lock_guard lock(m_mutex);