Skip to content

Add more functions to Motors and MotorGroups #23

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
OliverCieslak wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Add more functions to Motors and MotorGroups #23

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
58 changes: 58 additions & 0 deletions include/hardware/Motor/Motor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
*
Expand Down Expand Up @@ -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;
Expand Down
87 changes: 79 additions & 8 deletions include/hardware/Motor/MotorGroup.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -347,6 +347,62 @@ class MotorGroup : public Encoder {
* @endcode
*/
std::vector<Temperature> 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<Temperature> 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<Current> 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<Current> 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<AngularVelocity> 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<AngularVelocity> 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<AngularVelocity> getMotorCartridges() const;
/**
* @brief set the output velocity of the motors
*
Expand Down Expand Up @@ -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
*
Expand Down Expand Up @@ -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<Motor> vector of lemlib::Motor objects
*/
const std::vector<Motor> getMotors() const;
private:
struct MotorInfo {
ReversibleSmartPort port;
Expand All @@ -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<Motor> vector of lemlib::Motor objects
*/
const std::vector<Motor> getMotors() const;
/**
* @brief Get the Motor Infos
*
Expand Down
53 changes: 53 additions & 0 deletions src/hardware/Motor/Motor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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);
Expand All @@ -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
38 changes: 38 additions & 0 deletions src/hardware/Motor/MotorGroup.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -167,6 +167,44 @@ std::vector<Temperature> MotorGroup::getTemperatures() const {
return temperatures;
}

std::vector<Current> MotorGroup::getCurrents() const {
std::lock_guard lock(m_mutex);
std::vector<Motor> motors = getMotors();
std::vector<Current> currents;
for (const Motor motor : motors) { currents.push_back(motor.getCurrent()); }
return currents;
}

std::vector<AngularVelocity> MotorGroup::getMotorCartridges() const {
std::lock_guard lock(m_mutex);
std::vector<Motor> motors = getMotors();
std::vector<AngularVelocity> 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<Motor> 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);
Expand Down