Merge pull request #293 from bobbycar-graz/mick-mode

Add mick mode

main/CMakeLists.txt

@@ -116,6 +116,7 @@ set(headers
     displays/menus/lockscreensettingsmenu.h
     displays/menus/mainmenu.h
     displays/menus/manageprofilesmenu.h
+    displays/menus/mickmodesettingsmenu.h
     displays/menus/modessettingsmenu.h
     displays/menus/mosfetsmenu.h
     displays/menus/motorfeedbackdebugmenu.h
@@ -203,6 +204,7 @@ set(headers
     modes/gametrakmode.h
     modes/ignoreinputmode.h
     modes/larsmmode.h
+    modes/mickmode.h
     modes/motortestmode.h
     modes/remotecontrolmode.h
     modes/tempomatmode.h
@@ -351,6 +353,7 @@ set(sources
     displays/menus/lockscreensettingsmenu.cpp
     displays/menus/mainmenu.cpp
     displays/menus/manageprofilesmenu.cpp
+    displays/menus/mickmodesettingsmenu.cpp
     displays/menus/modessettingsmenu.cpp
     displays/menus/mosfetsmenu.cpp
     displays/menus/motorfeedbackdebugmenu.cpp
@@ -439,6 +442,7 @@ set(sources
     modes/gametrakmode.cpp
     modes/ignoreinputmode.cpp
     modes/larsmmode.cpp
+    modes/mickmode.cpp
     modes/motortestmode.cpp
     modes/remotecontrolmode.cpp
     modes/tempomatmode.cpp

main/accessors/settingsaccessors.h

@@ -160,6 +160,9 @@ struct LarsmModeIterationsAccessor : public RefAccessorSaveSettings<uint8_t> { u
 struct MotortestModeMultiplikatorAccessor : public RefAccessorSaveSettings<uint8_t> { uint8_t &getRef() const override { return profileSettings.motortestMode.multiplikator; } };
 struct MotortestMaxPwmAccessor : public RefAccessorSaveSettings<uint16_t> { uint16_t &getRef() const override { return profileSettings.motortestMode.maxPwm; } };
 
+struct MickModeModelModeAccessor : public RefAccessorSaveSettings<UnifiedModelMode> { UnifiedModelMode &getRef() const override { return profileSettings.mickMode.modelMode; } };
+struct MickModeSmoothingAccessor : public RefAccessorSaveSettings<uint16_t> { uint16_t &getRef() const override { return profileSettings.mickMode.smoothing; } };
+
 // Ledstrip
 struct EnableLedAnimationAccessor : public NewSettingsAccessor<bool> { ConfigWrapper<bool> &getConfig() const override { return configs.ledstrip.enableLedAnimation; } };
 struct EnableBrakeLightsAccessor : public NewSettingsAccessor<bool> { ConfigWrapper<bool> &getConfig() const override { return configs.ledstrip.enableBrakeLights; } };

main/actions/modesettingsaction.h

@@ -3,6 +3,7 @@
 #include "actioninterface.h"
 #include "globals.h"
 #include "modes/defaultmode.h"
+#include "modes/mickmode.h"
 #include "modes/tempomatmode.h"
 #include "modes/larsmmode.h"
 #include "modes/gametrakmode.h"
@@ -14,6 +15,7 @@
 #include "displays/menus/tempomatmodesettingsmenu.h"
 #include "displays/menus/larsmmodesettingsmenu.h"
 #include "displays/menus/gametrakmodesettingsmenu.h"
+#include "displays/menus/mickmodesettingsmenu.h"
 #include "displays/menus/motortestmodesettingsmenu.h"
 #include "screenmanager.h"
 
@@ -39,6 +41,8 @@ void ModeSettingsAction::triggered()
         switchScreen<TempomatModeSettingsMenu>();
     else if (currentMode == &modes::larsmMode)
         switchScreen<LarsmModeSettingsMenu>();
+    else if (currentMode == &modes::mickMode)
+        switchScreen<MickModeSettingsMenu>();
     else if (currentMode == &modes::motortestMode)
         switchScreen<MotortestModeSettingsMenu>();
 #ifdef FEATURE_GAMETRAK

main/displays/menus/mickmodesettingsmenu.cpp

@@ -0,0 +1,55 @@
+#include "mickmodesettingsmenu.h"
+
+// 3rdparty lib includes
+#include "changevaluedisplay.h"
+#include "menuitem.h"
+#include "actions/switchscreenaction.h"
+#include "icons/back.h"
+
+// local includes
+#include "displays/bobbychangevaluedisplay.h"
+#include "utils.h"
+#include "changevaluedisplay_unifiedmodelmode.h"
+#include "accessors/settingsaccessors.h"
+#include "displays/menus/modessettingsmenu.h"
+
+namespace {
+constexpr char TEXT_MICKMODESETTINGS[] = "Mick mode settings";
+constexpr char TEXT_MODELMODE[] = "Model mode";
+constexpr char TEXT_MICKMODE_SMOOTHING[] = "Tau in ms";
+constexpr char TEXT_BACK[] = "Back";
+
+using MickModeModelModeChangeDisplay = espgui::makeComponent<
+    BobbyChangeValueDisplay<UnifiedModelMode>,
+    espgui::StaticText<TEXT_MODELMODE>,
+    MickModeModelModeAccessor,
+    espgui::ConfirmActionInterface<espgui::SwitchScreenAction<MickModeSettingsMenu>>,
+    espgui::BackActionInterface<espgui::SwitchScreenAction<MickModeSettingsMenu>>
+>;
+using MickModeSmoothingChangeDisplay = espgui::makeComponent<
+    BobbyChangeValueDisplay<uint16_t>,
+    espgui::StaticText<TEXT_MICKMODE_SMOOTHING>,
+    MickModeSmoothingAccessor,
+    espgui::ConfirmActionInterface<espgui::SwitchScreenAction<MickModeSettingsMenu>>,
+    espgui::BackActionInterface<espgui::SwitchScreenAction<MickModeSettingsMenu>>
+>;
+} // namespace
+
+using namespace espgui;
+
+MickModeSettingsMenu::MickModeSettingsMenu()
+{
+    constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_MODELMODE>,          SwitchScreenAction<MickModeModelModeChangeDisplay>>>();
+    constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_MICKMODE_SMOOTHING>, SwitchScreenAction<MickModeSmoothingChangeDisplay>>>();
+    constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_BACK>,               SwitchScreenAction<ModesSettingsMenu>, StaticMenuItemIcon<&espgui::icons::back>>>();
+}
+
+std::string MickModeSettingsMenu::text() const
+{
+    return TEXT_MICKMODESETTINGS;
+}
+
+void MickModeSettingsMenu::back()
+{
+    switchScreen<ModesSettingsMenu>();
+}

main/displays/menus/mickmodesettingsmenu.h

@@ -0,0 +1,14 @@
+#pragma once
+
+// local includes
+#include "displays/bobbymenudisplay.h"
+
+class MickModeSettingsMenu : public BobbyMenuDisplay
+{
+public:
+    MickModeSettingsMenu();
+
+    std::string text() const override;
+
+    void back() override;
+};

main/displays/menus/selectmodemenu.cpp

@@ -14,6 +14,7 @@
 #include "modes/larsmmode.h"
 #include "modes/remotecontrolmode.h"
 #include "modes/gametrakmode.h"
+#include "modes/mickmode.h"
 #include "modes/motortestmode.h"
 #ifdef FEATURE_JOYSTICK
 #include "modes/wheelchairmode.h"
@@ -26,6 +27,7 @@ constexpr char TEXT_SELECTMODE[] = "Select mode";
 constexpr char TEXT_DEFAULT[] = "Default";
 constexpr char TEXT_TEMPOMAT[] = "Tempomat";
 constexpr char TEXT_LARSM[] = "Larsm";
+constexpr char TEXT_MICK[] = "mick";
 constexpr char TEXT_REMOTECONTROL[] = "Remote control";
 constexpr char TEXT_GAMETRAK[] = "Gametrak";
 constexpr char TEXT_MOTORTEST[] = "Motortest";
@@ -44,6 +46,7 @@ using SetDefaultModeAction = SetterAction<ModeInterface*, currentMode, DefaultMo
 using SetTempomatModeAction = SetterAction<ModeInterface*, currentMode, TempomatMode*, &modes::tempomatMode>;
 using SetLarsmModeAction = SetterAction<ModeInterface*, currentMode, LarsmMode*, &modes::larsmMode>;
 using SetRemoteControlModeAction = SetterAction<ModeInterface*, currentMode, RemoteControlMode*, &modes::remoteControlMode>;
+using SetMickModeAction = SetterAction<ModeInterface*, currentMode, MickMode*, &modes::mickMode>;
 using SetMotorTestModeAction = SetterAction<ModeInterface*, currentMode, MotortestMode*, &modes::motortestMode>;
 #ifdef FEATURE_GAMETRAK
 using SetGametrakModeAction = SetterAction<ModeInterface*, currentMode, GametrakMode*, &modes::gametrakMode>;
@@ -60,6 +63,7 @@ SelectModeMenu::SelectModeMenu()
     constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_DEFAULT>,       MultiAction<SetDefaultModeAction, SwitchScreenAction<MainMenu>>>>();
     if (!simplified) { constructMenuItem<makeComponent<MenuItem, TextWithValueHelper<TEXT_TEMPOMAT, AvgSpeedAccessor>,  MultiAction<SetTempomatModeAction, SwitchScreenAction<MainMenu>>>>(); }
     constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_LARSM>,         MultiAction<SetLarsmModeAction, SwitchScreenAction<MainMenu>>>>();
+    constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_MICK>,         MultiAction<SetMickModeAction, SwitchScreenAction<MainMenu>>>>();
     if (!simplified) { constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_REMOTECONTROL>, MultiAction<SetRemoteControlModeAction, SwitchScreenAction<MainMenu>>>>(); }
 #ifdef FEATURE_GAMETRAK
     constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_GAMETRAK>,  MultiAction<SetGametrakModeAction, SwitchScreenAction<MainMenu>>>>();
@@ -86,13 +90,15 @@ void SelectModeMenu::start()
         setSelectedIndex(1);
     else if (currentMode == &modes::larsmMode)
         setSelectedIndex(2);
-    else if (currentMode == &modes::remoteControlMode)
+    else if (currentMode == &modes::mickMode)
         setSelectedIndex(3);
-    else if (currentMode == &modes::motortestMode)
+    else if (currentMode == &modes::remoteControlMode)
         setSelectedIndex(4);
+    else if (currentMode == &modes::motortestMode)
+        setSelectedIndex(5);
 #ifdef FEATURE_JOYSTICK
     else if (currentMode == &modes::wheelchairMode)
-        setSelectedIndex(5);
+        setSelectedIndex(6);
 #endif
     else
     {
@@ -100,7 +106,7 @@ void SelectModeMenu::start()
 #ifdef FEATURE_JOYSTICK
         setSelectedIndex(6);
 #else
-        setSelectedIndex(4);
+        setSelectedIndex(5);
 #endif
     }
 }

main/modes/mickmode.cpp

@@ -0,0 +1,66 @@
+#include <cmath>
+
+#include "espchrono.h"
+#include "mickmode.h"
+
+// local includes
+#include "globals.h"
+#include "utils.h"
+
+namespace modes {
+MickMode mickMode;
+} // namespace modes
+
+void MickMode::start()
+{
+    Base::start();
+
+    pwm_ = 0.f;
+}
+
+void MickMode::update()
+{
+    if (!gas || !brems)
+    {
+        start();
+
+        for (bobbycar::protocol::serial::MotorState &motor : motors())
+        {
+            motor.ctrlTyp = bobbycar::protocol::ControlType::FieldOrientedControl;
+            motor.ctrlMod = bobbycar::protocol::ControlMode::OpenMode;
+            motor.pwm = 0;
+            motor.cruiseCtrlEna = false;
+            motor.nCruiseMotTgt = 0;
+        }
+    }
+    else
+    {
+        auto now = espchrono::millis_clock::now();
+        float timeDelta = std::chrono::floor<std::chrono::milliseconds>(now - lastUpdate_).count();
+        lastUpdate_ = now;
+
+        float alpha = 1.f - expf(-timeDelta / profileSettings.mickMode.smoothing);
+
+        float gasOf1500 = *gas * 1500.f / 1000.f;
+        float brakeOf1500 = *brems * 1500.f / 1000.f;
+        float controlInput = gasOf1500 - brakeOf1500;
+
+        pwm_ = pwm_ * (1.f-alpha) + controlInput * alpha;
+        // Should be in this range anyway
+        pwm_ = std::clamp(pwm_, -1500.f, 1500.f);
+
+        for (bobbycar::protocol::serial::MotorState &motor : motors())
+        {
+            const auto pair = split(profileSettings.mickMode.modelMode);
+            motor.ctrlTyp = pair.first;
+            motor.ctrlMod = pair.second;
+            motor.pwm = pwm_;
+            motor.cruiseCtrlEna = false;
+            motor.nCruiseMotTgt = 0;
+        }
+    }
+
+    fixCommonParams();
+
+    sendCommands();
+}

main/modes/mickmode.h

@@ -0,0 +1,27 @@
+#pragma once
+
+// 3rdparty lib includes
+#include <espchrono.h>
+
+// local includes
+#include "bobbycar-common.h"
+#include "modeinterface.h"
+
+class MickMode : public ModeInterface
+{
+    using Base = ModeInterface;
+
+public:
+    void start() override;
+    void update() override;
+
+    const char *displayName() const override { return "mick"; }
+
+private:
+    float pwm_;
+    espchrono::millis_clock::time_point lastUpdate_{espchrono::millis_clock::now()};
+};
+
+namespace modes {
+extern MickMode mickMode;
+} // namespace modes

main/presets.h

@@ -104,6 +104,11 @@ constexpr ProfileSettings::LarsmMode defaultLarsmMode {
     .iterations = 100
 };
 
+constexpr ProfileSettings::MickMode defaultMickMode {
+    .modelMode = UnifiedModelMode::FocVoltage,
+    .smoothing = 1245
+};
+
 constexpr ProfileSettings::MotortestMode defaultMotortestMode {
     .multiplikator = 2,
     .maxPwm = 400
@@ -122,6 +127,7 @@ constexpr ProfileSettings defaultProfileSettings {
     .defaultMode = defaultDefaultMode,
     .tempomatMode = defaultTempomatMode,
     .larsmMode = defaultLarsmMode,
+    .mickMode = defaultMickMode,
     .motortestMode = defaultMotortestMode,
 #ifdef FEATURE_JOYSTICK
     .wheelchairMode = defaultWheelChairMode

main/profilesettings.h

@@ -51,6 +51,11 @@ struct ProfileSettings
         uint8_t iterations;
     } larsmMode;
 
+    struct MickMode {
+        UnifiedModelMode modelMode;
+        uint16_t smoothing;
+    } mickMode;
+
     struct MotortestMode {
         uint8_t multiplikator;
         uint16_t maxPwm;
@@ -109,6 +114,9 @@ void ProfileSettings::executeForEveryProfileSetting(T &&callable)
     callable("larsm.mode", larsmMode.mode);
     callable("larsm.iters", larsmMode.iterations);
 
+    callable("mick.modelMode", mickMode.modelMode);
+    callable("mick.smoothing", mickMode.smoothing);
+
 #ifdef FEATURE_JOYSTICK
     callable("wc.ses0", wheelchairMode.sensitivity0Kmh);
     callable("wc.ses50", wheelchairMode.sensitivity50Kmh);