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/accessors/settingsaccessors.h b/main/accessors/settingsaccessors.h index 2eae0c0..242ffa2 100644 --- a/main/accessors/settingsaccessors.h +++ b/main/accessors/settingsaccessors.h @@ -39,6 +39,7 @@ struct PhaseAdvMaxAccessor : public RefAccessorSaveSettings { int16_t & // Bluetooth Low Energy struct BleEnabledAccessor : public NewSettingsAccessor { ConfigWrapper &getConfig() const override { return configs.bleSettings.bleEnabled; } }; +struct BleFenceEnabledAccessor : public NewSettingsAccessor { ConfigWrapper &getConfig() const override { return configs.bleSettings.bleFenceEnabled; } }; // Cloud struct CloudEnabledAccessor : public NewSettingsAccessor { ConfigWrapper &getConfig() const override { return configs.cloudSettings.cloudEnabled; } }; diff --git a/main/displays/menus/blesettingsmenu.cpp b/main/displays/menus/blesettingsmenu.cpp index 19480a6..c98a0b1 100644 --- a/main/displays/menus/blesettingsmenu.cpp +++ b/main/displays/menus/blesettingsmenu.cpp @@ -19,6 +19,7 @@ namespace { constexpr char TEXT_BLESETTINGS[] = "BLE settings"; constexpr char TEXT_ENABLED[] = "Enabled"; +constexpr char TEXT_FENCE_ENABLED[] = "Fence enabled"; constexpr char TEXT_NAME[] = "Name"; constexpr char TEXT_NAME_FORMATTED[] = "Name: &s"; constexpr char TEXT_BACK[] = "Back"; @@ -36,6 +37,7 @@ BleSettingsMenu::BleSettingsMenu() { using namespace espgui; constructMenuItem, BobbyCheckbox, BleEnabledAccessor>>(); + constructMenuItem, BobbyCheckbox, BleFenceEnabledAccessor>>(); constructMenuItem>(); constructMenuItem>(); constructMenuItem, PushScreenAction>>(); diff --git a/main/modes.cpp b/main/modes.cpp index 7e46328..3f449e6 100644 --- a/main/modes.cpp +++ b/main/modes.cpp @@ -2,6 +2,8 @@ // local includes #include "globals.h" +#include "motorpwmlimiter.h" +#include "utils.h" void initDrivingMode() { @@ -21,4 +23,10 @@ void updateDrivingMode() if (currentMode) currentMode->update(); + + fixCommonParams(); + motor_pwm_limiter::update(); + + // Last, send values to motor controllers + sendCommands(); } diff --git a/main/modes/defaultmode.cpp b/main/modes/defaultmode.cpp index 8473d25..9750a26 100644 --- a/main/modes/defaultmode.cpp +++ b/main/modes/defaultmode.cpp @@ -258,8 +258,6 @@ void DefaultMode::update() motor.cruiseCtrlEna = false; motor.nCruiseMotTgt = 0; } - fixCommonParams(); } } - sendCommands(); } diff --git a/main/modes/gametrakmode.cpp b/main/modes/gametrakmode.cpp index b714c85..62aeb0d 100644 --- a/main/modes/gametrakmode.cpp +++ b/main/modes/gametrakmode.cpp @@ -76,9 +76,5 @@ void GametrakMode::update() motor.nCruiseMotTgt = 0; } } - - fixCommonParams(); - - sendCommands(); } #endif diff --git a/main/modes/ignoreinputmode.cpp b/main/modes/ignoreinputmode.cpp index 5f87739..5399597 100644 --- a/main/modes/ignoreinputmode.cpp +++ b/main/modes/ignoreinputmode.cpp @@ -14,8 +14,4 @@ void IgnoreInputMode::update() motor.cruiseCtrlEna = false; motor.nCruiseMotTgt = 0; } - - fixCommonParams(); - - sendCommands(); } diff --git a/main/modes/larsmmode.cpp b/main/modes/larsmmode.cpp index 9d7decb..828fcec 100644 --- a/main/modes/larsmmode.cpp +++ b/main/modes/larsmmode.cpp @@ -105,8 +105,4 @@ void LarsmMode::update() motor.nCruiseMotTgt = 0; } } - - fixCommonParams(); - - sendCommands(); } diff --git a/main/modes/mickmode.cpp b/main/modes/mickmode.cpp index 255533a..cc5efce 100644 --- a/main/modes/mickmode.cpp +++ b/main/modes/mickmode.cpp @@ -78,8 +78,4 @@ void MickMode::update() motor.nCruiseMotTgt = 0; } } - - fixCommonParams(); - - sendCommands(); } diff --git a/main/modes/motortestmode.cpp b/main/modes/motortestmode.cpp index 6dbea6a..03b36d4 100644 --- a/main/modes/motortestmode.cpp +++ b/main/modes/motortestmode.cpp @@ -34,7 +34,4 @@ void MotortestMode::update() motor.cruiseCtrlEna = false; motor.nCruiseMotTgt = 0; } - - fixCommonParams(); - sendCommands(); } diff --git a/main/modes/remotecontrolmode.cpp b/main/modes/remotecontrolmode.cpp index 6b2c9a2..1dbe9f2 100644 --- a/main/modes/remotecontrolmode.cpp +++ b/main/modes/remotecontrolmode.cpp @@ -42,10 +42,6 @@ void RemoteControlMode::update() controllers.back.command.left.pwm = m_remoteCommand->backLeft; controllers.back.command.right.pwm = m_remoteCommand->backRight; } - - fixCommonParams(); - - sendCommands(); } void RemoteControlMode::setCommand(const RemoteCommand &command) diff --git a/main/modes/tempomatmode.cpp b/main/modes/tempomatmode.cpp index aa227ab..51cdba2 100644 --- a/main/modes/tempomatmode.cpp +++ b/main/modes/tempomatmode.cpp @@ -53,8 +53,4 @@ void TempomatMode::update() motor.nCruiseMotTgt = nCruiseMotTgt; } } - - fixCommonParams(); - - sendCommands(); } diff --git a/main/modes/wheelchairmode.cpp b/main/modes/wheelchairmode.cpp index 13db7a8..84325c4 100644 --- a/main/modes/wheelchairmode.cpp +++ b/main/modes/wheelchairmode.cpp @@ -219,9 +219,7 @@ void WheelchairMode::update() controller.command.right.cruiseCtrlEna = false; controller.command.right.nCruiseMotTgt = 0; } - fixCommonParams(); } } - sendCommands(); } #endif diff --git a/main/motorpwmlimiter.cpp b/main/motorpwmlimiter.cpp new file mode 100644 index 0000000..e97d601 --- /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.bleEnabled.value() || !configs.bleSettings.bleFenceEnabled.value() || (pServer && !pServer->getPeerDevices().empty())) + { + 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 new file mode 100644 index 0000000..abf9a7d --- /dev/null +++ b/main/motorpwmlimiter.h @@ -0,0 +1,5 @@ +#pragma once + +namespace motor_pwm_limiter { +void update(); +} // namespace motor_pwm_limiter diff --git a/main/newsettings.h b/main/newsettings.h index fa7d957..c3311bd 100644 --- a/main/newsettings.h +++ b/main/newsettings.h @@ -471,6 +471,7 @@ public: struct { ConfigWrapperLegacy bleEnabled {true, DoReset, {}, "bleEnabled" }; + ConfigWrapperLegacy bleFenceEnabled {false, DoReset, {}, "bleFenceEnabled" }; } bleSettings; #define NEW_SETTINGS(x) \ @@ -784,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) \ @@ -807,7 +808,7 @@ public: #define HELPER(x) callback(x); NEW_SETTINGS(HELPER) #undef HELPER - callback(bleSettings.bleEnabled); + callback(bleSettings.bleFenceEnabled); } auto getAllConfigParams() @@ -816,7 +817,7 @@ 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 new file mode 100644 index 0000000..1863ae1 --- /dev/null +++ b/main/softpwmlimiter.h @@ -0,0 +1,11 @@ +#pragma once + +namespace soft_pwm_limiter { + +extern bool trigger; + +void update(); + +float limit(float pwm); + +} // namespace soft_pwm_limiter