diff --git a/config_seatbot.cmake b/config_seatbot.cmake index 32558c9..cda1251 100644 --- a/config_seatbot.cmake +++ b/config_seatbot.cmake @@ -12,13 +12,13 @@ add_definitions( -DTFT_CS=14 -DTFT_DC=12 -DTFT_RST=2 - -DSPI_FREQUENCY=20000000 + -DSPI_FREQUENCY=40000000 ) set(BOBBYCAR_BUILDFLAGS -# -DFEATURE_ADC_IN -# -DPINS_GAS=34 -# -DPINS_BREMS=35 + -DFEATURE_ADC_IN + -DPINS_GAS=34 + -DPINS_BREMS=35 -DFEATURE_JOYSTICK diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 313e6c5..dd14754 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -70,6 +70,7 @@ set(headers displays/calibratevoltagedisplay.h displays/gameoflifedisplay.h displays/gametrakcalibratedisplay.h + displays/joystickdebugdisplay.h displays/ledstripcolorsdisplay.h displays/lockscreen.h displays/menudisplaywithtime.cpp @@ -199,6 +200,7 @@ set(headers modes/motortestmode.h modes/remotecontrolmode.h modes/tempomatmode.h + modes/wheelchairmode.h mosfets.h newsettings.h ota.h @@ -300,6 +302,7 @@ set(sources displays/calibratevoltagedisplay.cpp displays/gameoflifedisplay.cpp displays/gametrakcalibratedisplay.cpp + displays/joystickdebugdisplay.cpp displays/ledstripcolorsdisplay.cpp displays/lockscreen.cpp displays/menus/aboutmenu.cpp @@ -428,6 +431,7 @@ set(sources modes/motortestmode.cpp modes/remotecontrolmode.cpp modes/tempomatmode.cpp + modes/wheelchairmode.cpp mosfets.cpp newsettings.cpp ota.cpp diff --git a/main/actions/modesettingsaction.h b/main/actions/modesettingsaction.h index c7e51f2..2d77b00 100644 --- a/main/actions/modesettingsaction.h +++ b/main/actions/modesettingsaction.h @@ -7,6 +7,7 @@ #include "modes/larsmmode.h" #include "modes/gametrakmode.h" #include "modes/motortestmode.h" +#include "modes/wheelchairmode.h" #include "displays/menus/defaultmodesettingsmenu.h" #include "displays/menus/tempomatmodesettingsmenu.h" #include "displays/menus/larsmmodesettingsmenu.h" @@ -25,7 +26,12 @@ public: void ModeSettingsAction::triggered() { - if (currentMode == &modes::defaultMode) + if ( + currentMode == &modes::defaultMode + #ifdef FEATURE_JOYSTICK + || currentMode == &modes::wheelchairMode + #endif + ) switchScreen(); else if (currentMode == &modes::tempomatMode) switchScreen(); diff --git a/main/bobbybuttons.cpp b/main/bobbybuttons.cpp index a172324..f792164 100644 --- a/main/bobbybuttons.cpp +++ b/main/bobbybuttons.cpp @@ -6,7 +6,6 @@ // local includes #include "newsettings.h" #include "settingsutils.h" -#include "modes/defaultmode.h" #ifdef FEATURE_LEDSTRIP #include "ledstripdefines.h" #include "ledstrip.h" diff --git a/main/displays/joystickdebugdisplay.cpp b/main/displays/joystickdebugdisplay.cpp new file mode 100644 index 0000000..482ec63 --- /dev/null +++ b/main/displays/joystickdebugdisplay.cpp @@ -0,0 +1,91 @@ +#ifdef FEATURE_JOYSTICK +#include "joystickdebugdisplay.h" + +// 3rdparty lib includes +#include +#include + +// local includes +#include "displays/statusdisplay.h" +#include "displays/menus/boardcomputerhardwaresettingsmenu.h" +#include "newsettings.h" +#include "utils.h" +#include "globals.h" + +using namespace espgui; + +namespace { +constexpr char TEXT_JOYSTICK[] = "Joystick Debug"; +} // namespace + +JoystickDebugDisplay::JoystickDebugDisplay() +{ +} + +void JoystickDebugDisplay::start() +{ + copyFromSettings(); +} + +std::string JoystickDebugDisplay::text() const +{ + return TEXT_JOYSTICK; +} + +void JoystickDebugDisplay::update() +{ + Base::update(); + + if (!raw_gas) + m_x = std::nullopt; + else + { + m_x = map_analog_stick(m_gasMitte, m_gasMin, m_gasMax, *raw_gas); + } + + if (!raw_brems) + m_y = std::nullopt; + else + m_y = map_analog_stick(m_bremsMitte, m_bremsMin, m_bremsMax, *raw_brems); +} + +void JoystickDebugDisplay::redraw() +{ + Base::redraw(); + + if (m_x && m_y) + { + tft.drawPixel( + (tft.width() / 2) + *m_x / 10, + (tft.height() / 2) + *m_y / 10, + TFT_WHITE); + } +} + +void JoystickDebugDisplay::copyFromSettings() +{ + m_gasMitte = configs.gasMitte.value; + m_gasMin = configs.gasMin.value; + m_gasMax = configs.gasMax.value; + m_bremsMitte = configs.bremsMitte.value; + m_bremsMin = configs.bremsMin.value; + m_bremsMax = configs.bremsMax.value; +} + +void JoystickDebugDisplay::buttonPressed(espgui::Button button) +{ + Base::buttonPressed(button); + + switch (button) + { + using espgui::Button; + case Button::Right: + tft.fillScreen(TFT_BLACK); + break; + case Button::Left: + espgui::switchScreen(); + break; + default:; + } +} +#endif diff --git a/main/displays/joystickdebugdisplay.h b/main/displays/joystickdebugdisplay.h new file mode 100644 index 0000000..4b810a6 --- /dev/null +++ b/main/displays/joystickdebugdisplay.h @@ -0,0 +1,35 @@ +#pragma once + +// system libs +#include + +// 3rdparty lib includes +#include + +// local includes +#include "bobbydisplaywithtitle.h" +#include "modeinterface.h" +#include "modes/ignoreinputmode.h" + +class JoystickDebugDisplay : public BobbyDisplayWithTitle +{ + using Base = BobbyDisplayWithTitle; + +public: + JoystickDebugDisplay(); + + std::string text() const override; + void start() override; + void update() override; + void redraw() override; + + void buttonPressed(espgui::Button button) override; + + void copyFromSettings(); + void copyToSettings(); +private: + std::optional m_x; + std::optional m_y; + + int16_t m_gasMin, m_gasMax, m_bremsMin, m_bremsMax, m_gasMitte, m_bremsMitte; +}; diff --git a/main/displays/menus/boardcomputerhardwaresettingsmenu.cpp b/main/displays/menus/boardcomputerhardwaresettingsmenu.cpp index 0420ed0..19e3adb 100644 --- a/main/displays/menus/boardcomputerhardwaresettingsmenu.cpp +++ b/main/displays/menus/boardcomputerhardwaresettingsmenu.cpp @@ -20,6 +20,9 @@ #include "displays/buttoncalibratedisplay.h" #include "displays/menus/extrabuttoncalibratemenu.h" #include "displays/menus/setupquickactionsmenu.h" +#ifdef FEATURE_JOYSTICK +#include "displays/joystickdebugdisplay.h" +#endif namespace { constexpr char TEXT_BOARDCOMPUTERHARDWARESETTINGS[] = "Boardcomputer H/W settings"; @@ -28,6 +31,9 @@ constexpr char TEXT_EXTRABUTTONCALIBRATE[] = "Cal other Buttons"; constexpr char TEXT_QUICKACTIONS[] = "Quick Actions"; constexpr char TEXT_LOCKSCREENSETTINGS[] = "Lockscreen Settings"; constexpr char TEXT_POTISCALIBRATE[] = "Potis Calibrate"; +#ifdef FEATURE_JOYSTICK +constexpr char TEXT_JOYSTICK[] = "Debug Joystick"; +#endif constexpr char TEXT_SAMPLECOUNT[] = "sampleCount"; constexpr char TEXT_GASMIN[] = "gasMin"; constexpr char TEXT_GASMAX[] = "gasMax"; @@ -182,6 +188,9 @@ BoardcomputerHardwareSettingsMenu::BoardcomputerHardwareSettingsMenu() constructMenuItem, DummyAction>>(); constructMenuItem, DummyAction>>(); constructMenuItem, SwitchScreenAction>>(); +#ifdef FEATURE_JOYSTICK + constructMenuItem, SwitchScreenAction>>(); +#endif constructMenuItem, SwitchScreenAction>>(); constructMenuItem, SwitchScreenAction>>(); constructMenuItem, SwitchScreenAction>>(); diff --git a/main/displays/menus/selectmodemenu.cpp b/main/displays/menus/selectmodemenu.cpp index 31e9053..dbbed88 100644 --- a/main/displays/menus/selectmodemenu.cpp +++ b/main/displays/menus/selectmodemenu.cpp @@ -15,6 +15,9 @@ #include "modes/remotecontrolmode.h" #include "modes/gametrakmode.h" #include "modes/motortestmode.h" +#ifdef FEATURE_JOYSTICK +#include "modes/wheelchairmode.h" +#endif #include "accessors/globalaccessors.h" #include "displays/menus/mainmenu.h" @@ -26,6 +29,9 @@ constexpr char TEXT_LARSM[] = "Larsm"; constexpr char TEXT_REMOTECONTROL[] = "Remote control"; constexpr char TEXT_GAMETRAK[] = "Gametrak"; constexpr char TEXT_MOTORTEST[] = "Motortest"; +#ifdef FEATURE_JOYSTICK +constexpr char TEXT_WHEELCHAIR[] = "Wheelchair"; +#endif constexpr char TEXT_BACK[] = "Back"; template @@ -42,6 +48,9 @@ using SetMotorTestModeAction = SetterAction; #endif +#ifdef FEATURE_JOYSTICK +using SetWheelchairModeAction = SetterAction; +#endif } // namespace using namespace espgui; @@ -56,6 +65,9 @@ SelectModeMenu::SelectModeMenu() constructMenuItem, MultiAction>>>(); #endif if (!simplified) { constructMenuItem, MultiAction>>>(); } +#ifdef FEATURE_JOYSTICK + constructMenuItem, MultiAction>>>(); +#endif constructMenuItem, SwitchScreenAction, StaticMenuItemIcon<&espgui::icons::back>>>(); } @@ -76,10 +88,18 @@ void SelectModeMenu::start() setSelectedIndex(2); else if (currentMode == &modes::motortestMode) setSelectedIndex(3); +#ifdef FEATURE_JOYSTICK + else if (currentMode == &modes::wheelchairMode) + setSelectedIndex(4); +#endif else { //Serial.printf("Unknown mode: %s", currentMode?currentMode->displayName():""); +#ifdef FEATURE_JOYSTICK + setSelectedIndex(5); +#else setSelectedIndex(4); +#endif } } diff --git a/main/displays/potiscalibratedisplay.cpp b/main/displays/potiscalibratedisplay.cpp index 493264c..4456b46 100644 --- a/main/displays/potiscalibratedisplay.cpp +++ b/main/displays/potiscalibratedisplay.cpp @@ -106,6 +106,9 @@ void PotisCalibrateDisplay::redraw() switch (m_status) { case Status::Begin: return "Start calibrating?"; +#ifdef FEATURE_JOYSTICK + case Status::Mitte: return "Release joystick"; +#endif case Status::GasMin: return "Release gas"; case Status::GasMax: return "Press gas"; case Status::BremsMin: return "Release brems"; @@ -123,6 +126,9 @@ void PotisCalibrateDisplay::redraw() switch (m_status) { case Status::Begin: return "Yes"; +#ifdef FEATURE_JOYSTICK + case Status::Mitte: +#endif case Status::GasMin: case Status::GasMax: case Status::BremsMin: @@ -141,6 +147,9 @@ void PotisCalibrateDisplay::redraw() switch (m_status) { case Status::Begin: return "No"; +#ifdef FEATURE_JOYSTICK + case Status::Mitte: +#endif case Status::GasMin: case Status::GasMax: case Status::BremsMin: @@ -206,6 +215,9 @@ void PotisCalibrateDisplay::buttonPressed(espgui::Button button) else espgui::switchScreen(); break; +#ifdef FEATURE_JOYSTICK + case Status::Mitte: +#endif case Status::GasMin: case Status::GasMax: case Status::BremsMin: @@ -226,9 +238,21 @@ void PotisCalibrateDisplay::buttonPressed(espgui::Button button) switch (m_status) { +#ifndef FEATURE_JOYSTICK case Status::Begin: m_status = Status::GasMin; break; +#else + case Status::Begin: + m_status = Status::Mitte; + break; + + case Status::Mitte: + m_gasMitte = *raw_gas; + m_bremsMitte = *raw_brems; + m_status = Status::GasMin; + break; +#endif case Status::GasMin: m_gasMin = *raw_gas; m_status = Status::GasMax; @@ -275,6 +299,10 @@ void PotisCalibrateDisplay::buttonPressed(espgui::Button button) void PotisCalibrateDisplay::copyFromSettings() { +#ifdef FEATURE_JOYSTICK + m_gasMitte = configs.gasMitte.value; + m_bremsMitte = configs.bremsMitte.value; +#endif m_gasMin = configs.gasMin.value; m_gasMax = configs.gasMax.value; m_bremsMin = configs.bremsMin.value; @@ -283,6 +311,10 @@ void PotisCalibrateDisplay::copyFromSettings() void PotisCalibrateDisplay::copyToSettings() { +#ifdef FEATURE_JOYSTICK + configs.write_config(configs.gasMitte, m_gasMitte); + configs.write_config(configs.bremsMitte, m_bremsMitte); +#endif configs.write_config(configs.gasMin, m_gasMin); configs.write_config(configs.gasMax, m_gasMax); configs.write_config(configs.bremsMin, m_bremsMin); diff --git a/main/displays/potiscalibratedisplay.h b/main/displays/potiscalibratedisplay.h index e53ae83..b05aa79 100644 --- a/main/displays/potiscalibratedisplay.h +++ b/main/displays/potiscalibratedisplay.h @@ -65,6 +65,9 @@ private: enum Status { Begin, +#ifdef FEATURE_JOYSTICK + Mitte, +#endif GasMin, GasMax, BremsMin, @@ -75,6 +78,15 @@ private: int8_t m_selectedButton, m_renderedButton; Status m_status; - int16_t m_gasMin, m_gasMax, m_bremsMin, m_bremsMax; + int16_t + m_gasMin, + m_gasMax, + m_bremsMin, + m_bremsMax +#ifdef FEATURE_JOYSTICK + ,m_gasMitte + ,m_bremsMitte +#endif + ; std::optional m_gas, m_brems; }; diff --git a/main/globals.cpp b/main/globals.cpp index 7c1780c..58c2c15 100644 --- a/main/globals.cpp +++ b/main/globals.cpp @@ -12,14 +12,6 @@ float gametrakY; float gametrakDist; #endif -#ifdef FEATURE_JOYSTICK -bool joystick_button{}; -int16_t joystick_x{}; -int16_t joystick_y{}; -int16_t raw_joystick_x{}; -int16_t raw_joystick_y{}; -#endif - float avgSpeed{}; float avgSpeedKmh{}; float sumCurrent{}; diff --git a/main/globals.h b/main/globals.h index ac61975..65141f3 100644 --- a/main/globals.h +++ b/main/globals.h @@ -39,14 +39,6 @@ extern float gametrakY; extern float gametrakDist; #endif -#ifdef FEATURE_JOYSTICK -extern bool joystick_button; -extern int16_t joystick_x; -extern int16_t joystick_y; -extern int16_t raw_joystick_x; -extern int16_t raw_joystick_y; -#endif - extern float avgSpeed; extern float avgSpeedKmh; extern float sumCurrent; diff --git a/main/main.cpp b/main/main.cpp index 6dd1e40..1390255 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -20,7 +20,11 @@ using namespace std::chrono_literals; #include "screens.h" #include "presets.h" #include "statistics.h" +#ifdef FEATURE_JOYSTICK +#include "modes/wheelchairmode.h" +#else #include "modes/defaultmode.h" +#endif #include "displays/statusdisplay.h" #include "displays/lockscreen.h" #include "displays/potiscalibratedisplay.h" @@ -65,7 +69,11 @@ extern "C" void app_main() task.setup(); } +#ifdef FEATURE_JOYSTICK + currentMode = &modes::wheelchairMode; +#else currentMode = &modes::defaultMode; +#endif bootLabel.redraw("switchScreen"); diff --git a/main/modes/wheelchairmode.cpp b/main/modes/wheelchairmode.cpp new file mode 100644 index 0000000..beff612 --- /dev/null +++ b/main/modes/wheelchairmode.cpp @@ -0,0 +1,268 @@ +#include "wheelchairmode.h" + +using namespace std::chrono_literals; + +namespace modes { +WheelchairMode wheelchairMode; +} // namespace modes + +void WheelchairMode::start() +{ + Base::start(); + + copyFromSettings(); +} + +// left_right gas +// front_back brems + +void WheelchairMode::copyFromSettings() +{ + m_gasMitte = configs.gasMitte.value; + m_gasMin = configs.gasMin.value; + m_gasMax = configs.gasMax.value; + m_bremsMitte = configs.bremsMitte.value; + m_bremsMin = configs.bremsMin.value; + m_bremsMax = configs.bremsMax.value; +} + +void WheelchairMode::update() +{ + auto pair = split(profileSettings.defaultMode.modelMode); + if (!gas || !brems || !raw_gas || !raw_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 + { + const auto left_right = m_gasMin == m_gasMax ? 0 : map_analog_stick(m_gasMitte, m_gasMin, m_gasMax, *raw_gas); + const auto front_back = m_bremsMin == m_bremsMax ? 0 : map_analog_stick(m_bremsMitte, m_bremsMin, m_bremsMax, *raw_brems); + + float local_gas = 0; + float local_brems = 0; + + if (front_back > 0) + local_gas = front_back; + else if (front_back < 0) + local_brems = -front_back; + + if (waitForGasLoslass) + { + if (local_gas < 50) + waitForGasLoslass = false; + else + local_gas = 0; + } + + if (waitForBremsLoslass) + { + if (local_brems < 50) + waitForBremsLoslass = false; + else + local_brems = 0; + } + + ESP_LOGI("BOBBY", "left_right: %i, front_back: %i, local_gas: %.2f, local_brems: %.2f gas: %.2f, brems: %.2f", left_right, front_back, local_gas, local_brems, *gas, *brems); + + auto gas_processed = profileSettings.defaultMode.squareGas ? (local_gas * local_gas) / 1000.f : local_gas; + auto brems_processed = profileSettings.defaultMode.squareBrems ? (local_brems * local_brems) / 1000 : local_brems; + + const auto now = espchrono::millis_clock::now(); + + float pwm; + + if (configs.handbremse.enable.value && handbremse::stateWish == handbremse::StateWish::brake) + { + using namespace handbremse; + + const auto speed = abs(avgSpeedKmh); + const bool gas_und_brems_ist_null = (gas_processed < 1 && brems_processed < 1); + if (speed < 2 && gas_und_brems_ist_null) + { + angezogen = true; + stateWish = StateWish::none; + } + } + + if (configs.handbremse.enable.value && configs.handbremse.automatic.value && !handbremse::angezogen) + { + using namespace handbremse; + const auto speed = abs(avgSpeedKmh); + + if (speed < 1) + { + if (!standStillFirstDetected) + { + standStillFirstDetected = now; + } + } + else + standStillFirstDetected = std::nullopt; + + if (standStillFirstDetected && lastAutoRelease) + if (espchrono::ago(*standStillFirstDetected) > 100ms * configs.handbremse.triggerTimeout.value && espchrono::ago(*lastAutoRelease) > 5s ) + { + angezogen = true; + } + } + + if (handbremse::wishTimer) + { + using namespace handbremse; + if (espchrono::ago(*wishTimer) > 5s) + { + stateWish = StateWish::none; + wishTimer = std::nullopt; + } + } + + if (handbremse::angezogen) + { + using namespace handbremse; + + standStillFirstDetected = std::nullopt; + + const bool valid = (controllers.front.feedbackValid && controllers.back.feedbackValid); + const bool gas_oder_brems = (gas_processed > 10 || brems_processed > 10); + fixCommonParams(); + + if (stateWish == StateWish::release) + { + lastAutoRelease = now; + releaseTimer = now; + stateWish = StateWish::none; + finishedMotorUpdate = false; + } + + if (valid && gas_oder_brems) + { + if (!releaseTimer) + { + lastAutoRelease = now; + releaseTimer = now; + finishedMotorUpdate = false; + } + } + + if (releaseTimer) + { + for (bobbycar::protocol::serial::MotorState &motor : motors()) + { + motor.pwm = 0; + motor.ctrlTyp = pair.first; + motor.ctrlMod = pair.second; + } + + if (espchrono::ago(*releaseTimer) > 1s || finishedMotorUpdate) + { + releaseTimer = std::nullopt; + angezogen = false; + } + } + else + { + for (bobbycar::protocol::serial::MotorState &motor : motors()) + { + motor.ctrlTyp = bobbycar::protocol::ControlType::FieldOrientedControl; + switch (configs.handbremse.mode.value) + { + case HandbremseMode::MOSFETS_OFF: + motor.ctrlMod = bobbycar::protocol::ControlMode::Torque; + motor.enable = false; + break; + case HandbremseMode::OPENMODE: + motor.ctrlMod = bobbycar::protocol::ControlMode::OpenMode; + break; + case HandbremseMode::SPEED_0: + motor.ctrlMod = bobbycar::protocol::ControlMode::Speed; + break; + } + motor.pwm = 0; + motor.cruiseCtrlEna = false; + motor.nCruiseMotTgt = 0; + } + } + } + else + { + if (gas_processed >= profileSettings.defaultMode.add_schwelle) + { + pwm = (gas_processed/1000.*profileSettings.defaultMode.gas1_wert) + (brems_processed/1000.*profileSettings.defaultMode.brems1_wert); + + if ((profileSettings.defaultMode.enableSmoothingUp || profileSettings.defaultMode.enableSmoothingDown) && (pwm > 1000. || m_lastPwm > 1000.)) + { + if (m_lastPwm < pwm && profileSettings.defaultMode.enableSmoothingUp) + { + pwm = std::min(pwm, m_lastPwm + (profileSettings.defaultMode.smoothing * std::chrono::milliseconds{now - m_lastTime}.count() / 100.f)); + if (pwm < 1000.) + pwm = 1000.; + } + else if (m_lastPwm > pwm && profileSettings.defaultMode.enableSmoothingDown) + { + pwm = std::max(pwm, m_lastPwm - (profileSettings.defaultMode.smoothing * std::chrono::milliseconds{now - m_lastTime}.count() / 100.f)); + } + } + } + else + { + pwm = (gas_processed/1000.*profileSettings.defaultMode.gas2_wert) - (brems_processed/1000.*profileSettings.defaultMode.brems2_wert); + if ( + (profileSettings.defaultMode.enableFieldWeakSmoothingUp || profileSettings.defaultMode.enableFieldWeakSmoothingDown) && + (m_lastPwm > profileSettings.defaultMode.fwSmoothLowerLimit) && + brems_processed > 0) + { + if (m_lastPwm < pwm && profileSettings.defaultMode.enableFieldWeakSmoothingUp) + { + auto effective_smoothing = profileSettings.defaultMode.smoothing; + auto difference_to_target = std::abs(pwm-m_lastPwm); + effective_smoothing *= std::max((difference_to_target / 500),0.5f); + + pwm = std::min(pwm, m_lastPwm + (effective_smoothing * std::chrono::milliseconds{now - m_lastTime}.count() / 100.f)); + } + else if (m_lastPwm > pwm && profileSettings.defaultMode.enableFieldWeakSmoothingDown) + { + auto effective_smoothing = profileSettings.defaultMode.smoothing; + auto difference_to_target = std::abs(pwm-m_lastPwm); + effective_smoothing *= std::max((difference_to_target / 500),0.5f); + + pwm = std::max(pwm, m_lastPwm - (effective_smoothing * std::chrono::milliseconds{now - m_lastTime}.count() / 100.f)); + } + } + } + + m_lastPwm = pwm; + m_lastTime = now; + + float steer = cpputils::mapValueClamped(abs(avgSpeedKmh), 0, 50, profileSettings.wheelchairMode.sensitivity0Kmh, profileSettings.wheelchairMode.sensitivity50Kmh) / 1000. * left_right; + + for (Controller &controller : controllers) + { + +// motor.pwm = pwm / 100. * profileSettings.defaultMode.frontPercentage; + + controller.command.left.ctrlTyp = pair.first; + controller.command.left.ctrlMod = pair.second; + controller.command.left.pwm = (pwm + steer); + controller.command.left.cruiseCtrlEna = false; + controller.command.left.nCruiseMotTgt = 0; + + controller.command.right.ctrlTyp = pair.first; + controller.command.right.ctrlMod = pair.second; + controller.command.right.pwm = (pwm - steer); + controller.command.right.cruiseCtrlEna = false; + controller.command.right.nCruiseMotTgt = 0; + } + fixCommonParams(); + } + } + sendCommands(); +} diff --git a/main/modes/wheelchairmode.h b/main/modes/wheelchairmode.h new file mode 100644 index 0000000..c8d7622 --- /dev/null +++ b/main/modes/wheelchairmode.h @@ -0,0 +1,40 @@ +#pragma once + +// system includes +#include +#include + +// 3rdparty lib includes +#include + +// local includes +#include "modeinterface.h" +#include "globals.h" +#include "utils.h" + +class WheelchairMode : public ModeInterface +{ + using Base = ModeInterface; + +public: + void start() override; + void update() override; + + const char *displayName() const override { return "Wheelchair"; } + + bool waitForGasLoslass{false}; + bool waitForBremsLoslass{false}; + + void copyFromSettings(); + +private: + espchrono::millis_clock::time_point m_lastTime{espchrono::millis_clock::now()}; + float m_x{0}; + float m_lastPwm{0}; + + int16_t m_gasMin, m_gasMax, m_bremsMin, m_bremsMax, m_gasMitte, m_bremsMitte; +}; + +namespace modes { +extern WheelchairMode wheelchairMode; +} // namespace modes diff --git a/main/newsettings.h b/main/newsettings.h index f54ed18..78aa258 100644 --- a/main/newsettings.h +++ b/main/newsettings.h @@ -130,8 +130,10 @@ public: ConfigWrapper sampleCount {50, DoReset, {}, "sampleCount" }; ConfigWrapper gasMin {0, DoReset, MinMaxValue, "gasMin" }; ConfigWrapper gasMax {4095, DoReset, MinMaxValue, "gasMax" }; + ConfigWrapper gasMitte {2048, DoReset, MinMaxValue, "gasMiddle" }; ConfigWrapper bremsMin {0, DoReset, MinMaxValue, "bremsMin" }; ConfigWrapper bremsMax {4096, DoReset, MinMaxValue, "bremsMax" }; + ConfigWrapper bremsMitte {2048, DoReset, MinMaxValue, "bremsMiddle" }; ConfigWrapper dpadDebounce {25, DoReset, {}, "dpadDebounce" }; @@ -421,8 +423,10 @@ public: x(sampleCount) \ x(gasMin) \ x(gasMax) \ + x(gasMitte) \ x(bremsMin) \ x(bremsMax) \ + x(bremsMitte) \ \ x(dpadDebounce) \ \ diff --git a/main/presets.h b/main/presets.h index 1c10b51..c8cc9b0 100644 --- a/main/presets.h +++ b/main/presets.h @@ -122,12 +122,19 @@ constexpr ProfileSettings::MotortestMode defaultMotortestMode { .maxPwm = 400 }; + +constexpr ProfileSettings::WheelchairMode defaultWheelChairMode { + .sensitivity0Kmh = 100, + .sensitivity50Kmh = 10, +}; + constexpr ProfileSettings defaultProfileSettings { .limits = defaultLimits, .controllerHardware = defaultControllerHardware, .defaultMode = defaultDefaultMode, .tempomatMode = defaultTempomatMode, .larsmMode = defaultLarsmMode, - .motortestMode = defaultMotortestMode + .motortestMode = defaultMotortestMode, + .wheelchairMode = defaultWheelChairMode }; } // namespace presets diff --git a/main/profilesettings.h b/main/profilesettings.h index eecae0d..57d6e26 100644 --- a/main/profilesettings.h +++ b/main/profilesettings.h @@ -69,6 +69,13 @@ struct ProfileSettings uint16_t maxPwm; } motortestMode; +#ifdef FEATURE_JOYSTICK + struct WheelchairMode { + uint16_t sensitivity0Kmh; + uint16_t sensitivity50Kmh; + } wheelchairMode; +#endif + template void executeForEveryProfileSetting(T &&callable); }; @@ -114,4 +121,9 @@ void ProfileSettings::executeForEveryProfileSetting(T &&callable) callable("larsm.modelMode", larsmMode.modelMode); callable("larsm.mode", larsmMode.mode); callable("larsm.iters", larsmMode.iterations); + +#ifdef FEATURE_JOYSTICK + callable("wc.ses0", wheelchairMode.sensitivity0Kmh); + callable("wc.ses50", wheelchairMode.sensitivity50Kmh); +#endif } diff --git a/main/utils.cpp b/main/utils.cpp index d2fd801..edb3a05 100644 --- a/main/utils.cpp +++ b/main/utils.cpp @@ -282,3 +282,25 @@ std::string local_clock_string() const auto dt = espchrono::toDateTime(now); return fmt::format("{:02d}:{:02d}:{:02d}", dt.hour, dt.minute, dt.second); } + +int16_t map_analog_stick(uint16_t middle, uint16_t start, uint16_t end, uint16_t raw) + { + if (raw < middle) + { + const auto return_val = map(raw, start, middle, -1000, 0); + if (return_val > 0) + return 0; + if (return_val < -1000) + return -1000; + return return_val; + } + else + { + const auto return_val = map(raw, middle, end, 0, 1000); + if (return_val < 0) + return 0; + if (return_val > 1000) + return 1000; + return return_val; + } + } diff --git a/main/utils.h b/main/utils.h index 187abc0..bc22fd0 100644 --- a/main/utils.h +++ b/main/utils.h @@ -60,3 +60,4 @@ float wattToAmpere(float watt); float wattToMotorCurrent(float watt); uint8_t time_to_percent(espchrono::milliseconds32 repeat, espchrono::milliseconds32 riseTime, espchrono::milliseconds32 fullTime, size_t numLeds, bool invert); std::string local_clock_string(); +int16_t map_analog_stick(uint16_t middle, uint16_t start, uint16_t end, uint16_t raw);