diff --git a/include/hardware/Motor/Motor.hpp b/include/hardware/Motor/Motor.hpp index a914055..9b5c4b2 100644 --- a/include/hardware/Motor/Motor.hpp +++ b/include/hardware/Motor/Motor.hpp @@ -496,6 +496,8 @@ class Motor : public Encoder { */ int setOutputVelocity(AngularVelocity outputVelocity); private: + Current motorToBatt(Current current) const; + Current battToMotor(Current current) const; AngularVelocity m_outputVelocity; Angle m_offset = 0_stDeg; int m_port; diff --git a/src/hardware/Motor/Motor.cpp b/src/hardware/Motor/Motor.cpp index 1a85a6c..a84ad30 100644 --- a/src/hardware/Motor/Motor.cpp +++ b/src/hardware/Motor/Motor.cpp @@ -5,6 +5,7 @@ #include "pros/motors.h" #include "units/Temperature.hpp" #include "units/units.hpp" +#include #include namespace lemlib { @@ -162,10 +163,69 @@ int Motor::setReversed(bool reversed) { int Motor::getPort() const { return m_port; } +NEW_METRIC_PREFIXES(Current, amp) //temporary until added to units + +Current Motor::motorToBatt(Current current) const { + return from_amp(exp(current.convert(amp) + 4.324) / 0.925); +} + +Current Motor::battToMotor(Current current) const { + return from_amp(0.925 * log(current.convert(amp)) - 4.324); +} + Current Motor::getCurrentLimit() const { - const Current result = from_amp(pros::c::motor_get_current_limit(m_port)); - if (result.internal() == INT32_MAX) return from_amp(INFINITY); // error checking - return result; + const Current maxCurrent = 12.8_amp; + const Current maxLimit = 2.5_amp; + + const Current thisLimit = from_mamp(pros::c::motor_get_current_limit(m_port)); + if (thisLimit.internal() == INT32_MAX) return from_amp(INFINITY); // error checking + + //gets the ports of all the motors + std::vector ports; + Power power = 0_watt; + for (int i = 0; i < 20; i++) { + Motor m(i,200_rpm); + if (!m.isConnected()) continue; + else { + ports.push_back(i); + if(m.getType()==MotorType::V5) power += 11_watt; + else power += 5.5_watt; + } + } + //default behaviour if <= 88 watt + if (power <= 88_watt) { + //round thisLimit to 2dp + return from_amp(std::round(to_amp(thisLimit)*100)/100); + } + + //limit based on number of motors + Current defaultLimit = battToMotor(maxCurrent/ports.size()); + if (defaultLimit > maxLimit) defaultLimit = maxLimit; + + //sum the limited motors + Current totalLimit = 0_amp; + int numLimited = 0; + for (int i = 0; i < ports.size(); i++) { + const Current limit = from_mamp(pros::c::motor_get_current_limit(ports[i])); + if (limit.internal() == INT32_MAX) return from_amp(INFINITY); // error checking + + if (limit < defaultLimit){ + totalLimit += motorToBatt(limit); + numLimited++; + } + } + //adjust limit based on set limits + const Current battCurrentPerMotor = (maxCurrent - totalLimit)/(ports.size()-numLimited); + defaultLimit = battToMotor(battCurrentPerMotor); + if (defaultLimit > maxLimit) defaultLimit = maxLimit; + + //check if set limit should apply + if(thisLimit > defaultLimit){ + //round defaultLimit to 2dp + return from_amp(std::round(to_amp(defaultLimit)*100)/100); + } + //round thisLimit to 2dp + return from_amp(std::round(to_amp(thisLimit)*100)/100); } int Motor::setCurrentLimit(Current limit) { return pros::c::motor_set_current_limit(m_port, to_amp(limit) * 1000); }