From 56ebbe805581d32df3f93deb72a3a57387dda9ac Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Mon, 28 Jun 2021 14:22:05 +0200 Subject: [PATCH] ADC and button input via can --- platformio.ini | 4 + src/actions/switchprofileaction.h | 15 +- src/actions/updateswapfrontbackaction.h | 2 + src/bobbycar-protocol | 2 +- src/buttons.h | 80 +++++ src/can.h | 273 +++++++++++++----- src/displays/calibratedisplay.h | 45 +-- src/displays/dpad5wiredebugdisplay.h | 4 +- .../menus/boardcomputerhardwaresettingsmenu.h | 16 +- .../menus/controllerhardwaresettingsmenu.h | 2 - src/displays/statusdisplay.h | 12 +- src/dpad.h | 2 +- src/dpad3wire.h | 4 +- src/dpad5wire.h | 118 ++++---- src/globals.h | 66 +---- src/main.cpp | 16 +- src/modes/defaultmode.h | 119 ++++---- src/modes/gametrakmode.h | 56 ++-- src/modes/larsmmode.h | 122 ++++---- src/modes/tempomatmode.h | 42 ++- src/presets.h | 6 - src/settings.h | 4 - src/settingsaccessors.h | 4 +- src/settingspersister.h | 3 +- src/settingsutils.h | 23 ++ src/statistics.h | 6 +- src/utils.h | 53 +++- 27 files changed, 696 insertions(+), 403 deletions(-) create mode 100644 src/buttons.h create mode 100644 src/settingsutils.h diff --git a/platformio.ini b/platformio.ini index 474d435..555e5b5 100644 --- a/platformio.ini +++ b/platformio.ini @@ -68,6 +68,7 @@ build_flags = -DPINS_TX1=5 -DPINS_RX2=25 -DPINS_TX2=26 + -DFEATURE_ADC_IN -DFEATURE_MOSFETS -DPINS_MOSFET0=18 -DPINS_MOSFET1=19 @@ -105,6 +106,7 @@ lib_compat_mode = ${common_env_data.lib_compat_mode} build_unflags = ${common_env_data.build_unflags} build_flags = ${common_env_data.build_flags} + -DFEATURE_ADC_IN -DPINS_GAS=34 -DPINS_BREMS=35 ; -DPINS_LED=23 @@ -339,6 +341,7 @@ build_flags = -DPINS_TX1=25 -DPINS_RX2=23 -DPINS_TX2=34 + -DFEATURE_ADC_IN -DPINS_GAS=35 -DPINS_BREMS=33 -DDEFAULT_GASMIN=0 @@ -414,6 +417,7 @@ lib_compat_mode = ${common_env_data.lib_compat_mode} build_unflags = ${common_env_data.build_unflags} build_flags = ${common_env_data.build_flags} + -DFEATURE_ADC_IN -DPINS_GAS=33 -DPINS_BREMS=35 -DILI9341_DRIVER=1 diff --git a/src/actions/switchprofileaction.h b/src/actions/switchprofileaction.h index 797e4c2..26e6eb9 100644 --- a/src/actions/switchprofileaction.h +++ b/src/actions/switchprofileaction.h @@ -3,25 +3,16 @@ #include #include "actioninterface.h" -#include "globals.h" -#include "presets.h" +#include "settingsutils.h" namespace { -template +template class SwitchProfileAction : public virtual ActionInterface { public: void triggered() override { - settings = presets::defaultSettings; - - if (settingsPersister.openProfile(profile)) - { - if (!settingsPersister.load(settings)) - Serial.println("SwitchProfileAction::triggered() load failed"); - } - else - Serial.println("SwitchProfileAction::triggered() openProfile failed"); + switchProfile(index); } }; } diff --git a/src/actions/updateswapfrontbackaction.h b/src/actions/updateswapfrontbackaction.h index ac51e1a..f423d06 100644 --- a/src/actions/updateswapfrontbackaction.h +++ b/src/actions/updateswapfrontbackaction.h @@ -4,9 +4,11 @@ #include "utils.h" namespace { +#ifdef FEATURE_SERIAL class UpdateSwapFrontBackAction : public virtual ActionInterface { public: void triggered() override { updateSwapFrontBack(); } }; +#endif } diff --git a/src/bobbycar-protocol b/src/bobbycar-protocol index 2e0f97d..2c6fb11 160000 --- a/src/bobbycar-protocol +++ b/src/bobbycar-protocol @@ -1 +1 @@ -Subproject commit 2e0f97d6dd3e71cf01d0859bd332548be2187d24 +Subproject commit 2c6fb114f2ea6e3aed243acea1afd28ca34938c9 diff --git a/src/buttons.h b/src/buttons.h new file mode 100644 index 0000000..4e340e3 --- /dev/null +++ b/src/buttons.h @@ -0,0 +1,80 @@ +#pragma once + +// Arduino includes +#include + +// local includes +#include "types.h" +#include "settingsutils.h" + +namespace { + +int rotated{}; +bool requestFullRedraw{}; + +bool confirmButtonPressed{}; +bool confirmButtonLongPressed{}; +bool backButtonPressed{}; +bool backButtonLongPressed{}; + +class InputDispatcher +{ +public: + static void rotate(int offset) + { + rotated += offset; + } + + static void confirmButton(bool pressed) + { + static millis_t pressBegin = 0; + + const auto now = millis(); + + if (pressed) + pressBegin = now; + else + { + const auto duration = now - pressBegin; + + if (duration < 500) + confirmButtonPressed = true; + else if (duration < 2000) + confirmButtonLongPressed = true; + else + requestFullRedraw = true; + + pressBegin = 0; + } + } + + static void backButton(bool pressed) + { + static millis_t pressBegin = 0; + + const auto now = millis(); + + if (pressed) + pressBegin = now; + else + { + const auto duration = now - pressBegin; + + if (duration < 500) + backButtonPressed = true; + else + backButtonLongPressed = true; + + pressBegin = 0; + } + } + + static void profileButton(uint8_t index, bool pressed) + { + if (!pressed) + return; + + switchProfile(index); + } +}; +} diff --git a/src/can.h b/src/can.h index c54694e..dd1513f 100644 --- a/src/can.h +++ b/src/can.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -11,8 +12,26 @@ #include "types.h" #include "globals.h" +#include "buttons.h" +namespace can { namespace { +std::optional can_gas, can_brems; +millis_t last_can_gas{}, last_can_brems{}; + +struct CanButtonsState +{ + bool up{}; + bool down{}; + bool confirm{}; + bool back{}; + bool profile0{}; + bool profile1{}; + bool profile2{}; + bool profile3{}; +}; +CanButtonsState lastButtonsState; + void initCan() { Serial.println("initCan()"); @@ -59,7 +78,7 @@ void initCan() } template -bool parseCanMessage(const can_message_t &message, Controller &controller) +bool parseMotorControllerCanMessage(const can_message_t &message, Controller &controller) { switch (message.identifier) { @@ -135,7 +154,68 @@ bool parseCanMessage(const can_message_t &message, Controller &controller) return false; } -bool parseCanInput() +bool parseBoardcomputerCanMessage(const can_message_t &message) +{ + switch (message.identifier) + { + using namespace bobbycar::protocol::can; + case Boardcomputer::Command::ButtonPress: + { + const auto canButtonBits = *((uint16_t*)message.data); + CanButtonsState newState { + .up = bool(canButtonBits & Boardcomputer::ButtonUp), + .down = bool(canButtonBits & Boardcomputer::ButtonDown), + .confirm = bool(canButtonBits & Boardcomputer::ButtonConfirm), + .back = bool(canButtonBits & Boardcomputer::ButtonBack), + .profile0 = bool(canButtonBits & Boardcomputer::ButtonProfile0), + .profile1 = bool(canButtonBits & Boardcomputer::ButtonProfile1), + .profile2 = bool(canButtonBits & Boardcomputer::ButtonProfile2), + .profile3 = bool(canButtonBits & Boardcomputer::ButtonProfile3), + }; + + if (lastButtonsState.up != newState.up) + if (newState.up) + InputDispatcher::rotate(-1); + + if (lastButtonsState.down != newState.down) + if (newState.down) + InputDispatcher::rotate(1); + + if (lastButtonsState.confirm != newState.confirm) + InputDispatcher::confirmButton(newState.confirm); + + if (lastButtonsState.back != newState.back) + InputDispatcher::backButton(newState.back); + + if (lastButtonsState.profile0 != newState.profile0) + InputDispatcher::profileButton(0, newState.profile0); + + if (lastButtonsState.profile1 != newState.profile1) + InputDispatcher::profileButton(1, newState.profile1); + + if (lastButtonsState.profile2 != newState.profile2) + InputDispatcher::profileButton(2, newState.profile2); + + if (lastButtonsState.profile3 != newState.profile3) + InputDispatcher::profileButton(3, newState.profile3); + + lastButtonsState = newState; + break; + } + case Boardcomputer::Command::RawGas: + can_gas = *((int16_t*)message.data); + last_can_gas = millis(); + break; + case Boardcomputer::Command::RawBrems: + can_brems = *((int16_t*)message.data); + last_can_brems = millis(); + break; + } + + return false; +} + +bool tryParseCanInput() { can_message_t message; if (const auto result = can_receive(&message, pdMS_TO_TICKS(50)); result != ESP_OK) @@ -152,38 +232,51 @@ bool parseCanInput() return false; } - if (parseCanMessage(message, controllers.front)) - { - if (millis() - controllers.back.lastCanFeedback > 100) - controllers.back.feedbackValid = false; + Controller &front = settings.controllerHardware.swapFrontBack ? controllers.back : controllers.front; + Controller &back = settings.controllerHardware.swapFrontBack ? controllers.front : controllers.back; - controllers.front.lastCanFeedback = millis(); - controllers.front.feedbackValid = true; + if (parseMotorControllerCanMessage(message, front)) + { + if (millis() - back.lastCanFeedback > 100) + back.feedbackValid = false; + + front.lastCanFeedback = millis(); + front.feedbackValid = true; return true; } else { - if (millis() - controllers.front.lastCanFeedback > 100) - controllers.front.feedbackValid = false; + if (millis() - front.lastCanFeedback > 100) + front.feedbackValid = false; } - if (parseCanMessage(message, controllers.back)) + if (parseMotorControllerCanMessage(message, back)) { - controllers.back.lastCanFeedback = millis(); - controllers.back.feedbackValid = true; + back.lastCanFeedback = millis(); + back.feedbackValid = true; return true; } else { - if (millis() - controllers.back.lastCanFeedback > 100) - controllers.back.feedbackValid = false; + if (millis() - back.lastCanFeedback > 100) + back.feedbackValid = false; } + if (parseBoardcomputerCanMessage(message)) + return true; + //Serial.printf("WARNING Unknown CAN info received .identifier = %u\r\n", message.identifier); return true; } +void parseCanInput() +{ + for (int i = 0; i < 4; i++) + if (!tryParseCanInput()) + break; +} + void sendCanCommands() { constexpr auto send = [](uint32_t addr, auto value){ @@ -200,89 +293,129 @@ void sendCanCommands() return result; }; + const Controller &front = settings.controllerHardware.swapFrontBack ? controllers.back : controllers.front; + const Controller &back = settings.controllerHardware.swapFrontBack ? controllers.front : controllers.back; + using namespace bobbycar::protocol::can; - send(MotorController::Command::InpTgt, controllers.front.command.left.pwm); - send(MotorController::Command::InpTgt, controllers.front.command.right.pwm); - send(MotorController::Command::InpTgt, controllers.back.command.left.pwm); - send(MotorController::Command::InpTgt, controllers.back.command.right.pwm); + send(MotorController::Command::InpTgt, front.command.left.pwm); + send(MotorController::Command::InpTgt, front.command.right.pwm); + send(MotorController::Command::InpTgt, back.command.left.pwm); + send(MotorController::Command::InpTgt, back.command.right.pwm); + + uint16_t buttonLeds{}; + if (const auto index = settingsPersister.currentlyOpenProfileIndex()) + switch (*index) + { + case 0: buttonLeds |= Boardcomputer::ButtonProfile0; break; + case 1: buttonLeds |= Boardcomputer::ButtonProfile1; break; + case 2: buttonLeds |= Boardcomputer::ButtonProfile2; break; + case 3: buttonLeds |= Boardcomputer::ButtonProfile3; break; + } + + static struct { + struct { + uint8_t freq = 0; + uint8_t pattern = 0; + } front, back; + uint16_t buttonLeds{}; + } lastValues; static int i{}; + + if (front.command.buzzer.freq != lastValues.front.freq || + front.command.buzzer.pattern != lastValues.front.pattern || + back.command.buzzer.freq != lastValues.back.freq || + back.command.buzzer.pattern != lastValues.back.pattern) + i = 8; + else if (buttonLeds != lastValues.buttonLeds) + i = 10; + switch (i++) { case 0: - send(MotorController::Command::Enable, controllers.front.command.left.enable); - send(MotorController::Command::Enable, controllers.front.command.right.enable); - send(MotorController::Command::Enable, controllers.back.command.left.enable); - send(MotorController::Command::Enable, controllers.back.command.right.enable); + send(MotorController::Command::Enable, front.command.left.enable); + send(MotorController::Command::Enable, front.command.right.enable); + send(MotorController::Command::Enable, back.command.left.enable); + send(MotorController::Command::Enable, back.command.right.enable); break; case 1: - send(MotorController::Command::CtrlTyp, controllers.front.command.left.ctrlTyp); - send(MotorController::Command::CtrlTyp, controllers.front.command.right.ctrlTyp); - send(MotorController::Command::CtrlTyp, controllers.back.command.left.ctrlTyp); - send(MotorController::Command::CtrlTyp, controllers.back.command.right.ctrlTyp); + send(MotorController::Command::CtrlTyp, front.command.left.ctrlTyp); + send(MotorController::Command::CtrlTyp, front.command.right.ctrlTyp); + send(MotorController::Command::CtrlTyp, back.command.left.ctrlTyp); + send(MotorController::Command::CtrlTyp, back.command.right.ctrlTyp); break; case 2: - send(MotorController::Command::CtrlMod, controllers.front.command.left.ctrlMod); - send(MotorController::Command::CtrlMod, controllers.front.command.right.ctrlMod); - send(MotorController::Command::CtrlMod, controllers.back.command.left.ctrlMod); - send(MotorController::Command::CtrlMod, controllers.back.command.right.ctrlMod); + send(MotorController::Command::CtrlMod, front.command.left.ctrlMod); + send(MotorController::Command::CtrlMod, front.command.right.ctrlMod); + send(MotorController::Command::CtrlMod, back.command.left.ctrlMod); + send(MotorController::Command::CtrlMod, back.command.right.ctrlMod); break; case 3: - send(MotorController::Command::IMotMax, controllers.front.command.left.iMotMax); - send(MotorController::Command::IMotMax, controllers.front.command.right.iMotMax); - send(MotorController::Command::IMotMax, controllers.back.command.left.iMotMax); - send(MotorController::Command::IMotMax, controllers.back.command.right.iMotMax); + send(MotorController::Command::IMotMax, front.command.left.iMotMax); + send(MotorController::Command::IMotMax, front.command.right.iMotMax); + send(MotorController::Command::IMotMax, back.command.left.iMotMax); + send(MotorController::Command::IMotMax, back.command.right.iMotMax); break; case 4: - send(MotorController::Command::IDcMax, controllers.front.command.left.iDcMax); - send(MotorController::Command::IDcMax, controllers.front.command.right.iDcMax); - send(MotorController::Command::IDcMax, controllers.back.command.left.iDcMax); - send(MotorController::Command::IDcMax, controllers.back.command.right.iDcMax); + send(MotorController::Command::IDcMax, front.command.left.iDcMax); + send(MotorController::Command::IDcMax, front.command.right.iDcMax); + send(MotorController::Command::IDcMax, back.command.left.iDcMax); + send(MotorController::Command::IDcMax, back.command.right.iDcMax); break; case 5: - send(MotorController::Command::NMotMax, controllers.front.command.left.nMotMax); - send(MotorController::Command::NMotMax, controllers.front.command.right.nMotMax); - send(MotorController::Command::NMotMax, controllers.back.command.left.nMotMax); - send(MotorController::Command::NMotMax, controllers.back.command.right.nMotMax); + send(MotorController::Command::NMotMax, front.command.left.nMotMax); + send(MotorController::Command::NMotMax, front.command.right.nMotMax); + send(MotorController::Command::NMotMax, back.command.left.nMotMax); + send(MotorController::Command::NMotMax, back.command.right.nMotMax); break; case 6: - send(MotorController::Command::FieldWeakMax, controllers.front.command.left.fieldWeakMax); - send(MotorController::Command::FieldWeakMax, controllers.front.command.right.fieldWeakMax); - send(MotorController::Command::FieldWeakMax, controllers.back.command.left.fieldWeakMax); - send(MotorController::Command::FieldWeakMax, controllers.back.command.right.fieldWeakMax); + send(MotorController::Command::FieldWeakMax, front.command.left.fieldWeakMax); + send(MotorController::Command::FieldWeakMax, front.command.right.fieldWeakMax); + send(MotorController::Command::FieldWeakMax, back.command.left.fieldWeakMax); + send(MotorController::Command::FieldWeakMax, back.command.right.fieldWeakMax); break; case 7: - send(MotorController::Command::PhaseAdvMax, controllers.front.command.left.phaseAdvMax); - send(MotorController::Command::PhaseAdvMax, controllers.front.command.right.phaseAdvMax); - send(MotorController::Command::PhaseAdvMax, controllers.back.command.left.phaseAdvMax); - send(MotorController::Command::PhaseAdvMax, controllers.back.command.right.phaseAdvMax); + send(MotorController::Command::PhaseAdvMax, front.command.left.phaseAdvMax); + send(MotorController::Command::PhaseAdvMax, front.command.right.phaseAdvMax); + send(MotorController::Command::PhaseAdvMax, back.command.left.phaseAdvMax); + send(MotorController::Command::PhaseAdvMax, back.command.right.phaseAdvMax); break; case 8: - send(MotorController::Command::BuzzerFreq, controllers.front.command.buzzer.freq); - send(MotorController::Command::BuzzerFreq, controllers.front.command.buzzer.freq); - send(MotorController::Command::BuzzerFreq, controllers.back.command.buzzer.freq); - send(MotorController::Command::BuzzerFreq, controllers.back.command.buzzer.freq); + if (send(MotorController::Command::BuzzerFreq, front.command.buzzer.freq) == ESP_OK) + lastValues.front.freq = front.command.buzzer.freq; +// if (send(MotorController::Command::BuzzerFreq, front.command.buzzer.freq) == ESP_OK) +// lastValues.front.freq = front.command.buzzer.freq; + if (send(MotorController::Command::BuzzerFreq, back.command.buzzer.freq) == ESP_OK) + lastValues.back.freq = back.command.buzzer.freq; +// if (send(MotorController::Command::BuzzerFreq, back.command.buzzer.freq) == ESP_OK) +// lastValues.back.freq = back.command.buzzer.freq; + if (send(MotorController::Command::BuzzerPattern, front.command.buzzer.pattern) == ESP_OK) + lastValues.front.pattern = front.command.buzzer.pattern; +// if (send(MotorController::Command::BuzzerPattern, front.command.buzzer.pattern) == ESP_OK) +// lastValues.front.pattern = front.command.buzzer.pattern; + if (send(MotorController::Command::BuzzerPattern, back.command.buzzer.pattern) == ESP_OK) + lastValues.back.pattern = back.command.buzzer.pattern; +// if (send(MotorController::Command::BuzzerPattern, back.command.buzzer.pattern) == ESP_OK) +// lastValues.back.pattern = back.command.buzzer.pattern; break; case 9: - send(MotorController::Command::BuzzerPattern, controllers.front.command.buzzer.pattern); - send(MotorController::Command::BuzzerPattern, controllers.front.command.buzzer.pattern); - send(MotorController::Command::BuzzerPattern, controllers.back.command.buzzer.pattern); - send(MotorController::Command::BuzzerPattern, controllers.back.command.buzzer.pattern); + send(MotorController::Command::Led, front.command.led); + //send(MotorController::Command::Led, front.command.led); + send(MotorController::Command::Led, back.command.led); + //send(MotorController::Command::Led, back.command.led); + send(MotorController::Command::Poweroff, front.command.poweroff); + //send(MotorController::Command::Poweroff, front.command.poweroff); + send(MotorController::Command::Poweroff, back.command.poweroff); + //send(MotorController::Command::Poweroff, back.command.poweroff); break; case 10: - send(MotorController::Command::Led, controllers.front.command.led); - send(MotorController::Command::Led, controllers.front.command.led); - send(MotorController::Command::Led, controllers.back.command.led); - send(MotorController::Command::Led, controllers.back.command.led); - break; - case 11: - send(MotorController::Command::Poweroff, controllers.front.command.poweroff); - send(MotorController::Command::Poweroff, controllers.front.command.poweroff); - send(MotorController::Command::Poweroff, controllers.back.command.poweroff); - send(MotorController::Command::Poweroff, controllers.back.command.poweroff); + if (send(Boardcomputer::Feedback::ButtonLeds, buttonLeds) == ESP_OK) + lastValues.buttonLeds = buttonLeds; + default: i=0; break; } } -} +} // namespace +} // namespace can diff --git a/src/displays/calibratedisplay.h b/src/displays/calibratedisplay.h index 89082b8..4be0c20 100644 --- a/src/displays/calibratedisplay.h +++ b/src/displays/calibratedisplay.h @@ -79,7 +79,7 @@ private: Status m_status; int16_t m_gasMin, m_gasMax, m_bremsMin, m_bremsMax; - float m_gas, m_brems; + std::optional m_gas, m_brems; }; CalibrateDisplay::CalibrateDisplay(bool bootup) : @@ -94,8 +94,8 @@ void CalibrateDisplay::start() m_selectedButton = 0; m_status = Status::Begin; copyFromSettings(); - m_gas = 0.f; - m_brems = 0.f; + m_gas = std::nullopt; + m_brems = std::nullopt; } void CalibrateDisplay::initScreen() @@ -124,14 +124,21 @@ void CalibrateDisplay::initScreen() void CalibrateDisplay::update() { - m_gas = scaleBetween(raw_gas, m_gasMin, m_gasMax, 0., 1000.); - m_brems = scaleBetween(raw_brems, m_bremsMin, m_bremsMax, 0., 1000.); + if (raw_gas) + m_gas = scaleBetween(*raw_gas, m_gasMin, m_gasMax, 0., 1000.); + else + m_gas = std::nullopt; + + if (raw_brems) + m_brems = scaleBetween(*raw_brems, m_bremsMin, m_bremsMax, 0., 1000.); + else + m_brems = std::nullopt; } void CalibrateDisplay::redraw() { - m_labels[0].redraw(std::to_string(m_gas)); - m_labels[1].redraw(std::to_string(raw_gas)); + m_labels[0].redraw(m_gas ? std::to_string(*m_gas) : "?"); + m_labels[1].redraw(raw_gas ? std::to_string(*raw_gas) : "?"); if (m_status == Status::GasMin) tft.setTextColor(TFT_RED, TFT_BLACK); m_labels[2].redraw(std::to_string(m_gasMin)); @@ -143,10 +150,10 @@ void CalibrateDisplay::redraw() if (m_status == Status::GasMax) tft.setTextColor(TFT_WHITE, TFT_BLACK); - m_progressBars[0].redraw(m_gas); + m_progressBars[0].redraw(m_gas ? *m_gas : 0); - m_labels[4].redraw(std::to_string(m_brems)); - m_labels[5].redraw(std::to_string(raw_brems)); + m_labels[4].redraw(m_brems ? std::to_string(*m_brems) : "?"); + m_labels[5].redraw(raw_brems ? std::to_string(*raw_brems) : "?"); if (m_status == Status::BremsMin) tft.setTextColor(TFT_RED, TFT_BLACK); m_labels[6].redraw(std::to_string(m_bremsMin)); @@ -158,7 +165,7 @@ void CalibrateDisplay::redraw() if (m_status == Status::BremsMax) tft.setTextColor(TFT_WHITE, TFT_BLACK); - m_progressBars[1].redraw(m_brems); + m_progressBars[1].redraw(m_brems ? *m_brems : 0); m_labels[8].redraw([&](){ switch (m_status) @@ -174,7 +181,8 @@ void CalibrateDisplay::redraw() }()); { - const auto color = m_status == Status::Confirm && (m_gas > 100 || m_brems > 100) ? TFT_DARKGREY : TFT_WHITE; + const auto failed = !m_gas || !m_brems || (m_status == Status::Confirm && (*m_gas > 100 || *m_brems > 100)); + const auto color = failed ? TFT_DARKGREY : TFT_WHITE; tft.setTextColor(color, TFT_BLACK); m_labels[9].redraw([&](){ switch (m_status) @@ -255,17 +263,20 @@ void CalibrateDisplay::confirm() switch (m_selectedButton) { case 0: // left button pressed + if (!raw_gas || !raw_brems || !m_gas || !m_brems) + return; + switch (m_status) { case Status::Begin: m_status = Status::GasMin; break; case Status::GasMin: - m_gasMin = raw_gas; + m_gasMin = *raw_gas; m_status = Status::GasMax; break; case Status::GasMax: - m_gasMax = raw_gas; + m_gasMax = *raw_gas; m_status = Status::BremsMin; { const auto dead = (m_gasMax - m_gasMin)/20; @@ -274,11 +285,11 @@ void CalibrateDisplay::confirm() } break; case Status::BremsMin: - m_bremsMin = raw_brems; + m_bremsMin = *raw_brems; m_status = Status::BremsMax; break; case Status::BremsMax: - m_bremsMax = raw_brems; + m_bremsMax = *raw_brems; m_status = Status::Confirm; { const auto dead = (m_bremsMax - m_bremsMin)/20; @@ -287,7 +298,7 @@ void CalibrateDisplay::confirm() } break; case Status::Confirm: - if (m_gas > 100 || m_brems > 100) + if (*m_gas > 100 || *m_brems > 100) return; copyToSettings(); saveSettings(); diff --git a/src/displays/dpad5wiredebugdisplay.h b/src/displays/dpad5wiredebugdisplay.h index 09c38af..c0137be 100644 --- a/src/displays/dpad5wiredebugdisplay.h +++ b/src/displays/dpad5wiredebugdisplay.h @@ -83,8 +83,8 @@ void DPad5WireDebugDisplay::redraw() m_labelProfile1.redraw(std::get(dpad5wire::lastState) ? "1" : "0"); m_labelProfile2.redraw(std::get(dpad5wire::lastState) ? "1" : "0"); m_labelProfile3.redraw(std::get(dpad5wire::lastState) ? "1" : "0"); - m_labelGas.redraw(std::to_string(raw_gas)); - m_labelBrems.redraw(std::to_string(raw_brems)); + m_labelGas.redraw(raw_gas ? std::to_string(*raw_gas) : "?"); + m_labelBrems.redraw(raw_brems ? std::to_string(*raw_brems) : "?"); } #endif } diff --git a/src/displays/menus/boardcomputerhardwaresettingsmenu.h b/src/displays/menus/boardcomputerhardwaresettingsmenu.h index cd0931e..af0db44 100644 --- a/src/displays/menus/boardcomputerhardwaresettingsmenu.h +++ b/src/displays/menus/boardcomputerhardwaresettingsmenu.h @@ -24,11 +24,23 @@ class SettingsMenu; namespace { struct GasText : public virtual TextInterface { public: - std::string text() const override { return std::string{"gas: "} + std::to_string(raw_gas) + ": " + std::to_string(gas); } + std::string text() const override + { + return std::string{"gas: "} + + (raw_gas ? std::to_string(*raw_gas) : "?") + + ": " + + (gas ? std::to_string(*gas) : "?"); + } }; struct BremsText : public virtual TextInterface { public: - std::string text() const override { return std::string{"brems: "} + std::to_string(raw_brems) + ": " + std::to_string(brems); } + std::string text() const override + { + return std::string{"brems: "} + + (raw_brems ? std::to_string(*raw_brems) : "?") + + ": " + + (brems ? std::to_string(*brems) : "?"); + } }; using SampleCountChangeScreen = makeComponent< diff --git a/src/displays/menus/controllerhardwaresettingsmenu.h b/src/displays/menus/controllerhardwaresettingsmenu.h index df6c851..4199fa6 100644 --- a/src/displays/menus/controllerhardwaresettingsmenu.h +++ b/src/displays/menus/controllerhardwaresettingsmenu.h @@ -60,9 +60,7 @@ public: constructMenuItem, SwitchScreenAction>>(); constructMenuItem, SwitchScreenAction>>(); constructMenuItem, SwitchScreenAction>>(); -#ifdef FEATURE_SERIAL constructMenuItem, ToggleBoolAction, CheckboxIcon, SwapFrontBackAccessor>>(); -#endif constructMenuItem, SwitchScreenAction, StaticMenuItemIcon<&icons::back>>>(); } }; diff --git a/src/displays/statusdisplay.h b/src/displays/statusdisplay.h index 3c3e432..a2dd0d6 100644 --- a/src/displays/statusdisplay.h +++ b/src/displays/statusdisplay.h @@ -145,12 +145,12 @@ void StatusDisplay::initScreen() void StatusDisplay::redraw() { tft.setTextFont(2); - m_labelRawGas.redraw(std::to_string(raw_gas)); - m_labelGas.redraw(std::to_string(gas)); - m_progressBarGas.redraw(gas); - m_labelRawBrems.redraw(std::to_string(raw_brems)); - m_labelBrems.redraw(std::to_string(brems)); - m_progressBarBrems.redraw(brems); + m_labelRawGas.redraw(raw_gas ? std::to_string(*raw_gas) : "?"); + m_labelGas.redraw(gas ? std::to_string(*gas) : "?"); + m_progressBarGas.redraw(gas ? *gas : 0); + m_labelRawBrems.redraw(raw_brems ? std::to_string(*raw_brems) : "?"); + m_labelBrems.redraw(brems ? std::to_string(*brems) : "?"); + m_progressBarBrems.redraw(brems ? *brems : 0); m_frontStatus.redraw(controllers.front); m_backStatus.redraw(controllers.back); diff --git a/src/dpad.h b/src/dpad.h index 1d0f9da..1430f57 100644 --- a/src/dpad.h +++ b/src/dpad.h @@ -4,8 +4,8 @@ #include -#include "globals.h" #include "types.h" +#include "buttons.h" namespace { namespace dpad diff --git a/src/dpad3wire.h b/src/dpad3wire.h index 8d3efe5..1ee36a7 100644 --- a/src/dpad3wire.h +++ b/src/dpad3wire.h @@ -2,11 +2,9 @@ #include -#include "globals.h" - #include "dpad.h" -#include "globals.h" #include "types.h" +#include "buttons.h" namespace { namespace dpad3wire diff --git a/src/dpad5wire.h b/src/dpad5wire.h index 90e70b5..73f61b0 100644 --- a/src/dpad5wire.h +++ b/src/dpad5wire.h @@ -1,17 +1,42 @@ #pragma once -#include +#include #include -#include "globals.h" #include "types.h" -#include "actions/switchprofileaction.h" +#include "buttons.h" namespace { namespace dpad5wire { -using State = std::tuple; +class State : public std::array +{ +public: + State() : std::array{false, false, false, false, false, false, false, false} {} + State(const std::array &other) : std::array{} {} + + State &operator=(const std::array &other) + { + std::array::operator=(other); + return *this; + } + + State &operator=(const State &other) + { + std::array::operator=(other); + return *this; + } + + bool &up{this->at(DPAD_5WIRESW_UP)}; + bool &down{this->at(DPAD_5WIRESW_DOWN)}; + bool &confirm{this->at(DPAD_5WIRESW_CONFIRM)}; + bool &back{this->at(DPAD_5WIRESW_BACK)}; + bool &profile0{this->at(DPAD_5WIRESW_PROFILE0)}; + bool &profile1{this->at(DPAD_5WIRESW_PROFILE1)}; + bool &profile2{this->at(DPAD_5WIRESW_PROFILE2)}; + bool &profile3{this->at(DPAD_5WIRESW_PROFILE3)}; +}; template class Helper @@ -37,6 +62,8 @@ void Helper::begin() template State Helper::read() { + State result; + digitalWrite(OUT, LOW); pinMode(IN1, INPUT_PULLUP); @@ -46,10 +73,10 @@ State Helper::read() delay(1); - const bool result0 = digitalRead(IN1)==LOW; - const bool result1 = digitalRead(IN2)==LOW; - const bool result2 = digitalRead(IN3)==LOW; - const bool result3 = digitalRead(IN4)==LOW; + result[0] = digitalRead(IN1)==LOW; + result[1] = digitalRead(IN2)==LOW; + result[2] = digitalRead(IN3)==LOW; + result[3] = digitalRead(IN4)==LOW; digitalWrite(OUT, HIGH); @@ -60,12 +87,12 @@ State Helper::read() delay(1); - const bool result4 = digitalRead(IN1); - const bool result5 = digitalRead(IN2); - const bool result6 = digitalRead(IN3); - const bool result7 = digitalRead(IN4); + result[4] = digitalRead(IN1); + result[5] = digitalRead(IN2); + result[6] = digitalRead(IN3); + result[7] = digitalRead(IN4); - return std::make_tuple(result0, result1, result2, result3, result4, result5, result6, result7); + return result; } #ifdef FEATURE_DPAD_5WIRESW @@ -81,77 +108,66 @@ void init() void update() { - const auto state = helper.read(); + const auto newState = helper.read(); #ifdef DPAD_5WIRESW_DEBUG - lastState = state; + lastState = newState; return; #endif const auto now = millis(); - if (std::get(lastState) != std::get(state) && now-debounceUp > settings.boardcomputerHardware.dpadDebounce) + if (lastState.up != newState.up && now-debounceUp > settings.boardcomputerHardware.dpadDebounce) { - if (std::get(state)) + if (newState.up) InputDispatcher::rotate(-1); - std::get(lastState) = std::get(state); debounceUp = now; } - if (std::get(lastState) != std::get(state) && now-debounceDown > settings.boardcomputerHardware.dpadDebounce) + + if (lastState.down != newState.down && now-debounceDown > settings.boardcomputerHardware.dpadDebounce) { - if (std::get(state)) + if (newState.down) InputDispatcher::rotate(1); - std::get(lastState) = std::get(state); debounceDown = now; } - if (std::get(lastState) != std::get(state) && now-debounceConfirm > settings.boardcomputerHardware.dpadDebounce) + + if (lastState.confirm != newState.confirm && now-debounceConfirm > settings.boardcomputerHardware.dpadDebounce) { - InputDispatcher::confirmButton(std::get(state)); - std::get(lastState) = std::get(state); + InputDispatcher::confirmButton(std::get(newState)); debounceConfirm = now; } - if (std::get(lastState) != std::get(state) && now-debounceBack > settings.boardcomputerHardware.dpadDebounce) + + if (lastState.back != newState.back && now-debounceBack > settings.boardcomputerHardware.dpadDebounce) { - InputDispatcher::backButton(std::get(state)); - std::get(lastState) = std::get(state); + InputDispatcher::backButton(std::get(newState)); debounceBack = now; } - if (std::get(lastState) != std::get(state) && now-debounceProfile0 > settings.boardcomputerHardware.dpadDebounce) + + if (lastState.profile0 != newState.profile0 && now-debounceProfile0 > settings.boardcomputerHardware.dpadDebounce) { - if (std::get(state)) - { - SwitchProfileAction<0>{}.triggered(); - } - std::get(lastState) = std::get(state); + InputDispatcher::profileButton(0, std::get(newState)); debounceProfile0 = now; } - if (std::get(lastState) != std::get(state) && now-debounceProfile1 > settings.boardcomputerHardware.dpadDebounce) + + if (lastState.profile1 != newState.profile1 && now-debounceProfile1 > settings.boardcomputerHardware.dpadDebounce) { - if (std::get(state)) - { - SwitchProfileAction<1>{}.triggered(); - } - std::get(lastState) = std::get(state); + InputDispatcher::profileButton(1, std::get(newState)); debounceProfile1 = now; } - if (std::get(lastState) != std::get(state) && now-debounceProfile2 > settings.boardcomputerHardware.dpadDebounce) + + if (lastState.profile2 != newState.profile2 && now-debounceProfile2 > settings.boardcomputerHardware.dpadDebounce) { - if (std::get(state)) - { - SwitchProfileAction<2>{}.triggered(); - } - std::get(lastState) = std::get(state); + InputDispatcher::profileButton(2, std::get(newState)); debounceProfile2 = now; } - if (std::get(lastState) != std::get(state) && now-debounceProfile3 > settings.boardcomputerHardware.dpadDebounce) + + if (lastState.profile3 != newState.profile3 && now-debounceProfile3 > settings.boardcomputerHardware.dpadDebounce) { - if (std::get(state)) - { - SwitchProfileAction<3>{}.triggered(); - } - std::get(lastState) = std::get(state); + InputDispatcher::profileButton(3, std::get(newState)); debounceProfile3 = now; } + + lastState = newState; } #endif } diff --git a/src/globals.h b/src/globals.h index 8c0c2f4..f9a5f6c 100644 --- a/src/globals.h +++ b/src/globals.h @@ -3,6 +3,7 @@ // system includes #include #include +#include // Arduino includes #ifdef FEATURE_BLUETOOTH @@ -21,8 +22,9 @@ #include "types.h" namespace { -int16_t raw_gas, raw_brems; -float gas, brems; +std::optional raw_gas, raw_brems; +std::optional gas, brems; + #ifdef FEATURE_GAMETRAK int16_t raw_gametrakX, raw_gametrakY, raw_gametrakDist; float gametrakX, gametrakY, gametrakDist; @@ -82,64 +84,4 @@ TFT_eSPI tft = TFT_eSPI(); ModeInterface *currentMode{}; std::unique_ptr currentDisplay; - -int rotated{}; -bool requestFullRedraw{}; -bool confirmButtonPressed{}; -bool confirmButtonLongPressed{}; -bool backButtonPressed{}; -bool backButtonLongPressed{}; - -class InputDispatcher -{ -public: - static void rotate(int offset) - { - rotated += offset; - } - - static void confirmButton(bool pressed) - { - static millis_t pressBegin = 0; - - const auto now = millis(); - - if (pressed) - pressBegin = now; - else - { - const auto duration = now - pressBegin; - - if (duration < 500) - confirmButtonPressed = true; - else if (duration < 2000) - confirmButtonLongPressed = true; - else - requestFullRedraw = true; - - pressBegin = 0; - } - } - - static void backButton(bool pressed) - { - static millis_t pressBegin = 0; - - const auto now = millis(); - - if (pressed) - pressBegin = now; - else - { - const auto duration = now - pressBegin; - - if (duration < 500) - backButtonPressed = true; - else - backButtonLongPressed = true; - - pressBegin = 0; - } - } -}; } diff --git a/src/main.cpp b/src/main.cpp index bc30dbe..4be24eb 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -308,7 +308,7 @@ void setup() #endif #ifdef FEATURE_CAN - initCan(); + can::initCan(); #endif #ifdef FEATURE_SERIAL @@ -319,10 +319,10 @@ void setup() controllers.back.serial.get().begin(38400, SERIAL_8N1, PINS_RX2, PINS_TX2); #endif - raw_gas = 0; - raw_brems = 0; - gas = 0; - brems = 0; + raw_gas = std::nullopt; + raw_brems = std::nullopt; + gas = std::nullopt; + brems = std::nullopt; for (Controller &controller : controllers) controller.command.buzzer = {}; @@ -357,7 +357,7 @@ void setup() return; #endif - if (gas > 200.f || brems > 200.f) + if (!gas || !brems || *gas > 200.f || *brems > 200.f) switchScreen(true); else switchScreen(); @@ -439,9 +439,7 @@ void loop() } #ifdef FEATURE_CAN - for (int i = 0; i < 4; i++) - if (!parseCanInput()) - break; + can::parseCanInput(); #endif #ifdef FEATURE_SERIAL diff --git a/src/modes/defaultmode.h b/src/modes/defaultmode.h index 0039af2..ff10507 100644 --- a/src/modes/defaultmode.h +++ b/src/modes/defaultmode.h @@ -31,64 +31,81 @@ DefaultMode defaultMode; void DefaultMode::update() { - if (waitForGasLoslass) + if (!gas || !brems) { - if (gas < 50) - waitForGasLoslass = false; - else - gas = 0; - } + start(); - if (waitForBremsLoslass) - { - if (brems < 50) - waitForBremsLoslass = false; - else - brems = 0; - } - - const auto gas_processed = settings.defaultMode.squareGas ? (gas * gas) / 1000.f : gas; - const auto brems_processed = settings.defaultMode.squareBrems ? (brems * brems) / 1000 : brems; - - const auto now = millis(); - - float pwm; - if (gas_processed >= settings.defaultMode.add_schwelle) - { - pwm = (gas_processed/1000.*settings.defaultMode.gas1_wert) + (brems_processed/1000.*settings.defaultMode.brems1_wert); - - if (settings.defaultMode.enableSmoothing && (pwm > 1000. || lastPwm > 1000.)) + for (bobbycar::protocol::serial::MotorState &motor : motors()) { - if (lastPwm < pwm) - { - pwm = std::min(pwm, lastPwm+(settings.defaultMode.smoothing*(now-lastTime)/100.f)); - if (pwm < 1000.) - pwm = 1000.; - } - else if (lastPwm > pwm) - { - pwm = std::max(pwm, lastPwm-(settings.defaultMode.smoothing*(now-lastTime)/100.f)); - } + motor.ctrlTyp = bobbycar::protocol::ControlType::FieldOrientedControl; + motor.ctrlMod = bobbycar::protocol::ControlMode::OpenMode; + motor.pwm = 0; } } else - pwm = (gas_processed/1000.*settings.defaultMode.gas2_wert) - (brems_processed/1000.*settings.defaultMode.brems2_wert); - - lastPwm = pwm; - lastTime = now; - - const auto pair = split(settings.defaultMode.modelMode); - for (bobbycar::protocol::serial::MotorState &motor : motorsInController(controllers.front)) { - motor.ctrlTyp = pair.first; - motor.ctrlMod = pair.second; - motor.pwm = pwm / 100. * settings.defaultMode.frontPercentage; - } - for (bobbycar::protocol::serial::MotorState &motor : motorsInController(controllers.back)) - { - motor.ctrlTyp = pair.first; - motor.ctrlMod = pair.second; - motor.pwm = pwm / 100. * settings.defaultMode.backPercentage; + float local_gas = *gas; + float local_brems = *brems; + + if (waitForGasLoslass) + { + if (local_gas < 50) + waitForGasLoslass = false; + else + local_gas = 0; + } + + if (waitForBremsLoslass) + { + if (local_brems < 50) + waitForBremsLoslass = false; + else + local_brems = 0; + } + + const auto gas_processed = settings.defaultMode.squareGas ? (local_gas * local_gas) / 1000.f : local_gas; + const auto brems_processed = settings.defaultMode.squareBrems ? (local_brems * local_brems) / 1000 : local_brems; + + const auto now = millis(); + + float pwm; + if (gas_processed >= settings.defaultMode.add_schwelle) + { + pwm = (gas_processed/1000.*settings.defaultMode.gas1_wert) + (brems_processed/1000.*settings.defaultMode.brems1_wert); + + if (settings.defaultMode.enableSmoothing && (pwm > 1000. || lastPwm > 1000.)) + { + if (lastPwm < pwm) + { + pwm = std::min(pwm, lastPwm+(settings.defaultMode.smoothing*(now-lastTime)/100.f)); + if (pwm < 1000.) + pwm = 1000.; + } + else if (lastPwm > pwm) + { + pwm = std::max(pwm, lastPwm-(settings.defaultMode.smoothing*(now-lastTime)/100.f)); + } + } + } + else + pwm = (gas_processed/1000.*settings.defaultMode.gas2_wert) - (brems_processed/1000.*settings.defaultMode.brems2_wert); + + lastPwm = pwm; + lastTime = now; + + const auto pair = split(settings.defaultMode.modelMode); + for (bobbycar::protocol::serial::MotorState &motor : motorsInController(controllers.front)) + { + motor.ctrlTyp = pair.first; + motor.ctrlMod = pair.second; + motor.pwm = pwm / 100. * settings.defaultMode.frontPercentage; + } + for (bobbycar::protocol::serial::MotorState &motor : motorsInController(controllers.back)) + { + motor.ctrlTyp = pair.first; + motor.ctrlMod = pair.second; + motor.pwm = pwm / 100. * settings.defaultMode.backPercentage; + } } fixCommonParams(); diff --git a/src/modes/gametrakmode.h b/src/modes/gametrakmode.h index a4166d6..2287280 100644 --- a/src/modes/gametrakmode.h +++ b/src/modes/gametrakmode.h @@ -41,36 +41,50 @@ void GametrakMode::start() void GametrakMode::update() { - if (gas > 500. || brems > 500.) + if (!gas || !brems) { - modes::defaultMode.waitForGasLoslass = true; - modes::defaultMode.waitForBremsLoslass = true; - currentMode = &modes::defaultMode; - return; - } + start(); - int16_t pwm; - if (gametrakDist < 150) - { - pwm = 0; - m_flag = false; + for (bobbycar::protocol::serial::MotorState &motor : motors()) + { + motor.ctrlTyp = bobbycar::protocol::ControlType::FieldOrientedControl; + motor.ctrlMod = bobbycar::protocol::ControlMode::OpenMode; + motor.pwm = 0; + } } else { - if (m_flag || gametrakDist >= 400) + if (*gas > 500. || *brems > 500.) { - m_flag = true; - pwm = clamp((gametrakDist - 400) / 2, -200, 200); + modes::defaultMode.waitForGasLoslass = true; + modes::defaultMode.waitForBremsLoslass = true; + currentMode = &modes::defaultMode; + return; + } + + int16_t pwm; + if (gametrakDist < 150) + { + pwm = 0; + m_flag = false; } else - pwm = 0; - } + { + if (m_flag || gametrakDist >= 400) + { + m_flag = true; + pwm = clamp((gametrakDist - 400) / 2, -200, 200); + } + else + pwm = 0; + } - for (bobbycar::protocol::serial::MotorState &motor : motors()) - { - motor.ctrlTyp = bobbycar::protocol::ControlType::FieldOrientedControl; - motor.ctrlMod = bobbycar::protocol::ControlMode::Speed; - motor.pwm = pwm; + for (bobbycar::protocol::serial::MotorState &motor : motors()) + { + motor.ctrlTyp = bobbycar::protocol::ControlType::FieldOrientedControl; + motor.ctrlMod = bobbycar::protocol::ControlMode::Speed; + motor.pwm = pwm; + } } fixCommonParams(); diff --git a/src/modes/larsmmode.h b/src/modes/larsmmode.h index 5fb219c..9ede112 100644 --- a/src/modes/larsmmode.h +++ b/src/modes/larsmmode.h @@ -36,72 +36,86 @@ void LarsmMode::start() void LarsmMode::update() { - for (uint8_t i = 0; i < settings.larsmMode.iterations; i++) // run multiple times to emulate higher refreshrate + if (!gas || !brems) { - // ####### larsm's bobby car code ####### + start(); - // LOW-PASS FILTER (fliessender Mittelwert) - adc1_filtered = adc1_filtered * 0.9 + gas * 0.1; // ADC1, TX, rechts, vorwaerts, blau - adc2_filtered = adc2_filtered * 0.9 + brems * 0.1; // ADC2, RX, links, rueckwearts, gruen + for (bobbycar::protocol::serial::MotorState &motor : motors()) + { + motor.ctrlTyp = bobbycar::protocol::ControlType::FieldOrientedControl; + motor.ctrlMod = bobbycar::protocol::ControlMode::OpenMode; + motor.pwm = 0; + } + } + else + { + for (uint8_t i = 0; i < settings.larsmMode.iterations; i++) // run multiple times to emulate higher refreshrate + { + // ####### larsm's bobby car code ####### - // magic numbers die ich nicht mehr nachvollziehen kann, faehrt sich aber gut ;-) - #define LOSLASS_BREMS_ACC 0.996f // naeher an 1 = gemaechlicher - #define DRUECK_ACC2 (1.0f - LOSLASS_BREMS_ACC + 0.001f) // naeher an 0 = gemaechlicher - #define DRUECK_ACC1 (1.0f - LOSLASS_BREMS_ACC + 0.001f) // naeher an 0 = gemaechlicher - //die + 0.001f gleichen float ungenauigkeiten aus. + // LOW-PASS FILTER (fliessender Mittelwert) + adc1_filtered = adc1_filtered * 0.9 + *gas * 0.1; // ADC1, TX, rechts, vorwaerts, blau + adc2_filtered = adc2_filtered * 0.9 + *brems * 0.1; // ADC2, RX, links, rueckwearts, gruen - #define ADC1_MIN 0 - #define ADC2_MIN 0 - #define ADC1_MAX 1000 - #define ADC2_MAX 1000 + // magic numbers die ich nicht mehr nachvollziehen kann, faehrt sich aber gut ;-) + #define LOSLASS_BREMS_ACC 0.996f // naeher an 1 = gemaechlicher + #define DRUECK_ACC2 (1.0f - LOSLASS_BREMS_ACC + 0.001f) // naeher an 0 = gemaechlicher + #define DRUECK_ACC1 (1.0f - LOSLASS_BREMS_ACC + 0.001f) // naeher an 0 = gemaechlicher + //die + 0.001f gleichen float ungenauigkeiten aus. - #define ADC2_DELTA (ADC2_MAX - ADC2_MIN) - #define ADC1_DELTA (ADC1_MAX - ADC1_MIN) + #define ADC1_MIN 0 + #define ADC2_MIN 0 + #define ADC1_MAX 1000 + #define ADC2_MAX 1000 - #define CLAMP(x, low, high) (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x))) + #define ADC2_DELTA (ADC2_MAX - ADC2_MIN) + #define ADC1_DELTA (ADC1_MAX - ADC1_MIN) - if (settings.larsmMode.mode == LarsmModeMode::Mode1) { // Mode 1, links: 3 kmh - speed = (float)speed * LOSLASS_BREMS_ACC // bremsen wenn kein poti gedrueckt - - (CLAMP(brems - ADC2_MIN, 0, ADC2_DELTA) / (ADC2_DELTA / 280.0f)) * DRUECK_ACC2 // links gedrueckt = zusatzbremsen oder rueckwaertsfahren - + (CLAMP(gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 350.0f)) * DRUECK_ACC1; // vorwaerts gedrueckt = beschleunigen 12s: 350=3kmh - weak = 0; + #define CLAMP(x, low, high) (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x))) - } else if (settings.larsmMode.mode == LarsmModeMode::Mode2) { // Mode 2, default: 6 kmh - speed = (float)speed * LOSLASS_BREMS_ACC - - (CLAMP(brems - ADC2_MIN, 0, ADC2_DELTA) / (ADC2_DELTA / 310.0f)) * DRUECK_ACC2 - + (CLAMP(gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 420.0f)) * DRUECK_ACC1; // 12s: 400=5-6kmh 450=7kmh - weak = 0; + if (settings.larsmMode.mode == LarsmModeMode::Mode1) { // Mode 1, links: 3 kmh + speed = (float)speed * LOSLASS_BREMS_ACC // bremsen wenn kein poti gedrueckt + - (CLAMP(*brems - ADC2_MIN, 0, ADC2_DELTA) / (ADC2_DELTA / 280.0f)) * DRUECK_ACC2 // links gedrueckt = zusatzbremsen oder rueckwaertsfahren + + (CLAMP(*gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 350.0f)) * DRUECK_ACC1; // vorwaerts gedrueckt = beschleunigen 12s: 350=3kmh + weak = 0; - } else if (settings.larsmMode.mode == LarsmModeMode::Mode3) { // Mode 3, rechts: 12 kmh - speed = (float)speed * LOSLASS_BREMS_ACC - - (CLAMP(brems - ADC2_MIN, 0, ADC2_DELTA) / (ADC2_DELTA / 340.0f)) * DRUECK_ACC2 - + (CLAMP(gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 600.0f)) * DRUECK_ACC1; // 12s: 600=12kmh - weak = 0; + } else if (settings.larsmMode.mode == LarsmModeMode::Mode2) { // Mode 2, default: 6 kmh + speed = (float)speed * LOSLASS_BREMS_ACC + - (CLAMP(*brems - ADC2_MIN, 0, ADC2_DELTA) / (ADC2_DELTA / 310.0f)) * DRUECK_ACC2 + + (CLAMP(*gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 420.0f)) * DRUECK_ACC1; // 12s: 400=5-6kmh 450=7kmh + weak = 0; - } else if (settings.larsmMode.mode == LarsmModeMode::Mode4) { // Mode 4, l + r: full kmh - // Feldschwaechung wird nur aktiviert wenn man schon sehr schnell ist. So gehts: Rechts voll druecken und warten bis man schnell ist, dann zusaetzlich links schnell voll druecken. - if (adc2_filtered > (ADC2_MAX - 450) && speed > 800) { // field weakening at high speeds - speed = (float)speed * LOSLASS_BREMS_ACC - + (CLAMP(gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 1000.0f)) * DRUECK_ACC1; - weak = weak * 0.95 + 400.0 * 0.05; // sanftes hinzuschalten des turbos, 12s: 400=29kmh - } else { //normale fahrt ohne feldschwaechung - speed = (float)speed * LOSLASS_BREMS_ACC - - (CLAMP(brems - ADC2_MIN, 0, ADC2_DELTA) / (ADC2_DELTA / 340.0f)) * DRUECK_ACC2 - + (CLAMP(gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 1000.0f)) * DRUECK_ACC1; // 12s: 1000=22kmh - weak = weak * 0.95; // sanftes abschalten des turbos - } - // weak should never exceed 400 or 450 MAX!! + } else if (settings.larsmMode.mode == LarsmModeMode::Mode3) { // Mode 3, rechts: 12 kmh + speed = (float)speed * LOSLASS_BREMS_ACC + - (CLAMP(*brems - ADC2_MIN, 0, ADC2_DELTA) / (ADC2_DELTA / 340.0f)) * DRUECK_ACC2 + + (CLAMP(*gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 600.0f)) * DRUECK_ACC1; // 12s: 600=12kmh + weak = 0; + + } else if (settings.larsmMode.mode == LarsmModeMode::Mode4) { // Mode 4, l + r: full kmh + // Feldschwaechung wird nur aktiviert wenn man schon sehr schnell ist. So gehts: Rechts voll druecken und warten bis man schnell ist, dann zusaetzlich links schnell voll druecken. + if (adc2_filtered > (ADC2_MAX - 450) && speed > 800) { // field weakening at high speeds + speed = (float)speed * LOSLASS_BREMS_ACC + + (CLAMP(*gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 1000.0f)) * DRUECK_ACC1; + weak = weak * 0.95 + 400.0 * 0.05; // sanftes hinzuschalten des turbos, 12s: 400=29kmh + } else { //normale fahrt ohne feldschwaechung + speed = (float)speed * LOSLASS_BREMS_ACC + - (CLAMP(*brems - ADC2_MIN, 0, ADC2_DELTA) / (ADC2_DELTA / 340.0f)) * DRUECK_ACC2 + + (CLAMP(*gas - ADC1_MIN, 0, ADC1_DELTA) / (ADC1_DELTA / 1000.0f)) * DRUECK_ACC1; // 12s: 1000=22kmh + weak = weak * 0.95; // sanftes abschalten des turbos + } + // weak should never exceed 400 or 450 MAX!! + } + + speed = CLAMP(speed, -1000, 1000); // clamp output } - speed = CLAMP(speed, -1000, 1000); // clamp output - } - - for (bobbycar::protocol::serial::MotorState &motor : motors()) - { - const auto pair = split(settings.larsmMode.modelMode); - motor.ctrlTyp = pair.first; - motor.ctrlMod = pair.second; - motor.pwm = speed + weak; + for (bobbycar::protocol::serial::MotorState &motor : motors()) + { + const auto pair = split(settings.larsmMode.modelMode); + motor.ctrlTyp = pair.first; + motor.ctrlMod = pair.second; + motor.pwm = speed + weak; + } } fixCommonParams(); diff --git a/src/modes/tempomatmode.h b/src/modes/tempomatmode.h index acc1e5b..81dcff2 100644 --- a/src/modes/tempomatmode.h +++ b/src/modes/tempomatmode.h @@ -32,23 +32,37 @@ void TempomatMode::start() void TempomatMode::update() { - if (gas > 500. && brems > 500.) + if (!gas || !brems) { - pwm = 0; - modes::defaultMode.waitForGasLoslass = true; - modes::defaultMode.waitForBremsLoslass = true; - currentMode = &modes::defaultMode; - return; + start(); + + for (bobbycar::protocol::serial::MotorState &motor : motors()) + { + motor.ctrlTyp = bobbycar::protocol::ControlType::FieldOrientedControl; + motor.ctrlMod = bobbycar::protocol::ControlMode::OpenMode; + motor.pwm = 0; + } } - - pwm += (gas/1000.) - (brems/1000.); - - for (bobbycar::protocol::serial::MotorState &motor : motors()) + else { - const auto pair = split(settings.tempomatMode.modelMode); - motor.ctrlTyp = pair.first; - motor.ctrlMod = pair.second; - motor.pwm = pwm; + if (*gas > 500. && *brems > 500.) + { + pwm = 0; + modes::defaultMode.waitForGasLoslass = true; + modes::defaultMode.waitForBremsLoslass = true; + currentMode = &modes::defaultMode; + return; + } + + pwm += (*gas/1000.) - (*brems/1000.); + + for (bobbycar::protocol::serial::MotorState &motor : motors()) + { + const auto pair = split(settings.tempomatMode.modelMode); + motor.ctrlTyp = pair.first; + motor.ctrlMod = pair.second; + motor.pwm = pwm; + } } fixCommonParams(); diff --git a/src/presets.h b/src/presets.h index 86897f9..351c860 100644 --- a/src/presets.h +++ b/src/presets.h @@ -32,9 +32,7 @@ constexpr Settings::ControllerHardware defaultControllerHardware { .wheelDiameter = DEFAULT_WHEELDIAMETER, .numMagnetPoles = 15, -#ifdef FEATURE_SERIAL .swapFrontBack = false -#endif }; constexpr Settings::ControllerHardware mosfetsOffControllerHardware { @@ -50,9 +48,7 @@ constexpr Settings::ControllerHardware mosfetsOffControllerHardware { .wheelDiameter = 165, .numMagnetPoles = 15, -#ifdef FEATURE_SERIAL .swapFrontBack = false -#endif }; constexpr Settings::WifiSettings defaultWifiSettings { @@ -79,9 +75,7 @@ constexpr Settings::ControllerHardware spinnerControllerHardware { .wheelDiameter = 165, .numMagnetPoles = 15, -#ifdef FEATURE_SERIAL .swapFrontBack = false -#endif }; constexpr Settings::BoardcomputerHardware::TimersSettings defaultTimersSettings { diff --git a/src/settings.h b/src/settings.h index 570b1aa..2c2ea53 100644 --- a/src/settings.h +++ b/src/settings.h @@ -51,9 +51,7 @@ struct Settings int16_t wheelDiameter; // in mm int16_t numMagnetPoles; // virtual RPM per one real RPM -#ifdef FEATURE_SERIAL bool swapFrontBack; -#endif } controllerHardware; struct BoardcomputerHardware { @@ -144,9 +142,7 @@ void Settings::executeForEverySetting(T &&callable) callable("wheelDiameter", controllerHardware.wheelDiameter); callable("numMagnetPoles", controllerHardware.numMagnetPoles); -#ifdef FEATURE_SERIAL callable("swapFrontBack", controllerHardware.swapFrontBack); -#endif callable("sampleCount", boardcomputerHardware.sampleCount); callable("gasMin", boardcomputerHardware.gasMin); diff --git a/src/settingsaccessors.h b/src/settingsaccessors.h index bc1377a..5f38cb9 100644 --- a/src/settingsaccessors.h +++ b/src/settingsaccessors.h @@ -56,12 +56,12 @@ struct WheelDiameterInchAccessor : public virtual AccessorInterface void setValue(float value) override { settings.controllerHardware.wheelDiameter = convertFromInch(value); saveSettings(); } }; struct NumMagnetPolesAccessor : public RefAccessorSaveSettings { int16_t &getRef() const override { return settings.controllerHardware.numMagnetPoles; } }; -#ifdef FEATURE_SERIAL struct SwapFrontBackAccessor : public RefAccessorSaveSettings { bool &getRef() const override { return settings.controllerHardware.swapFrontBack; } +#ifdef FEATURE_SERIAL void setValue(bool value) override { RefAccessorSaveSettings::setValue(value); updateSwapFrontBack(); }; -}; #endif +}; struct SampleCountAccessor : public RefAccessorSaveSettings { int16_t &getRef() const override { return settings.boardcomputerHardware.sampleCount; } }; struct GasMinAccessor : public RefAccessorSaveSettings { int16_t &getRef() const override { return settings.boardcomputerHardware.gasMin; } }; diff --git a/src/settingspersister.h b/src/settingspersister.h index 87bed2d..e6720b6 100644 --- a/src/settingspersister.h +++ b/src/settingspersister.h @@ -1,13 +1,12 @@ #pragma once #include +#include #include #include #include -#include - #include "settings.h" #ifdef FEATURE_BLUETOOTH #include "bluetoothmode.h" diff --git a/src/settingsutils.h b/src/settingsutils.h new file mode 100644 index 0000000..d4e285b --- /dev/null +++ b/src/settingsutils.h @@ -0,0 +1,23 @@ +#pragma once + +// Arduino includes +#include + +// local includes +#include "globals.h" +#include "presets.h" + +namespace { +void switchProfile(uint8_t index) +{ + settings = presets::defaultSettings; + + if (settingsPersister.openProfile(index)) + { + if (!settingsPersister.load(settings)) + Serial.println("switchProfile() load failed"); + } + else + Serial.println("switchProfile() openProfile failed"); +} +} diff --git a/src/statistics.h b/src/statistics.h index 2bc6a2b..5313fe7 100644 --- a/src/statistics.h +++ b/src/statistics.h @@ -18,8 +18,10 @@ ContainerType gas, brems, avgSpeed, avgSpeedKmh, sumCurrent, frontVoltage, backV void pushStats() { - statistics::gas.push_back(gas); - statistics::brems.push_back(brems); + if (gas) + statistics::gas.push_back(*gas); + if (brems) + statistics::brems.push_back(*brems); statistics::avgSpeed.push_back(avgSpeed); statistics::avgSpeedKmh.push_back(avgSpeedKmh); statistics::sumCurrent.push_back(sumCurrent); diff --git a/src/utils.h b/src/utils.h index 88eb8e8..e2d282f 100644 --- a/src/utils.h +++ b/src/utils.h @@ -265,7 +265,7 @@ void sendCommands() } #endif #ifdef FEATURE_CAN - sendCanCommands(); + can::sendCanCommands(); #endif } @@ -321,20 +321,55 @@ void updateAccumulators() } void readPotis() -{ - const auto sampleMultipleTimes = [](int pin){ +{ + [[maybe_unused]] + constexpr auto sampleMultipleTimes = [](int pin){ analogRead(pin); double sum{}; - for (int i = 0; i < settings.boardcomputerHardware.sampleCount; i++) + const auto sampleCount = settings.boardcomputerHardware.sampleCount; + for (int i = 0; i < sampleCount; i++) sum += analogRead(pin); - return sum/settings.boardcomputerHardware.sampleCount; + return sum / sampleCount; }; - raw_gas = sampleMultipleTimes(PINS_GAS); - gas = scaleBetween(raw_gas, settings.boardcomputerHardware.gasMin, settings.boardcomputerHardware.gasMax, 0., 1000.); + raw_gas = std::nullopt; + raw_brems = std::nullopt; - raw_brems = sampleMultipleTimes(PINS_BREMS); - brems = scaleBetween(raw_brems, settings.boardcomputerHardware.bremsMin, settings.boardcomputerHardware.bremsMax, 0., 1000.); +#ifdef FEATURE_CAN + const auto now = millis(); + + if (can::can_gas) + { + if (now - can::last_can_gas < 100) + raw_gas = *can::can_gas; + else + can::can_gas = std::nullopt; + } + + if (can::can_brems) + { + if (now - can::last_can_brems < 100) + raw_brems = *can::can_brems; + else + can::can_brems = std::nullopt; + } +#endif + +#ifdef FEATURE_ADC_IN + if (!raw_gas) + raw_gas = sampleMultipleTimes(PINS_GAS); + if (!raw_brems) + raw_brems = sampleMultipleTimes(PINS_BREMS); +#endif + + if (raw_gas) + gas = scaleBetween(*raw_gas, settings.boardcomputerHardware.gasMin, settings.boardcomputerHardware.gasMax, 0., 1000.); + else + gas = std::nullopt; + if (raw_brems) + brems = scaleBetween(*raw_brems, settings.boardcomputerHardware.bremsMin, settings.boardcomputerHardware.bremsMax, 0., 1000.); + else + brems = std::nullopt; #ifdef FEATURE_GAMETRAK raw_gametrakX = sampleMultipleTimes(PINS_GAMETRAKX);