From 3e70a1fc74100001e4775d5a4faa33aee5d9d064 Mon Sep 17 00:00:00 2001 From: Michael Ehrenreich Date: Thu, 23 Jun 2022 00:06:50 +0200 Subject: [PATCH 1/4] Pull fixCommonParams and sendCommand out of driving modes --- main/modes.cpp | 5 +++++ main/modes/defaultmode.cpp | 2 -- main/modes/gametrakmode.cpp | 4 ---- main/modes/ignoreinputmode.cpp | 4 ---- main/modes/larsmmode.cpp | 4 ---- main/modes/mickmode.cpp | 4 ---- main/modes/motortestmode.cpp | 3 --- main/modes/remotecontrolmode.cpp | 4 ---- main/modes/tempomatmode.cpp | 4 ---- main/modes/wheelchairmode.cpp | 2 -- 10 files changed, 5 insertions(+), 31 deletions(-) diff --git a/main/modes.cpp b/main/modes.cpp index 7e46328..4be2e63 100644 --- a/main/modes.cpp +++ b/main/modes.cpp @@ -21,4 +21,9 @@ void updateDrivingMode() if (currentMode) currentMode->update(); + + fixCommonParams(); + + // 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 From cf278e4d18f4e53b1980cce9494eede32a6a29d1 Mon Sep 17 00:00:00 2001 From: Michael Ehrenreich Date: Thu, 23 Jun 2022 00:07:23 +0200 Subject: [PATCH 2/4] Add BLE fence --- main/accessors/settingsaccessors.h | 1 + main/displays/menus/blesettingsmenu.cpp | 2 + main/modes.cpp | 2 + main/motorpwmlimiter.h | 41 ++++++++++++++++++++ main/newsettings.h | 5 ++- main/softpwmlimiter.h | 50 +++++++++++++++++++++++++ 6 files changed, 100 insertions(+), 1 deletion(-) create mode 100644 main/motorpwmlimiter.h create mode 100644 main/softpwmlimiter.h 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 4be2e63..2443144 100644 --- a/main/modes.cpp +++ b/main/modes.cpp @@ -2,6 +2,7 @@ // local includes #include "globals.h" +#include "motorpwmlimiter.h" void initDrivingMode() { @@ -23,6 +24,7 @@ void updateDrivingMode() currentMode->update(); fixCommonParams(); + motor_pwm_limiter::update(); // Last, send values to motor controllers sendCommands(); diff --git a/main/motorpwmlimiter.h b/main/motorpwmlimiter.h new file mode 100644 index 0000000..d4ea9fb --- /dev/null +++ b/main/motorpwmlimiter.h @@ -0,0 +1,41 @@ +#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; + } + } +} diff --git a/main/newsettings.h b/main/newsettings.h index fa7d957..f81b2f6 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) \ @@ -808,6 +809,7 @@ public: NEW_SETTINGS(HELPER) #undef HELPER callback(bleSettings.bleEnabled); + callback(bleSettings.bleFenceEnabled); } auto getAllConfigParams() @@ -816,7 +818,8 @@ public: #define HELPER(x) std::ref(x), NEW_SETTINGS(HELPER) #undef HELPER - std::ref(bleSettings.bleEnabled) + std::ref(bleSettings.bleEnabled), + std::ref(bleSettings.bleFenceEnabled) ); } diff --git a/main/softpwmlimiter.h b/main/softpwmlimiter.h new file mode 100644 index 0000000..1318058 --- /dev/null +++ b/main/softpwmlimiter.h @@ -0,0 +1,50 @@ +#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; + } + + inline bool trigger = false; + + void update(float current_pwm) { + if (trigger) { + auto now = espchrono::millis_clock::now(); + if (!active) { + last_update = now; + active = true; + return; + } + + 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); + } +} From 0843e4881b3ca30202194677514992d4d4b31a5c Mon Sep 17 00:00:00 2001 From: CommanderRedYT Date: Thu, 23 Jun 2022 19:02:04 +0200 Subject: [PATCH 3/4] Fixes --- main/CMakeLists.txt | 4 +++ main/modes.cpp | 1 + main/motorpwmlimiter.cpp | 47 ++++++++++++++++++++++++++++++++++ main/motorpwmlimiter.h | 40 ++--------------------------- main/newsettings.h | 6 ++--- main/softpwmlimiter.cpp | 55 ++++++++++++++++++++++++++++++++++++++++ main/softpwmlimiter.h | 47 +++------------------------------- 7 files changed, 115 insertions(+), 85 deletions(-) create mode 100644 main/motorpwmlimiter.cpp create mode 100644 main/softpwmlimiter.cpp 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 From 08e2f6e4d369b218a6c6befa39f902189c4d8fb2 Mon Sep 17 00:00:00 2001 From: CommanderRedYT Date: Thu, 23 Jun 2022 19:58:35 +0200 Subject: [PATCH 4/4] Fixed crash --- main/motorpwmlimiter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/main/motorpwmlimiter.cpp b/main/motorpwmlimiter.cpp index 2bc1c43..e97d601 100644 --- a/main/motorpwmlimiter.cpp +++ b/main/motorpwmlimiter.cpp @@ -9,7 +9,7 @@ namespace motor_pwm_limiter { void update() { - if (!configs.bleSettings.bleFenceEnabled.value() || pServer->getPeerDevices().size()) + if (!configs.bleSettings.bleEnabled.value() || !configs.bleSettings.bleFenceEnabled.value() || (pServer && !pServer->getPeerDevices().empty())) { soft_pwm_limiter::trigger = false; soft_pwm_limiter::update();