diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 8edd997..3930740 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -215,6 +215,7 @@ set(headers modes/tempomatmode.h modes/wheelchairmode.h mosfets.h + motorpwmlimiter.h newsettings.h ota.h potis.h @@ -226,6 +227,7 @@ set(headers serial_bobby.h settingspersister.h settingsutils.h + softpwmlimiter.h statistics.h statustexthelper.h taskmanager.h @@ -458,6 +460,7 @@ set(sources modes/tempomatmode.cpp modes/wheelchairmode.cpp mosfets.cpp + motorpwmlimiter.cpp newsettings.cpp ota.cpp potis.cpp @@ -469,6 +472,7 @@ set(sources serial_bobby.cpp settingspersister.cpp settingsutils.cpp + softpwmlimiter.cpp statistics.cpp statustexthelper.cpp taskmanager.cpp diff --git a/main/modes.cpp b/main/modes.cpp index 2443144..3f449e6 100644 --- a/main/modes.cpp +++ b/main/modes.cpp @@ -3,6 +3,7 @@ // local includes #include "globals.h" #include "motorpwmlimiter.h" +#include "utils.h" void initDrivingMode() { diff --git a/main/motorpwmlimiter.cpp b/main/motorpwmlimiter.cpp new file mode 100644 index 0000000..2bc1c43 --- /dev/null +++ b/main/motorpwmlimiter.cpp @@ -0,0 +1,47 @@ +#include "motorpwmlimiter.h" + +// local includes +#include "ble_bobby.h" +#include "globals.h" +#include "softpwmlimiter.h" +#include "utils.h" + +namespace motor_pwm_limiter { +void update() +{ + if (!configs.bleSettings.bleFenceEnabled.value() || pServer->getPeerDevices().size()) + { + soft_pwm_limiter::trigger = false; + soft_pwm_limiter::update(); + return; + } + + soft_pwm_limiter::trigger = true; + + const auto mot = motors(); + + int max_pwm = 0; + for (const bobbycar::protocol::serial::MotorState &motor : mot) + { + max_pwm = std::max(max_pwm, std::abs(motor.pwm)); + } + + // Scale PWM proportionally to avoid loss of steering in case of wheelchair or RC mode + soft_pwm_limiter::update(); + float new_max = soft_pwm_limiter::limit(max_pwm); + float ratio; + if (max_pwm > 0.f) + ratio = new_max / max_pwm; + else + ratio = 0.f; + + // Should not happen + if (ratio > 1.f) + ratio = 1.f; + + for (bobbycar::protocol::serial::MotorState &motor : mot) + { + motor.pwm *= ratio; + } +} +} diff --git a/main/motorpwmlimiter.h b/main/motorpwmlimiter.h index d4ea9fb..abf9a7d 100644 --- a/main/motorpwmlimiter.h +++ b/main/motorpwmlimiter.h @@ -1,41 +1,5 @@ #pragma once -#include "globals.h" -#include "utils.h" -#include "softpwmlimiter.h" -#include "ble_bobby.h" - namespace motor_pwm_limiter { - void update() { - if (!configs.bleSettings.bleFenceEnabled.value() || pServer->getPeerDevices().size()) { - soft_pwm_limiter::trigger = false; - soft_pwm_limiter::update(1500.f); - return; - } - - soft_pwm_limiter::trigger = true; - - auto mot = motors(); - int max_pwm = 0; - for (bobbycar::protocol::serial::MotorState &motor : mot) { - max_pwm = std::max(max_pwm, std::abs(motor.pwm)); - } - - // Scale PWM proportionally to avoid loss of steering in case of wheelchair or RC mode - soft_pwm_limiter::update(max_pwm); - float new_max = soft_pwm_limiter::limit(max_pwm); - float ratio; - if (max_pwm > 0.f) - ratio = new_max / max_pwm; - else - ratio = 0.f; - - // Should not happen - if (ratio > 1.f) - ratio = 1.f; - - for (bobbycar::protocol::serial::MotorState &motor : mot) { - motor.pwm = motor.pwm * ratio; - } - } -} +void update(); +} // namespace motor_pwm_limiter diff --git a/main/newsettings.h b/main/newsettings.h index f81b2f6..c3311bd 100644 --- a/main/newsettings.h +++ b/main/newsettings.h @@ -785,8 +785,8 @@ public: x(feature.ota.isEnabled) \ x(feature.udpcloud.isEnabled) \ x(feature.webserver.isEnabled) \ - x(feature.webserver_disable_lock.isEnabled) - //x(bleSettings.bleEnabled) + x(feature.webserver_disable_lock.isEnabled) \ + x(bleSettings.bleEnabled) #define FEATURES(x) \ x(feature.ble) \ @@ -808,7 +808,6 @@ public: #define HELPER(x) callback(x); NEW_SETTINGS(HELPER) #undef HELPER - callback(bleSettings.bleEnabled); callback(bleSettings.bleFenceEnabled); } @@ -818,7 +817,6 @@ public: #define HELPER(x) std::ref(x), NEW_SETTINGS(HELPER) #undef HELPER - std::ref(bleSettings.bleEnabled), std::ref(bleSettings.bleFenceEnabled) ); } diff --git a/main/softpwmlimiter.cpp b/main/softpwmlimiter.cpp new file mode 100644 index 0000000..0a4ee29 --- /dev/null +++ b/main/softpwmlimiter.cpp @@ -0,0 +1,55 @@ +#include "softpwmlimiter.h" + +// system includes +#include + +// 3rdparty lib includes +#include + +namespace soft_pwm_limiter { +constexpr float MAX_LIMIT = 1500.f; + +namespace { +float actual_limit = MAX_LIMIT; +bool active = false; +espchrono::millis_clock::time_point last_update; +} // namespace + +bool trigger = false; + +void update() +{ + using namespace std::chrono_literals; + + if (trigger) + { + const auto now = espchrono::millis_clock::now(); + if (!active) + { + last_update = now; + active = true; + return; + } + + // int time_delta = std::chrono::floor(now - last_update).count(); + int time_delta = espchrono::ago(last_update) / 1ms; + last_update = now; + time_delta = std::max(time_delta, 0); + + // Exit field weakening area ]1000, 1500] within 2 seconds + actual_limit -= 500.f * time_delta / 2000.f; + actual_limit = std::max(actual_limit, 0.f); + } + else + { + actual_limit = MAX_LIMIT; + active = false; + } +} + +float limit(float pwm) +{ + return std::clamp(pwm, -actual_limit, actual_limit); +} + +} // namespace soft_pwm_limiter diff --git a/main/softpwmlimiter.h b/main/softpwmlimiter.h index 1318058..1863ae1 100644 --- a/main/softpwmlimiter.h +++ b/main/softpwmlimiter.h @@ -1,50 +1,11 @@ #pragma once -#include -#include - -#include - namespace soft_pwm_limiter { - namespace { - constexpr float MAX_LIMIT = 1500.f; - //inline constexpr float TAU = 5000.f; - inline float actual_limit = MAX_LIMIT; - inline bool active = false; - inline espchrono::millis_clock::time_point last_update; - } +extern bool trigger; - inline bool trigger = false; +void update(); - void update(float current_pwm) { - if (trigger) { - auto now = espchrono::millis_clock::now(); - if (!active) { - last_update = now; - active = true; - return; - } +float limit(float pwm); - float time_delta = std::chrono::floor(now - last_update).count(); - last_update = now; - time_delta = std::max(time_delta, 0.f); - - //float smoothness = expf(-time_delta / TAU); - - //actual_limit = actual_limit * smoothness; - // Exit field weakening area (]1000, 1500]) within 2 seconds - actual_limit -= 500.f * time_delta / 2000.f; - actual_limit = std::max(actual_limit, 0.f); - //if (current_pwm >= -1500.f && current_pwm <= 1500.f) - // actual_limit = std::min(actual_limit, std::abs(current_pwm)); - } else { - actual_limit = MAX_LIMIT; - active = false; - } - } - - float limit(float pwm) { - return std::clamp(pwm, -actual_limit, actual_limit); - } -} +} // namespace soft_pwm_limiter