diff --git a/robots/src/Sabo/robot_ex.hpp b/robots/src/Sabo/robot_ex.hpp index dde1d55..1d9a079 100644 --- a/robots/src/Sabo/robot_ex.hpp +++ b/robots/src/Sabo/robot_ex.hpp @@ -10,4 +10,13 @@ #define LINE_EMERGENCY_3 LINE_GPIO11 // Top stop button (Hall) #define LINE_EMERGENCY_4 LINE_GPIO10 // Back-handle stop (Capacitive) +// CoverUI +#define SPI_UI SPID1 // CoverUI is wired to SPI1 +#define LINE_UI_SCK LINE_SPI1_SCK // Clock for HEF4794BT & 74HC165 +#define LINE_UI_MISO LINE_SPI1_MISO // Button from 74HC165 +#define LINE_UI_MOSI LINE_SPI1_MOSI // LED & Button matrix to HEF4794BT +#define LINE_UI_LATCH_LOAD LINE_GPIO9 // HEF4794BT STR or 74HC165 /PL +#define LINE_UI_LED_OE LINE_GPIO8 // HEF4794BT OE +#define LINE_UI_BTN_CE LINE_GPIO1 // 74HC165 /CE + #endif // ROBOT_EX_HPP diff --git a/robots/src/Sabo/sabo_robot.cpp b/robots/src/Sabo/sabo_robot.cpp index 672a055..dab57f7 100644 --- a/robots/src/Sabo/sabo_robot.cpp +++ b/robots/src/Sabo/sabo_robot.cpp @@ -3,6 +3,7 @@ // #include +#include #include #include @@ -11,6 +12,9 @@ #include #include "robot.hpp" +#include "sabo_ui_controller.hpp" +#include "sabo_ui_driver.hpp" + namespace Robot { static BQ2576 charger{}; @@ -23,6 +27,9 @@ static DebugTCPInterface left_esc_driver_interface_{65102, &left_motor_driver}; static DebugTCPInterface mower_esc_driver_interface_{65103, &mower_motor_driver}; static DebugTCPInterface right_esc_driver_interface_{65104, &right_motor_driver}; +static SaboUIDriver ui_driver; +static SaboUIController ui(&ui_driver); + namespace General { void InitPlatform() { // Front left wheel lift (Hall) @@ -62,10 +69,20 @@ void InitPlatform() { mower_service.SetDriver(&mower_motor_driver); charger.setI2C(&I2CD1); power_service.SetDriver(&charger); + + // UI + ui.start(); + ui.setLED(SaboUIController::LEDID::START_RD, SaboUIController::LEDMode::BLINK_SLOW); } bool IsHardwareSupported() { - // FIXME: Fix EEPROM reading and check EEPROM - return true; + // Accept Sabo 0.1.x boards + if (strncmp("hw-openmower-sabo", carrier_board_info.board_id, sizeof(carrier_board_info.board_id)) == 0 && + strncmp("xcore", board_info.board_id, sizeof(board_info.board_id)) == 0 && board_info.version_major == 1 && + carrier_board_info.version_major == 0 && carrier_board_info.version_minor == 1) { + return true; + } + + return false; } } // namespace General diff --git a/robots/src/Sabo/sabo_ui_controller.cpp b/robots/src/Sabo/sabo_ui_controller.cpp new file mode 100644 index 0000000..c3c4893 --- /dev/null +++ b/robots/src/Sabo/sabo_ui_controller.cpp @@ -0,0 +1,151 @@ +// +// Created by Apehaenger on 4/20/25. +// +#include "sabo_ui_controller.hpp" + +#include + +static constexpr uint8_t EVT_PACKET_RECEIVED = 1; + +void SaboUIController::start() { + if (thread_ != nullptr) { + ULOG_ERROR("Started Sabo UI Controller twice!"); + return; + } + + if (driver_ == nullptr) { + ULOG_ERROR("Sabo UI Driver not set!"); + return; + } + if (!driver_->init()) { + ULOG_ERROR("Failed to initialize Sabo UI Driver!"); + return; + } + + thread_ = chThdCreateStatic(&wa_, sizeof(wa_), NORMALPRIO, ThreadHelper, this); +#ifdef USE_SEGGER_SYSTEMVIEW + processing_thread_->name = "SaboUIController"; +#endif + + // Now that driver is initialized and thread got started, we can enable output + driver_->enableOutput(); + chThdSleepMilliseconds(100); + playPowerOnAnimation(); +} + +void SaboUIController::ThreadHelper(void* instance) { + auto* i = static_cast(instance); + i->ThreadFunc(); +} + +void SaboUIController::tick() { + const systime_t now = chVTGetSystemTime(); + + // Slow blink handling + if (leds_.slow_blink_mask && (now - leds_.last_slow_update >= TIME_MS2I(500))) { + leds_.last_slow_update = now; + leds_.slow_blink_state = !leds_.slow_blink_state; + } + + // Fast blink handling + if (leds_.fast_blink_mask && (now - leds_.last_fast_update >= TIME_MS2I(100))) { + leds_.last_fast_update = now; + leds_.fast_blink_state = !leds_.fast_blink_state; + } + + // Final LED state calculation + uint8_t active_leds = leds_.on_mask; + active_leds |= leds_.slow_blink_state ? leds_.slow_blink_mask : 0; + active_leds |= leds_.fast_blink_state ? leds_.fast_blink_mask : 0; + + driver_->setLEDs(active_leds); + driver_->latchLoad(); + + // Button debouncing (of all buttons) + const uint16_t raw = driver_->getRawButtonStates(); + const uint16_t changed_bits = btn_last_raw_ ^ raw; // XOR to find changed bits + if (changed_bits == 0) { + if (btn_debounce_counter_ < DEBOUNCE_TICKS) btn_debounce_counter_++; + } else { + btn_debounce_counter_ = 0; + } + btn_stable_states_ = (btn_debounce_counter_ >= DEBOUNCE_TICKS) ? raw : btn_stable_states_; + btn_last_raw_ = raw; + + // Debug buttons + /*static uint16_t last_reported_states_ = 0xFFFF; // Last debug output + if (last_reported_states_ != btn_stable_states_) { + last_reported_states_ = btn_stable_states_; + ULOG_INFO("DEBUG: Buttons: 0x%04X (Raw: 0x%04X)", (~btn_stable_states_ & 0xFFFF), btn_stable_states_); + }*/ + + // Debug stack size + /*size_t stack_size = sizeof(wa_); + size_t unused_stack = (uint8_t*)thread_->ctx.sp - (uint8_t*)&wa_[0]; + size_t used_stack = stack_size - unused_stack; + ULOG_INFO("DEBUG: Stack: %u/%u used", used_stack, stack_size);*/ +} + +void SaboUIController::setLED(LEDID id, LEDMode mode) { + const uint8_t bit = 1 << uint8_t(id); + + // Clear existing state + leds_.on_mask &= ~bit; + leds_.slow_blink_mask &= ~bit; + leds_.fast_blink_mask &= ~bit; + + // Set new state + switch (mode) { + case LEDMode::ON: leds_.on_mask |= bit; break; + case LEDMode::BLINK_SLOW: leds_.slow_blink_mask |= bit; break; + case LEDMode::BLINK_FAST: leds_.fast_blink_mask |= bit; break; + case LEDMode::OFF: + default: break; + } +} + +void SaboUIController::playPowerOnAnimation() { + leds_.on_mask = 0x1F; // All on + chThdSleepMilliseconds(400); + leds_.on_mask = 0x00; // All off + chThdSleepMilliseconds(800); + + // Knight Rider + for (int i = 0; i <= 5; i++) { + leds_.on_mask = (1 << i) - 1; + chThdSleepMilliseconds(100); + } + for (int i = 4; i >= 0; i--) { + leds_.on_mask = (1 << i) - 1; + chThdSleepMilliseconds(100); + } +} + +void SaboUIController::ThreadFunc() { + while (true) { + tick(); + + // FIXME: Sample code to wait for some other event + eventmask_t event = chEvtWaitAnyTimeout( + EVT_PACKET_RECEIVED, + TIME_MS2I(1)); // FIXME: Once buttons are implemented, this needs to become something around 1ms + if (event & EVT_PACKET_RECEIVED) { + /*chSysLock(); + // Forbid packet reception + processing = true; + chSysUnlock(); + if (buffer_fill > 0) { + ProcessPacket(); + } + buffer_fill = 0; + chSysLock(); + // Allow packet reception + processing = false; + chSysUnlock();*/ + } + } +} + +bool SaboUIController::isButtonPressed(ButtonID btn) { + return (btn_stable_states_ & (1 << uint8_t(btn))) == 0; +} diff --git a/robots/src/Sabo/sabo_ui_controller.hpp b/robots/src/Sabo/sabo_ui_controller.hpp new file mode 100644 index 0000000..9f38fa3 --- /dev/null +++ b/robots/src/Sabo/sabo_ui_controller.hpp @@ -0,0 +1,58 @@ +// +// Created by Apehaenger on 4/20/25. +// + +#ifndef OPENMOWER_SABO_UI_CONTROLLER_HPP +#define OPENMOWER_SABO_UI_CONTROLLER_HPP + +#include "ch.h" +#include "sabo_ui_driver.hpp" + +class SaboUIController { + public: + explicit SaboUIController(SaboUIDriver* driver) : driver_(driver) { + } + + static constexpr uint8_t DEBOUNCE_TICKS = 20; // 20 * 2ms(tick) = 40ms debounce time + + enum class LEDID : uint8_t { AUTO, MOWING, HOME, START_GN, START_RD }; // Same bits as connected to HEF4794BT + enum class LEDMode { OFF, ON, BLINK_SLOW, BLINK_FAST }; + enum class ButtonID : uint8_t { UP = 0, DOWN, LEFT, RIGHT, OK, START, MENU = 8, BACK, AUTO, MOW, HOME }; + + void start(); // Initializes the UI driver and starts the controller thread + void setLED(LEDID id, LEDMode mode); // Set LED state + + bool isButtonPressed(ButtonID btn); // Debounced safe check if a button is pressed + // void setButtonCallback(std::function callback); + + void playPowerOnAnimation(); + + private: + THD_WORKING_AREA(wa_, 1024); + thread_t* thread_ = nullptr; + + SaboUIDriver* driver_; + // std::function button_callback_; + + struct LEDState { + uint8_t on_mask = 0; + uint8_t slow_blink_mask = 0; + uint8_t fast_blink_mask = 0; + systime_t last_slow_update = 0; + systime_t last_fast_update = 0; + bool slow_blink_state = false; + bool fast_blink_state = false; + }; + LEDState leds_; + + uint16_t btn_last_raw_ = 0xFFFF; // Last raw button state + uint16_t btn_stable_states_ = 0xFFFF; // Stable (debounced) button state + uint8_t btn_debounce_counter_ = 0; // If this counter is >= DEBOUNCE_TICKS, the button state is stable/debounced + + static void ThreadHelper(void* instance); + void ThreadFunc(); + + void tick(); +}; + +#endif // OPENMOWER_SABO_UI_CONTROLLER_HPP diff --git a/robots/src/Sabo/sabo_ui_driver.cpp b/robots/src/Sabo/sabo_ui_driver.cpp new file mode 100644 index 0000000..35f9dd4 --- /dev/null +++ b/robots/src/Sabo/sabo_ui_driver.cpp @@ -0,0 +1,92 @@ +// +// Created by Apehaenger on 4/19/25. +// +#include "sabo_ui_driver.hpp" + +#include + +#include "globals.hpp" +#include "robot_ex.hpp" + +bool SaboUIDriver::init() { + // Init SPI pins + palSetLineMode(LINE_UI_SCK, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_MID2 | PAL_STM32_PUPDR_FLOATING); + palSetLineMode(LINE_UI_MISO, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_MID2 | PAL_STM32_PUPDR_FLOATING); + palSetLineMode(LINE_UI_MOSI, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_MID2 | PAL_STM32_PUPDR_FLOATING); + + // Init GPIOs for chip control + palSetLineMode(LINE_UI_LATCH_LOAD, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_MID2 | PAL_STM32_PUPDR_FLOATING); + palSetLineMode(LINE_UI_LED_OE, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_MID2 | PAL_STM32_PUPDR_FLOATING); + palSetLineMode(LINE_UI_BTN_CE, + PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_MID2 | + PAL_STM32_PUPDR_PULLDOWN); // FIXME: CoverUI doesn't has any pull-up/down resistors? + + // Configure SPI + spi_ = &SPI_UI; + spi_cfg_ = { + .circular = false, + .slave = false, + .data_cb = nullptr, + .error_cb = nullptr, + .ssline = 0, // Master mode + .cfg1 = SPI_CFG1_MBR_0 | SPI_CFG1_MBR_1 | // Baudrate = FPCLK/16 (~10 MHz @ 160 MHz SPI-C) + SPI_CFG1_DSIZE_2 | SPI_CFG1_DSIZE_1 | SPI_CFG1_DSIZE_0, // 8-Bit (DS = 0b111) + .cfg2 = SPI_CFG2_MASTER // Master mode + }; + + // Start SPI + if (spiStart(spi_, &spi_cfg_) != MSG_OK) { + ULOG_ERROR("Error starting UI-SPI"); + return false; + } + + return true; +} + +void SaboUIDriver::latchLoad() { + uint8_t button_data = 0; + uint8_t tx_data = current_leds_ | ((current_button_row_ + 1) << 5); // Bit5/6 for row0/1 + + // Load 74HC165 shift register (the very first load will load nothing, because row is not set yet. Who cares?) + palWriteLine(LINE_UI_LATCH_LOAD, PAL_LOW); // /PL (parallel load) Pulse + chThdSleepMicroseconds(1); + palWriteLine(LINE_UI_LATCH_LOAD, PAL_HIGH); + + // SPI transfer LEDs and read button for previously set row + spiAcquireBus(spi_); + palWriteLine(LINE_UI_LATCH_LOAD, PAL_HIGH); // Latch HEF4794BT (STR = HIGH) + palWriteLine(LINE_UI_BTN_CE, PAL_LOW); // HC165 /CE + spiExchange(spi_, 1, &tx_data, &button_data); // Send data and receive button data + palWriteLine(LINE_UI_LATCH_LOAD, PAL_LOW); // Stop latching (STR = LOW) + palWriteLine(LINE_UI_BTN_CE, PAL_HIGH); + spiReleaseBus(spi_); + + // Buffer / Shift & buffer depending on current row + if (current_button_row_ == 0) { + button_states_ = (button_states_ & 0xFF00) | button_data; + } else { + button_states_ = (button_states_ & 0x00FF) | (button_data << 8); + } + + // Alternate between row0 and row1 + current_button_row_ ^= 1; +} + +void SaboUIDriver::enableOutput() { + // TODO: OE could also be used with a PWM signal to dim the LEDs + + if (carrier_board_info.version_major <= 0 && carrier_board_info.version_minor <= 1) { + // Sabo v0.1 has an HEF4794BT OE driver which inverts the signal. Newer boards will not have this driver anymore. + palWriteLine(LINE_UI_LED_OE, PAL_LOW); + } else { + palWriteLine(LINE_UI_BTN_CE, PAL_HIGH); + } +} + +void SaboUIDriver::setLEDs(uint8_t leds) { + current_leds_ = leds & 0x1F; // LEDs are on the lower 5 bits +} + +uint16_t SaboUIDriver::getRawButtonStates() const { + return button_states_; +} diff --git a/robots/src/Sabo/sabo_ui_driver.hpp b/robots/src/Sabo/sabo_ui_driver.hpp new file mode 100644 index 0000000..024e4f0 --- /dev/null +++ b/robots/src/Sabo/sabo_ui_driver.hpp @@ -0,0 +1,28 @@ +// +// Created by Apehaenger on 4/19/25. +// + +#ifndef OPENMOWER_SABO_UI_DRIVER_HPP +#define OPENMOWER_SABO_UI_DRIVER_HPP + +#include "ch.h" +#include "hal.h" + +class SaboUIDriver { + private: + SPIDriver* spi_; + SPIConfig spi_cfg_; + + uint8_t current_leds_ = 0; + uint8_t current_button_row_ = 0; // Alternating button rows + uint16_t button_states_ = 0; // Bits 0-7: Row0, Bits 8-15: Row1. Low-active! + + public: + bool init(); // Init SPI and GPIOs + void latchLoad(); // Latch LEDs as well as button-row, and load button columns + void enableOutput(); // Enable output for HEF4794BT + uint16_t getRawButtonStates() const; // Get the raw button states (0-15). Low-active! + void setLEDs(uint8_t leds); // Set LEDs to a specific pattern +}; + +#endif // OPENMOWER_SABO_UI_DRIVER_HPP