From 0e5f13b88b6d6046b711688257d3798d5c07c7d7 Mon Sep 17 00:00:00 2001 From: toxicdefender404 Date: Sat, 30 Nov 2024 23:50:28 +0000 Subject: [PATCH 1/2] smart current limit --- include/hardware/Motor/Motor.hpp | 2 ++ src/hardware/Motor/Motor.cpp | 57 ++++++++++++++++++++++++++++++-- 2 files changed, 56 insertions(+), 3 deletions(-) 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..e8274b3 100644 --- a/src/hardware/Motor/Motor.cpp +++ b/src/hardware/Motor/Motor.cpp @@ -162,10 +162,61 @@ 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; + } + } + if (power <= 88_watt) return thisLimit; + + //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) return defaultLimit; + return thisLimit; } int Motor::setCurrentLimit(Current limit) { return pros::c::motor_set_current_limit(m_port, to_amp(limit) * 1000); } From bee3817b4da273670b2c8188a084c103e9d2d2ba Mon Sep 17 00:00:00 2001 From: toxicdefender404 Date: Sun, 1 Dec 2024 11:11:46 +0000 Subject: [PATCH 2/2] round limits to 2dp --- src/hardware/Motor/Motor.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/hardware/Motor/Motor.cpp b/src/hardware/Motor/Motor.cpp index e8274b3..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 { @@ -191,7 +192,11 @@ Current Motor::getCurrentLimit() const { else power += 5.5_watt; } } - if (power <= 88_watt) return thisLimit; + //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()); @@ -215,8 +220,12 @@ Current Motor::getCurrentLimit() const { if (defaultLimit > maxLimit) defaultLimit = maxLimit; //check if set limit should apply - if(thisLimit > defaultLimit) return defaultLimit; - return thisLimit; + 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); }