From cf278e4d18f4e53b1980cce9494eede32a6a29d1 Mon Sep 17 00:00:00 2001 From: Michael Ehrenreich Date: Thu, 23 Jun 2022 00:07:23 +0200 Subject: [PATCH] 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); + } +}