diff --git a/stm32-modules/flex-stacker/firmware/CMakeLists.txt b/stm32-modules/flex-stacker/firmware/CMakeLists.txt
index af0368367..4b5c63034 100644
--- a/stm32-modules/flex-stacker/firmware/CMakeLists.txt
+++ b/stm32-modules/flex-stacker/firmware/CMakeLists.txt
@@ -21,7 +21,9 @@ set(${TARGET_MODULE_NAME}_FW_LINTABLE_SRCS
     ${SYSTEM_DIR}/freertos_idle_timer_task.cpp
     ${SYSTEM_DIR}/freertos_system_task.cpp
     ${SYSTEM_DIR}/system_policy.cpp
+    ${SYSTEM_DIR}/i2c_comms.cpp
     ${UI_DIR}/freertos_ui_task.cpp
+    ${UI_DIR}/ui_policy.cpp
     ${MOTOR_CONTROL_DIR}/freertos_motor_task.cpp
     ${MOTOR_CONTROL_DIR}/freertos_motor_driver_task.cpp
     ${MOTOR_CONTROL_DIR}/motor_driver_policy.cpp
@@ -36,6 +38,7 @@ set(${TARGET_MODULE_NAME}_FW_NONLINTABLE_SRCS
     ${SYSTEM_DIR}/hal_tick.c
     ${SYSTEM_DIR}/system_serial_number.c
     ${SYSTEM_DIR}/system_hardware.c
+    ${SYSTEM_DIR}/i2c_hardware.c
     ${UI_DIR}/ui_hardware.c
     ${COMMS_DIR}/usbd_conf.c
     ${COMMS_DIR}/usbd_desc.c
diff --git a/stm32-modules/flex-stacker/firmware/main.cpp b/stm32-modules/flex-stacker/firmware/main.cpp
index bb09f4f91..cd661e9a9 100644
--- a/stm32-modules/flex-stacker/firmware/main.cpp
+++ b/stm32-modules/flex-stacker/firmware/main.cpp
@@ -1,6 +1,8 @@
 #include "FreeRTOS.h"
 #include "firmware/firmware_tasks.hpp"
 #include "firmware/freertos_tasks.hpp"
+#include "firmware/i2c_comms.hpp"
+#include "firmware/i2c_hardware.h"
 #include "firmware/motor_hardware.h"
 #include "firmware/system_stm32g4xx.h"
 #include "flex-stacker/messages.hpp"
@@ -15,6 +17,8 @@
 #pragma GCC diagnostic pop
 
 using EntryPoint = std::function<void(tasks::FirmwareTasks::QueueAggregator *)>;
+using EntryPointUI = std::function<void(tasks::FirmwareTasks::QueueAggregator *,
+                                        i2c::hardware::I2C *)>;
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static auto motor_driver_task_entry = EntryPoint(motor_driver_task::run);
@@ -23,7 +27,7 @@ static auto motor_driver_task_entry = EntryPoint(motor_driver_task::run);
 static auto motor_task_entry = EntryPoint(motor_control_task::run);
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
-static auto ui_task_entry = EntryPoint(ui_control_task::run);
+static auto ui_task_entry = EntryPointUI(ui_control_task::run);
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static auto host_comms_entry = EntryPoint(host_comms_control_task::run);
@@ -51,7 +55,7 @@ static auto host_comms_task =
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static auto ui_task =
-    ot_utils::freertos_task::FreeRTOSTask<tasks::UI_STACK_SIZE, EntryPoint>(
+    ot_utils::freertos_task::FreeRTOSTask<tasks::UI_STACK_SIZE, EntryPointUI>(
         ui_task_entry);
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
@@ -71,18 +75,24 @@ extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
     }
 }
 
+static auto i2c2_comms = i2c::hardware::I2C();
+static auto i2c3_comms = i2c::hardware::I2C();
+static auto i2c_handles = I2CHandlerStruct{};
+
 auto main() -> int {
     HardwareInit();
 
-    system_task.start(tasks::SYSTEM_TASK_PRIORITY, "System", &aggregator);
+    i2c_hardware_init(&i2c_handles);
+
+    i2c2_comms.set_handle(i2c_handles.i2c2, I2C_BUS_2);
+    i2c3_comms.set_handle(i2c_handles.i2c3, I2C_BUS_3);
 
+    system_task.start(tasks::SYSTEM_TASK_PRIORITY, "System", &aggregator);
     driver_task.start(tasks::MOTOR_DRIVER_TASK_PRIORITY, "Motor Driver",
                       &aggregator);
     motor_task.start(tasks::MOTOR_TASK_PRIORITY, "Motor", &aggregator);
-
     host_comms_task.start(tasks::COMMS_TASK_PRIORITY, "Comms", &aggregator);
-
-    ui_task.start(tasks::UI_TASK_PRIORITY, "UI", &aggregator);
+    ui_task.start(tasks::UI_TASK_PRIORITY, "UI", &aggregator, &i2c2_comms);
 
     vTaskStartScheduler();
     return 0;
diff --git a/stm32-modules/flex-stacker/firmware/system/i2c_comms.cpp b/stm32-modules/flex-stacker/firmware/system/i2c_comms.cpp
new file mode 100644
index 000000000..d1e05205a
--- /dev/null
+++ b/stm32-modules/flex-stacker/firmware/system/i2c_comms.cpp
@@ -0,0 +1,27 @@
+#include "firmware/i2c_comms.hpp"
+
+#include <stdint.h>
+
+#include "firmware/i2c_hardware.h"
+#include "systemwide.h"
+
+using namespace i2c::hardware;
+
+auto I2C::set_handle(HAL_I2C_HANDLE i2c_handle, I2C_BUS i2c_bus) -> void {
+    this->bus = i2c_bus;
+    i2c_register_handle(i2c_handle, i2c_bus);
+}
+
+auto I2C::i2c_write(uint16_t dev_addr, uint16_t reg, uint8_t* data,
+                    uint16_t size) -> RxTxReturn {
+    MessageT resp{0};
+    auto ret = hal_i2c_write(bus, dev_addr, reg, data, size);
+    return RxTxReturn(ret, resp);
+}
+
+auto I2C::i2c_read(uint16_t dev_addr, uint16_t reg, uint16_t size)
+    -> RxTxReturn {
+    MessageT resp{0};
+    auto ret = hal_i2c_read(bus, dev_addr, reg, resp.data(), size);
+    return RxTxReturn(ret, resp);
+}
diff --git a/stm32-modules/flex-stacker/firmware/system/i2c_hardware.c b/stm32-modules/flex-stacker/firmware/system/i2c_hardware.c
new file mode 100644
index 000000000..82f2751f0
--- /dev/null
+++ b/stm32-modules/flex-stacker/firmware/system/i2c_hardware.c
@@ -0,0 +1,275 @@
+#include <stdint.h>
+
+#include "stm32g4xx_hal.h"
+#include "stm32g4xx_hal_conf.h"
+#include "stm32g4xx_hal_gpio.h"
+#include "stm32g4xx_hal_def.h"
+#include "stm32g4xx_it.h"
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "firmware/i2c_hardware.h"
+
+#define MAX_TIMEOUT (pdMS_TO_TICKS(100))
+#define MAX_RETRIES (3)
+#define MAX_I2C_HANDLES (3)
+#define NO_HANDLE_ERROR (255)
+#define REGISTER_ADDR_LEN (1)  // 1 byte
+
+static I2C_HandleTypeDef hi2c2;
+static I2C_HandleTypeDef hi2c3;
+
+typedef struct {
+    I2C_BUS bus;
+    I2C_HandleTypeDef *i2c_handle;
+    TaskHandle_t task_to_notify;
+    bool should_retry;
+} NotificationHandle_t;
+
+static NotificationHandle_t _notification_handles[MAX_I2C_HANDLES];
+
+static bool _initialized = false;
+
+HAL_I2C_HANDLE MX_I2C2_Init()
+{
+    hi2c2.State = HAL_I2C_STATE_RESET;
+    hi2c2.Instance = I2C2;
+    hi2c2.Init.Timing = 0x10C0ECFF;
+    hi2c2.Init.OwnAddress1 = 0;
+    hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+    hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+    hi2c2.Init.OwnAddress2 = 0;
+    hi2c2.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
+    hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+    hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
+    if (HAL_I2C_Init(&hi2c2) != HAL_OK)
+    {
+        Error_Handler();
+    }
+    /** Configure Analogue filter
+    */
+    if (HAL_I2CEx_ConfigAnalogFilter(&hi2c2, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
+    {
+        Error_Handler();
+    }
+    /** Configure Digital filter
+    */
+    if (HAL_I2CEx_ConfigDigitalFilter(&hi2c2, 0) != HAL_OK)
+    {
+        Error_Handler();
+    }
+
+    /** I2C Fast mode Plus enable */
+    __HAL_SYSCFG_FASTMODEPLUS_ENABLE(I2C_FASTMODEPLUS_I2C2);
+
+    return &hi2c2;
+}
+
+HAL_I2C_HANDLE MX_I2C3_Init() {
+    hi2c3.Instance = I2C3;
+    hi2c3.Init.Timing = 0x10C0ECFF;
+    hi2c3.Init.OwnAddress1 = 0;
+    hi2c3.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+    hi2c3.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+    hi2c3.Init.OwnAddress2 = 0;
+    hi2c3.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
+    hi2c3.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+    hi2c3.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
+    if (HAL_I2C_Init(&hi2c3) != HAL_OK) {
+        Error_Handler();
+    }
+    /** Configure Analogue filter
+     */
+    if (HAL_I2CEx_ConfigAnalogFilter(&hi2c3, I2C_ANALOGFILTER_ENABLE) !=
+        HAL_OK) {
+        Error_Handler();
+    }
+    /** Configure Digital filter
+     */
+    if (HAL_I2CEx_ConfigDigitalFilter(&hi2c3, 0) != HAL_OK) {
+        Error_Handler();
+    }
+
+    /** I2C Fast mode Plus enable */
+    __HAL_SYSCFG_FASTMODEPLUS_ENABLE(I2C_FASTMODEPLUS_I2C3);
+
+    return &hi2c3;
+}
+
+void i2c_hardware_init(I2CHandlerStruct* i2c_handles) {
+    HAL_I2C_HANDLE i2c2 = MX_I2C2_Init();
+    HAL_I2C_HANDLE i2c3 = MX_I2C3_Init();
+    i2c_handles->i2c2 = i2c2;
+    i2c_handles->i2c3 = i2c3;
+}
+
+/**
+ * @brief Get the notification handle based on the I2C Bus.
+ * Returns NULL if the handle is not found.
+ */
+static NotificationHandle_t* lookup_handle(I2C_BUS bus) {
+    for(size_t i = 0; i < MAX_I2C_HANDLES; ++i) {
+        I2C_BUS notif_bus = _notification_handles[i].bus;
+        if(notif_bus != NO_BUS && notif_bus == bus) {
+            return &_notification_handles[i];
+        }
+    }
+    return NULL;
+}
+
+/**
+ * @brief Get the notification handle based on the HAL I2C struct.
+ * Returns NULL if the handle is not found.
+ */
+static NotificationHandle_t* get_handle(I2C_HandleTypeDef *i2c_handle) {
+    for(size_t i = 0; i < MAX_I2C_HANDLES; ++i) {
+        if(_notification_handles[i].i2c_handle == i2c_handle) {
+            return &_notification_handles[i];
+        }
+    }
+    return NULL;
+}
+
+static void initialize_notification_handles() {
+    if(!_initialized) {
+        for(size_t i = 0; i < MAX_I2C_HANDLES; ++i) {
+            _notification_handles[i].i2c_handle = NULL;
+            _notification_handles[i].task_to_notify = NULL;
+            _notification_handles[i].should_retry = false;
+            _notification_handles[i].bus = NO_BUS;
+        }
+        _initialized = true;
+    }
+}
+
+/**
+ * @brief Common handler for all I2C callbacks.
+ */
+static void handle_i2c_callback(I2C_HandleTypeDef *i2c_handle, bool error) {
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+    NotificationHandle_t *instance = get_handle(i2c_handle);
+    if(instance == NULL) return;
+    if(instance->task_to_notify == NULL) return;
+
+    vTaskNotifyGiveFromISR(instance->task_to_notify, 
+                            &xHigherPriorityTaskWoken);
+    instance->task_to_notify = NULL;
+    instance->should_retry = error;
+    portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
+}
+
+bool i2c_register_handle(HAL_I2C_HANDLE handle, I2C_BUS bus) {
+    initialize_notification_handles();
+
+    I2C_HandleTypeDef* i2c_handle = (I2C_HandleTypeDef*)handle;
+    NotificationHandle_t *notif_handle = lookup_handle(bus);
+    if(notif_handle != NULL) return true;  // Already Registered
+
+    // Now find an empty slot
+    notif_handle = get_handle(NULL);
+    if(notif_handle == NULL) return false; // No empty slots
+
+    notif_handle->i2c_handle = i2c_handle;
+    notif_handle->bus = bus;
+    return true;
+}
+
+/**
+ * Wrapper around HAL_I2C_Mem_Write
+ */
+uint8_t hal_i2c_write(I2C_BUS bus, uint16_t DevAddress, uint8_t reg, uint8_t *data, uint16_t size) {
+    uint32_t notification_val = 0;
+    NotificationHandle_t *notification_handle = lookup_handle(bus);
+    I2C_HandleTypeDef* i2c_handle = (I2C_HandleTypeDef*)notification_handle->i2c_handle;
+
+    // Bus was not registered
+    if(notification_handle == NULL) return false;
+
+    uint8_t retries = 0;
+    HAL_StatusTypeDef tx_result = HAL_OK;
+    do {
+        // Setup task handle and send message
+        notification_handle->task_to_notify = xTaskGetCurrentTaskHandle();
+        tx_result = HAL_I2C_Mem_Write_IT(i2c_handle,
+                            DevAddress, reg, REGISTER_ADDR_LEN, data, size);
+
+        // Wait for callback and check result
+        notification_val = ulTaskNotifyTake(pdTRUE, MAX_TIMEOUT);
+        if (tx_result == HAL_OK) break;
+        if (notification_handle->should_retry) tx_result = HAL_BUSY;
+        if (notification_val != 1) tx_result = HAL_TIMEOUT;
+
+        retries += 1;
+    } while(retries < MAX_RETRIES);
+    return tx_result;
+}
+
+/**
+ * Wrapper around HAL_I2C_Mem_Read
+ */
+uint8_t hal_i2c_read(I2C_BUS bus, uint16_t DevAddress, uint16_t reg, uint8_t *data, uint16_t size) {
+    uint32_t notification_val = 0;
+    NotificationHandle_t *notification_handle = lookup_handle(bus);
+    I2C_HandleTypeDef* i2c_handle = (I2C_HandleTypeDef*)notification_handle->i2c_handle;
+
+    // Bus was not registered
+    if(notification_handle == NULL) return NO_HANDLE_ERROR;
+
+    uint8_t retries = 0;
+    HAL_StatusTypeDef rx_result = HAL_OK;
+    do {
+        // Setup task handle and send message
+        notification_handle->task_to_notify = xTaskGetCurrentTaskHandle();
+        rx_result = HAL_I2C_Mem_Read_IT(i2c_handle,
+                            DevAddress, reg, REGISTER_ADDR_LEN, data, size);
+
+        // Wait for callback and check result.
+        notification_val = ulTaskNotifyTake(pdTRUE, MAX_TIMEOUT);
+        if (rx_result == HAL_OK) break;
+        if (notification_handle->should_retry) rx_result = HAL_BUSY;
+        if (notification_val != 1) rx_result = HAL_TIMEOUT;
+
+        retries += 1;
+    } while (retries < MAX_RETRIES);
+    return rx_result;
+}
+
+void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *i2c_handle) {
+    handle_i2c_callback(i2c_handle, false);
+}
+
+void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *i2c_handle) {
+    handle_i2c_callback(i2c_handle, false);
+}
+
+void HAL_I2C_MasterTxCpltCallback(I2C_HandleTypeDef *i2c_handle) {
+    handle_i2c_callback(i2c_handle, false);
+}
+
+void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *i2c_handle) {
+    handle_i2c_callback(i2c_handle, false);
+}
+
+void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *i2c_handle) {
+    handle_i2c_callback(i2c_handle, true);
+}
+
+void I2C2_EV_IRQHandler(void)
+{
+    HAL_I2C_EV_IRQHandler(&hi2c2);
+}
+
+void I2C2_ER_IRQHandler(void)
+{
+    HAL_I2C_ER_IRQHandler(&hi2c2);
+}
+
+void I2C3_EV_IRQHandler(void)
+{
+    HAL_I2C_EV_IRQHandler(&hi2c3);
+}
+
+void I2C3_ER_IRQHandler(void)
+{
+    HAL_I2C_ER_IRQHandler(&hi2c3);
+}
diff --git a/stm32-modules/flex-stacker/firmware/system/main.h b/stm32-modules/flex-stacker/firmware/system/main.h
index 5d5b0f1f3..20da410de 100644
--- a/stm32-modules/flex-stacker/firmware/system/main.h
+++ b/stm32-modules/flex-stacker/firmware/system/main.h
@@ -72,10 +72,14 @@ void Error_Handler(void);
 #define TOF_I2C3_SCL_GPIO_Port GPIOC
 #define TOF_I2C3_SDA_Pin GPIO_PIN_9
 #define TOF_I2C3_SDA_GPIO_Port GPIOC
+
 #define EEPROM_I2C2_SDA_Pin GPIO_PIN_8
 #define EEPROM_I2C2_SDA_GPIO_Port GPIOA
 #define EEPOM_I2C2_SCL_Pin GPIO_PIN_9
 #define EEPOM_I2C2_SCL_GPIO_Port GPIOA
+#define EEPROM_WP_PIN GPIO_PIN_10
+#define EEPROM_WP_PORT GPIOA
+
 #define nSTATUS_LED_Pin GPIO_PIN_10
 #define nSTATUS_LED_GPIO_Port GPIOC
 #define nLW_RELEASED_Pin GPIO_PIN_11
diff --git a/stm32-modules/flex-stacker/firmware/system/stm32g4xx_hal_msp.c b/stm32-modules/flex-stacker/firmware/system/stm32g4xx_hal_msp.c
index bec6e11c1..5f3a45390 100644
--- a/stm32-modules/flex-stacker/firmware/system/stm32g4xx_hal_msp.c
+++ b/stm32-modules/flex-stacker/firmware/system/stm32g4xx_hal_msp.c
@@ -73,18 +73,8 @@ void HAL_MspDeInit(void)
 void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c)
 {
     GPIO_InitTypeDef GPIO_InitStruct = {0};
-    RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
     if(hi2c->Instance==I2C2)
     {
-        /** Initializes the peripherals clocks
-         */
-        PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_I2C2;
-        PeriphClkInit.I2c2ClockSelection = RCC_I2C2CLKSOURCE_PCLK1;
-        if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
-        {
-            Error_Handler();
-        }
-
         __HAL_RCC_GPIOA_CLK_ENABLE();
         /**I2C2 GPIO Configuration
         PA8     ------> I2C2_SDA
@@ -93,24 +83,20 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c)
         GPIO_InitStruct.Pin = EEPROM_I2C2_SDA_Pin|EEPOM_I2C2_SCL_Pin;
         GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
         GPIO_InitStruct.Pull = GPIO_NOPULL;
-        GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+        GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
         GPIO_InitStruct.Alternate = GPIO_AF4_I2C2;
         HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
         /* Peripheral clock enable */
         __HAL_RCC_I2C2_CLK_ENABLE();
+
+        HAL_NVIC_SetPriority(I2C2_EV_IRQn, 7, 0);
+        HAL_NVIC_SetPriority(I2C2_ER_IRQn, 7, 0);
+        HAL_NVIC_EnableIRQ(I2C2_EV_IRQn);
+        HAL_NVIC_EnableIRQ(I2C2_ER_IRQn);
     }
     else if(hi2c->Instance==I2C3)
     {
-        /** Initializes the peripherals clocks
-         */
-        PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_I2C3;
-        PeriphClkInit.I2c3ClockSelection = RCC_I2C3CLKSOURCE_PCLK1;
-        if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
-        {
-            Error_Handler();
-        }
-
         __HAL_RCC_GPIOC_CLK_ENABLE();
         /**I2C3 GPIO Configuration
         PC8     ------> I2C3_SCL
@@ -119,14 +105,18 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c)
         GPIO_InitStruct.Pin = TOF_I2C3_SCL_Pin|TOF_I2C3_SDA_Pin;
         GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
         GPIO_InitStruct.Pull = GPIO_NOPULL;
-        GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+        GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
         GPIO_InitStruct.Alternate = GPIO_AF8_I2C3;
         HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
 
         /* Peripheral clock enable */
         __HAL_RCC_I2C3_CLK_ENABLE();
-    }
 
+        HAL_NVIC_SetPriority(I2C3_EV_IRQn, 7, 0);
+        HAL_NVIC_SetPriority(I2C3_ER_IRQn, 7, 0);
+        HAL_NVIC_EnableIRQ(I2C3_EV_IRQn);
+        HAL_NVIC_EnableIRQ(I2C3_ER_IRQn);
+    }
 }
 
 /**
diff --git a/stm32-modules/flex-stacker/firmware/ui/freertos_ui_task.cpp b/stm32-modules/flex-stacker/firmware/ui/freertos_ui_task.cpp
index a69376669..7a11b41c6 100644
--- a/stm32-modules/flex-stacker/firmware/ui/freertos_ui_task.cpp
+++ b/stm32-modules/flex-stacker/firmware/ui/freertos_ui_task.cpp
@@ -1,17 +1,35 @@
+#include "FreeRTOS.h"
 #include "firmware/freertos_tasks.hpp"
+#include "firmware/i2c_comms.hpp"
 #include "firmware/ui_hardware.h"
-
-static constexpr uint32_t HALF_SECOND = 500;
+#include "firmware/ui_policy.hpp"
+#include "flex-stacker/ui_task.hpp"
 
 namespace ui_control_task {
+using namespace ui_policy;
+
+enum class Notifications : uint8_t {
+    INCOMING_MESSAGE = 1,
+};
+
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+static tasks::FirmwareTasks::UIQueue
+    // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+    _queue(static_cast<uint8_t>(Notifications::INCOMING_MESSAGE), "UI Queue");
+
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+static auto _top_task = ui_task::UITask(_queue, nullptr, nullptr);
+
+auto run(tasks::FirmwareTasks::QueueAggregator* aggregator,
+         i2c::hardware::I2C* i2c_comms) -> void {
+    auto* handle = xTaskGetCurrentTaskHandle();
+    _queue.provide_handle(handle);
+    aggregator->register_queue(_queue);
+    _top_task.provide_aggregator(aggregator);
 
-auto run(tasks::FirmwareTasks::QueueAggregator* aggregator) -> void {
-    std::ignore = aggregator;
+    auto policy = UIPolicy(i2c_comms);
     while (true) {
-        ui_hardware_set_heartbeat_led(true);
-        vTaskDelay(HALF_SECOND);
-        ui_hardware_set_heartbeat_led(false);
-        vTaskDelay(HALF_SECOND);
+        _top_task.run_once(policy);
     }
 }
 
diff --git a/stm32-modules/flex-stacker/firmware/ui/ui_hardware.c b/stm32-modules/flex-stacker/firmware/ui/ui_hardware.c
index cf57bde44..316a55822 100644
--- a/stm32-modules/flex-stacker/firmware/ui/ui_hardware.c
+++ b/stm32-modules/flex-stacker/firmware/ui/ui_hardware.c
@@ -1,18 +1,10 @@
-#include "main.h"
-
 #include <stdbool.h>
 
 #include "stm32g4xx_hal.h"
-
-
-#define nSTATUS_LED_Pin GPIO_PIN_10
-#define nSTATUS_LED_GPIO_Port GPIOC
+#include "main.h"
 
 void ui_hardware_set_heartbeat_led(bool setting) {
     //NOLINTNEXTLINE(performance-no-int-to-ptr)
     HAL_GPIO_WritePin(nSTATUS_LED_GPIO_Port, nSTATUS_LED_Pin,
         setting ? GPIO_PIN_SET : GPIO_PIN_RESET);
-
-//    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_2,
-//                      setting ? GPIO_PIN_SET : GPIO_PIN_RESET);
 }
diff --git a/stm32-modules/flex-stacker/firmware/ui/ui_policy.cpp b/stm32-modules/flex-stacker/firmware/ui/ui_policy.cpp
new file mode 100644
index 000000000..3f24ab6fb
--- /dev/null
+++ b/stm32-modules/flex-stacker/firmware/ui/ui_policy.cpp
@@ -0,0 +1,11 @@
+#include "firmware/ui_policy.hpp"
+
+#include "firmware/ui_hardware.h"
+
+namespace ui_policy {
+
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+auto UIPolicy::set_heartbeat_led(bool value) -> void {
+    ui_hardware_set_heartbeat_led(value);
+}
+};  // namespace ui_policy
diff --git a/stm32-modules/flex-stacker/src/errors.cpp b/stm32-modules/flex-stacker/src/errors.cpp
index b1e23d767..79b0f169e 100644
--- a/stm32-modules/flex-stacker/src/errors.cpp
+++ b/stm32-modules/flex-stacker/src/errors.cpp
@@ -16,6 +16,8 @@ const char* const SYSTEM_SERIAL_NUMBER_HAL_ERROR =
     "ERR302:system:HAL error, busy, or timeout\n";
 const char* const SYSTEM_EEPROM_ERROR =
     "ERR303:system:EEPROM communication error\n";
+const char* const SYSTEM_SET_STATUSBAR_COLOR_ERROR =
+    "ERR304:system:STATUSBAR communication error\n";
 const char* const TMC2160_READ_ERROR = "ERR901:TMC2160 driver read error\n";
 const char* const TMC2160_WRITE_ERROR = "ERR902:TMC2160 driver write error\n";
 const char* const TMC2160_INVALID_ADDRESS = "ERR903:TMC2160 invalid address\n";
@@ -49,6 +51,7 @@ auto errors::errorstring(ErrorCode code) -> const char* {
         HANDLE_CASE(SYSTEM_SERIAL_NUMBER_INVALID);
         HANDLE_CASE(SYSTEM_SERIAL_NUMBER_HAL_ERROR);
         HANDLE_CASE(SYSTEM_EEPROM_ERROR);
+        HANDLE_CASE(SYSTEM_SET_STATUSBAR_COLOR_ERROR);
         HANDLE_CASE(TMC2160_READ_ERROR);
         HANDLE_CASE(TMC2160_WRITE_ERROR);
         HANDLE_CASE(TMC2160_INVALID_ADDRESS);
diff --git a/stm32-modules/include/common/core/is31fl_driver.hpp b/stm32-modules/include/common/core/is31fl_driver.hpp
index 061703c70..e84e7b41c 100644
--- a/stm32-modules/include/common/core/is31fl_driver.hpp
+++ b/stm32-modules/include/common/core/is31fl_driver.hpp
@@ -51,14 +51,13 @@ class IS31FL {
     // Send updated values to the driver
     template <IS31FL_Policy Policy>
     auto send_update(Policy& policy) -> bool {
-        if (!policy.i2c_write(Address, PWM_REGISTER_BASE, _pwm_settings)) {
-            return false;
-        }
-        if (!policy.i2c_write(Address, LED_CONTROL_REGISTER_BASE,
-                              _current_settings)) {
-            return false;
-        }
-        return write_single_reg(UPDATE_REGISTER, TRIGGER_UPDATE_VALUE, policy);
+        auto res1 = policy.i2c_write(Address, PWM_REGISTER_BASE, _pwm_settings);
+        //policy.sleep_ms(1);
+        auto res2 = policy.i2c_write(Address, LED_CONTROL_REGISTER_BASE, _current_settings);
+        //policy.sleep_ms(1);
+        auto res3 = write_single_reg(UPDATE_REGISTER, TRIGGER_UPDATE_VALUE, policy);
+        //policy.sleep_ms(1);
+        return res1 && res2 && res3;
     }
 
     // Update current setting for a channel
@@ -152,4 +151,4 @@ class IS31FL {
     std::array<uint8_t, 1> _single_reg_buffer{};
 };
 
-};  // namespace is31fl
\ No newline at end of file
+};  // namespace is31fl
diff --git a/stm32-modules/include/flex-stacker/firmware/freertos_tasks.hpp b/stm32-modules/include/flex-stacker/firmware/freertos_tasks.hpp
index 016cd4a51..55d42ecc8 100644
--- a/stm32-modules/include/flex-stacker/firmware/freertos_tasks.hpp
+++ b/stm32-modules/include/flex-stacker/firmware/freertos_tasks.hpp
@@ -3,6 +3,7 @@
 #include "FreeRTOS.h"
 #include "firmware/firmware_tasks.hpp"
 #include "firmware/freertos_message_queue.hpp"
+#include "firmware/i2c_comms.hpp"
 #include "task.h"
 
 namespace motor_driver_task {
@@ -22,7 +23,8 @@ auto run(tasks::FirmwareTasks::QueueAggregator* aggregator) -> void;
 namespace ui_control_task {
 
 // Actual function that runs in the task
-auto run(tasks::FirmwareTasks::QueueAggregator* aggregator) -> void;
+auto run(tasks::FirmwareTasks::QueueAggregator* aggregator,
+         i2c::hardware::I2C* i2c_comms) -> void;
 }  // namespace ui_control_task
 
 namespace host_comms_control_task {
diff --git a/stm32-modules/include/flex-stacker/firmware/hardware_iface.hpp b/stm32-modules/include/flex-stacker/firmware/hardware_iface.hpp
new file mode 100644
index 000000000..52ccc8f29
--- /dev/null
+++ b/stm32-modules/include/flex-stacker/firmware/hardware_iface.hpp
@@ -0,0 +1,33 @@
+#pragma once
+
+#include <stdint.h>
+
+#include <cstdint>
+#include <optional>
+#include <tuple>
+
+using std::size_t;
+static constexpr size_t MESSAGE_LEN = 5;
+using MessageT = std::array<uint8_t, MESSAGE_LEN>;
+
+namespace i2c {
+namespace hardware {
+using RxTxReturn = std::tuple<uint8_t, MessageT>;
+class I2CBase {
+  public:
+    I2CBase() = default;
+    virtual ~I2CBase() = default;
+    I2CBase(const I2CBase&) = default;
+    auto operator=(const I2CBase&) -> I2CBase& = default;
+    I2CBase(I2CBase&&) = default;
+    auto operator=(I2CBase&&) -> I2CBase& = default;
+
+    virtual auto i2c_read(uint16_t dev_addr, uint16_t reg, uint16_t size)
+        -> RxTxReturn;
+    virtual auto i2c_write(uint16_t dev_addr, uint16_t reg, uint8_t* data,
+                           uint16_t size) -> RxTxReturn;
+};
+
+};  // namespace hardware
+
+};  // namespace i2c
diff --git a/stm32-modules/include/flex-stacker/firmware/i2c_comms.hpp b/stm32-modules/include/flex-stacker/firmware/i2c_comms.hpp
new file mode 100644
index 000000000..47cf28bae
--- /dev/null
+++ b/stm32-modules/include/flex-stacker/firmware/i2c_comms.hpp
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <stdint.h>
+
+#include <algorithm>
+#include <cstdint>
+#include <optional>
+
+#include "firmware/hardware_iface.hpp"
+#include "firmware/i2c_hardware.h"
+#include "systemwide.h"
+
+namespace i2c {
+namespace hardware {
+class I2C : public I2CBase {
+  public:
+    explicit I2C() = default;
+    ~I2C() final = default;
+    I2C(const I2C &) = delete;
+    I2C(const I2C &&) = delete;
+    auto operator=(const I2C &) = delete;
+    auto operator=(const I2C &&) = delete;
+
+    auto i2c_read(uint16_t dev_addr, uint16_t reg, uint16_t size) -> RxTxReturn;
+    auto i2c_write(uint16_t dev_addr, uint16_t reg, uint8_t *data,
+                   uint16_t size) -> RxTxReturn;
+    auto set_handle(HAL_I2C_HANDLE i2c_handle, I2C_BUS bus) -> void;
+
+  private:
+    I2C_BUS bus = NO_BUS;
+    HAL_I2C_HANDLE handle = nullptr;
+};
+};  // namespace hardware
+};  // namespace i2c
diff --git a/stm32-modules/include/flex-stacker/firmware/i2c_hardware.h b/stm32-modules/include/flex-stacker/firmware/i2c_hardware.h
new file mode 100644
index 000000000..d8dc814ba
--- /dev/null
+++ b/stm32-modules/include/flex-stacker/firmware/i2c_hardware.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "systemwide.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif  // __cplusplus
+
+typedef enum I2C_BUS {
+    I2C_BUS_2,
+    I2C_BUS_3,
+    NO_BUS,
+} I2C_BUS;
+
+typedef void *HAL_I2C_HANDLE;
+
+typedef struct HandlerStruct {
+    HAL_I2C_HANDLE i2c2;
+    HAL_I2C_HANDLE i2c3;
+} I2CHandlerStruct;
+
+void i2c_hardware_init(I2CHandlerStruct *i2c_handles);
+bool i2c_register_handle(HAL_I2C_HANDLE handle, I2C_BUS bus);
+uint8_t hal_i2c_write(I2C_BUS bus, uint16_t DevAddress, uint8_t reg,
+                      uint8_t *data, uint16_t size);
+uint8_t hal_i2c_read(I2C_BUS bus, uint16_t DevAddress, uint16_t reg,
+                     uint8_t *data, uint16_t size);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif  // __cplusplus
diff --git a/stm32-modules/include/flex-stacker/firmware/ui_policy.hpp b/stm32-modules/include/flex-stacker/firmware/ui_policy.hpp
new file mode 100644
index 000000000..1526342d2
--- /dev/null
+++ b/stm32-modules/include/flex-stacker/firmware/ui_policy.hpp
@@ -0,0 +1,31 @@
+#pragma once
+
+#include <array>
+#include <cstdint>
+
+#include "i2c_comms.hpp"
+
+namespace ui_policy {
+using namespace i2c::hardware;
+
+class UIPolicy {
+  public:
+    UIPolicy(I2C *i2c) : i2c_comms{i2c} {}
+    UIPolicy(const UIPolicy &) = delete;
+    UIPolicy(const UIPolicy &&) = delete;
+    auto operator=(const UIPolicy &) = delete;
+    auto operator=(const UIPolicy &&) = delete;
+
+    auto set_heartbeat_led(bool value) -> void;
+    template <size_t Len>
+    auto i2c_write(uint8_t device_address, uint8_t register_address,
+                   std::array<uint8_t, Len> &data) -> bool {
+        auto [ret, _] = i2c_comms->i2c_write(device_address, register_address,
+                                             data.data(), Len);
+        return ret == 0;
+    }
+
+  private:
+    I2C *i2c_comms;
+};
+};  // namespace ui_policy
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp b/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp
index f0fda3c37..8a463e8d1 100644
--- a/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp
+++ b/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp
@@ -19,6 +19,7 @@ enum class ErrorCode {
     SYSTEM_SERIAL_NUMBER_INVALID = 301,
     SYSTEM_SERIAL_NUMBER_HAL_ERROR = 302,
     SYSTEM_EEPROM_ERROR = 303,
+    SYSTEM_SET_STATUSBAR_COLOR_ERROR = 304,
     // 9xx - TMC2160
     TMC2160_READ_ERROR = 901,
     TMC2160_WRITE_ERROR = 902,
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp b/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp
index 21251861e..4a9f01185 100644
--- a/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp
+++ b/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp
@@ -211,4 +211,55 @@ struct GetDoorClosed {
     }
 };
 
+struct SetStatusBarColor {
+    std::optional<StatusBarID> bar_id;
+    std::optional<StatusBarColor> color;
+    float power;
+
+    using ParseResult = std::optional<SetStatusBarColor>;
+    static constexpr auto prefix = std::array{'M', '2', '0', '0', ' '};
+    static constexpr const char* response = "M200 OK\n";
+
+    using PowerArg = Arg<float, 'P'>;
+    using ColorArg = Arg<uint8_t, 'C'>;
+    using KArg = Arg<uint8_t, 'K'>;
+
+    template <typename InputIt, typename Limit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<Limit, InputIt>
+    static auto parse(const InputIt& input, Limit limit)
+        -> std::pair<ParseResult, InputIt> {
+        auto res = gcode::SingleParser<PowerArg, ColorArg, KArg>::parse_gcode(
+            input, limit, prefix);
+        if (!res.first.has_value()) {
+            return std::make_pair(ParseResult(), input);
+        }
+
+        auto ret = SetStatusBarColor{
+            .bar_id = std::nullopt, .color = std::nullopt, .power = 0};
+
+        auto arguments = res.first.value();
+        if (std::get<0>(arguments).present) {
+            ret.power = static_cast<float>(std::get<0>(arguments).value);
+        }
+
+        if (std::get<1>(arguments).present) {
+            ret.color =
+                static_cast<StatusBarColor>(std::get<1>(arguments).value);
+        }
+
+        if (std::get<2>(arguments).present) {
+            ret.bar_id = static_cast<StatusBarID>(std::get<2>(arguments).value);
+        }
+        return std::make_pair(ret, res.second);
+    }
+
+    template <typename InputIt, typename InLimit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<InputIt, InLimit>
+    static auto write_response_into(InputIt buf, InLimit limit) -> InputIt {
+        return write_string_to_iterpair(buf, limit, response);
+    }
+};
+
 }  // namespace gcode
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp
index 9f607ebd5..1c9d8e7ed 100644
--- a/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp
+++ b/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp
@@ -15,6 +15,7 @@
 #include "flex-stacker/errors.hpp"
 #include "flex-stacker/gcodes.hpp"
 #include "flex-stacker/messages.hpp"
+#include "gcodes.hpp"
 #include "gcodes_motor.hpp"
 #include "hal/message_queue.hpp"
 #include "messages.hpp"
@@ -48,14 +49,15 @@ class HostCommsTask {
         gcode::GetLimitSwitches, gcode::SetMicrosteps, gcode::GetMoveParams,
         gcode::SetMotorStallGuard, gcode::GetMotorStallGuard, gcode::HomeMotor,
         gcode::GetPlatformSensors, gcode::GetDoorClosed, gcode::GetEstopStatus,
-        gcode::StopMotor>;
+        gcode::StopMotor, gcode::SetStatusBarColor>;
     using AckOnlyCache =
         AckCache<8, gcode::EnterBootloader, gcode::SetSerialNumber,
                  gcode::SetTMCRegister, gcode::SetRunCurrent,
                  gcode::SetHoldCurrent, gcode::EnableMotor, gcode::DisableMotor,
                  gcode::MoveMotorInSteps, gcode::MoveToLimitSwitch,
                  gcode::MoveMotorInMm, gcode::SetMicrosteps,
-                 gcode::SetMotorStallGuard, gcode::HomeMotor, gcode::StopMotor>;
+                 gcode::SetStatusBarColor, gcode::SetMotorStallGuard,
+                 gcode::HomeMotor, gcode::StopMotor>;
     using GetSystemInfoCache = AckCache<8, gcode::GetSystemInfo>;
     using GetTMCRegisterCache = AckCache<8, gcode::GetTMCRegister>;
     using GetLimitSwitchesCache = AckCache<8, gcode::GetLimitSwitches>;
@@ -997,6 +999,32 @@ class HostCommsTask {
         return std::make_pair(true, tx_into);
     }
 
+    template <typename InputIt, typename InputLimit>
+    requires std::forward_iterator<InputIt> &&
+        std::sized_sentinel_for<InputLimit, InputIt>
+    auto visit_gcode(const gcode::SetStatusBarColor& gcode, InputIt tx_into,
+                     InputLimit tx_limit) -> std::pair<bool, InputIt> {
+        auto id = ack_only_cache.add(gcode);
+        if (id == 0) {
+            return std::make_pair(
+                false, errors::write_into(tx_into, tx_limit,
+                                          errors::ErrorCode::GCODE_CACHE_FULL));
+        }
+        auto message = messages::SetStatusBarColorMessage{
+            .id = id,
+            .bar_id = gcode.bar_id,
+            .power = gcode.power,
+            .color = gcode.color,
+        };
+        if (!task_registry->send(message, TICKS_TO_WAIT_ON_SEND)) {
+            auto wrote_to = errors::write_into(
+                tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL);
+            ack_only_cache.remove_if_present(id);
+            return std::make_pair(false, wrote_to);
+        }
+        return std::make_pair(true, tx_into);
+    }
+
     // Our error handler just writes an error and bails
     template <typename InputIt, typename InputLimit>
     requires std::forward_iterator<InputIt> &&
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp b/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp
index d9e6df612..cec348e7a 100644
--- a/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp
+++ b/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp
@@ -272,6 +272,13 @@ struct GetEstopResponse {
     bool triggered;
 };
 
+struct SetStatusBarColorMessage {
+    uint32_t id = 0;
+    std::optional<StatusBarID> bar_id = std::nullopt;
+    std::optional<float> power = std::nullopt;
+    std::optional<StatusBarColor> color = std::nullopt;
+};
+
 using HostCommsMessage =
     ::std::variant<std::monostate, IncomingMessageFromHost, ForceUSBDisconnect,
                    ErrorMessage, AcknowledgePrevious, GetSystemInfoResponse,
@@ -285,6 +292,8 @@ using SystemMessage =
                    SetSerialNumberMessage, EnterBootloaderMessage,
                    GetDoorClosedMessage>;
 
+using UIMessage = ::std::variant<std::monostate, SetStatusBarColorMessage>;
+
 using MotorDriverMessage =
     ::std::variant<std::monostate, SetTMCRegisterMessage, GetTMCRegisterMessage,
                    PollTMCRegisterMessage, StopPollTMCRegisterMessage,
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp
index 09c81066b..3544ccf2f 100644
--- a/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp
+++ b/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp
@@ -69,7 +69,7 @@ struct Defaults {
         static constexpr float SPEED_DISCONT = 5.0;
 
         static constexpr float MM_PER_REV =
-            lms::GearBoxConfig::mm_per_rev(16.0, 16.0 / 30.0);
+            lms::GearBoxConfig::mm_per_rev(16.0, 30.0 / 16.0);
         static constexpr float STEPS_PER_REV = 200;
         static constexpr float MICROSTEP = 16;
     };
@@ -422,6 +422,15 @@ class MotorTask {
         _z_controller.stop_movement(0, true);
         _x_controller.stop_movement(0, false);
         _l_controller.stop_movement(0, false);
+
+        // Set status bars to RED
+        auto message = messages::SetStatusBarColorMessage{
+            .bar_id = StatusBarID::Internal, .color = StatusBarColor::Red};
+        static_cast<void>(
+            _task_registry->send_to_address(message, Queues::UIAddress));
+        message.bar_id = StatusBarID::External;
+        static_cast<void>(
+            _task_registry->send_to_address(message, Queues::UIAddress));
     }
 
     /**
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/tasks.hpp b/stm32-modules/include/flex-stacker/flex-stacker/tasks.hpp
index 79e85127a..fc2b91a45 100644
--- a/stm32-modules/include/flex-stacker/flex-stacker/tasks.hpp
+++ b/stm32-modules/include/flex-stacker/flex-stacker/tasks.hpp
@@ -19,10 +19,12 @@ struct Tasks {
     using HostCommsQueue = QueueImpl<messages::HostCommsMessage>;
     // Message queue for system task
     using SystemQueue = QueueImpl<messages::SystemMessage>;
+    // Message queue for UI task
+    using UIQueue = QueueImpl<messages::UIMessage>;
     // Central aggregator
     using QueueAggregator =
         queue_aggregator::QueueAggregator<MotorDriverQueue, MotorQueue,
-                                          HostCommsQueue, SystemQueue>;
+                                          HostCommsQueue, SystemQueue, UIQueue>;
 
     // Addresses
     static constexpr size_t MotorDriverAddress =
@@ -33,6 +35,8 @@ struct Tasks {
         QueueAggregator::template get_queue_idx<HostCommsQueue>();
     static constexpr size_t SystemAddress =
         QueueAggregator::template get_queue_idx<SystemQueue>();
+    static constexpr size_t UIAddress =
+        QueueAggregator::template get_queue_idx<UIQueue>();
 };
 
 };  // namespace tasks
diff --git a/stm32-modules/include/flex-stacker/flex-stacker/ui_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/ui_task.hpp
new file mode 100644
index 000000000..f0539a6d1
--- /dev/null
+++ b/stm32-modules/include/flex-stacker/flex-stacker/ui_task.hpp
@@ -0,0 +1,228 @@
+#pragma once
+
+#include <algorithm>
+#include <array>
+
+#include "core/is31fl_driver.hpp"
+#include "errors.hpp"
+#include "hal/message_queue.hpp"
+#include "messages.hpp"
+#include "ot_utils/freertos/freertos_timer.hpp"
+#include "systemwide.h"
+#include "ui_policy.hpp"
+
+namespace ui_task {
+using namespace ot_utils::freertos_timer;
+using namespace ui_policy;
+
+template <typename Policy>
+concept UIPolicyIface = requires(Policy& p) {
+    // A function to set the heartbeat LED on or off
+    {p.set_heartbeat_led(true)};
+}
+&&is31fl::IS31FL_Policy<Policy>;
+
+// There are 3 channels per color
+using ChannelMapping = std::array<size_t, 3>;
+
+static constexpr ChannelMapping white_channels{3, 4, 5};
+static constexpr ChannelMapping red_channels{6, 9, 12};
+static constexpr ChannelMapping green_channels{7, 10, 13};
+static constexpr ChannelMapping blue_channels{8, 11, 14};
+
+static auto color_to_channels(StatusBarColor color) -> const ChannelMapping& {
+    switch (color) {
+        case StatusBarColor::White:
+            return white_channels;
+        case StatusBarColor::Red:
+            return red_channels;
+        case StatusBarColor::Green:
+            return green_channels;
+        case StatusBarColor::Blue:
+            return blue_channels;
+        default:
+            return white_channels;
+    }
+}
+
+typedef struct StatusBarState {
+    StatusBarID kind;
+    StatusBarColor color;
+    float power;
+} StatusBarState;
+
+StatusBarState led_bar_internal = {
+    .kind = StatusBarID::Internal,
+    .color = StatusBarColor::Blue,
+    .power = 0.5F,
+};
+
+StatusBarState led_bar_external = {
+    .kind = StatusBarID::External,
+    .color = StatusBarColor::Blue,
+    .power = 0.5F,
+};
+
+// The timer driving LED update frequency should run at this period
+static constexpr uint32_t UPDATE_PERIOD_MS = 1000;
+static constexpr uint8_t LED_DRIVER0_I2C_ADDRESS = 0x6C << 1;  // Internal
+static constexpr uint8_t LED_DRIVER1_I2C_ADDRESS = 0x6F << 1;  // External
+
+using Message = messages::UIMessage;
+
+template <template <class> class QueueImpl>
+requires MessageQueue<QueueImpl<Message>, Message>
+class UITask {
+  public:
+    using Queue = QueueImpl<Message>;
+    using Aggregator = typename tasks::Tasks<QueueImpl>::QueueAggregator;
+    using Queues = typename tasks::Tasks<QueueImpl>;
+
+    explicit UITask(Queue& q, Aggregator* aggregator, UIPolicy* policy)
+        : _message_queue(q),
+          _task_registry(aggregator),
+          _policy(policy),
+          _led_driver0(),
+          _led_driver1(),
+          _ui_timer(
+              "UI Timer", [ThisPtr = this] { ThisPtr->heartbeat_led(); },
+              UPDATE_PERIOD_MS) {
+        _ui_timer.start();
+    }
+    UITask(const UITask& other) = delete;
+    auto operator=(const UITask& other) -> UITask& = delete;
+    UITask(UITask&& other) noexcept = delete;
+    auto operator=(UITask&& other) noexcept -> UITask& = delete;
+    ~UITask() = default;
+
+    auto provide_aggregator(Aggregator* aggregator) {
+        _task_registry = aggregator;
+    }
+
+    template <UIPolicyIface Policy>
+    auto run_once(Policy& policy) -> void {
+        auto message = Message(std::monostate());
+
+        if (!_initialized) {
+            _policy = &policy;
+
+            if (!_led_driver0.initialized()) {
+                _led_driver0.initialize(policy);
+                StatusBarState bar_state = get_statusbar_state(Internal);
+                set_status_bar(Internal, bar_state.color, bar_state.power);
+            }
+            if (!_led_driver1.initialized()) {
+                _led_driver1.initialize(policy);
+                StatusBarState bar_state = get_statusbar_state(External);
+                set_status_bar(External, bar_state.color, bar_state.power);
+            }
+            _initialized = true;
+        }
+
+        _message_queue.recv(&message);
+        auto visit_helper = [this, &policy](auto& message) -> void {
+            this->visit_message(message, policy);
+        };
+        std::visit(visit_helper, message);
+    }
+
+  private:
+    template <UIPolicyIface Policy>
+    auto visit_message(const std::monostate& m, Policy& policy) -> void {
+        static_cast<void>(m);
+        static_cast<void>(policy);
+    }
+
+    template <UIPolicyIface Policy>
+    auto visit_message(const messages::SetStatusBarColorMessage& m,
+                       Policy& policy) -> void {
+        static_cast<void>(policy);
+        auto response = messages::AcknowledgePrevious{.responding_to_id = m.id};
+
+        for (auto bar_id : {StatusBarID::Internal, StatusBarID::External}) {
+            if (m.bar_id.has_value()) bar_id = m.bar_id.value();
+            StatusBarState bar = get_statusbar_state(bar_id);
+            StatusBarColor color =
+                (m.color.has_value()) ? m.color.value() : bar.color;
+            float power = (m.power.has_value()) ? m.power.value() : bar.power;
+            if (!set_status_bar(bar_id, color, power)) {
+                response.with_error =
+                    errors::ErrorCode::SYSTEM_SET_STATUSBAR_COLOR_ERROR;
+            }
+            // Only set one status bar if one was given.
+            if (m.bar_id.has_value()) break;
+        }
+
+        static_cast<void>(_task_registry->send_to_address(
+            response, Queues::HostCommsAddress));
+    }
+
+    /**
+     * @brief Set the power (separate from PWM) for a color. Each color has
+     * 3 channels, so this helper will set all of the channels.
+     */
+    auto set_color_power(StatusBarID bar, StatusBarColor color, float power)
+        -> bool {
+        const auto& channels = color_to_channels(color);
+
+        // clear the current leds
+        if (bar == Internal) _led_driver0.set_current(0);
+        if (bar == External) _led_driver1.set_current(0);
+
+        return std::ranges::all_of(
+            channels.cbegin(), channels.cend(), [bar, power, this](size_t c) {
+                if (bar == Internal) return _led_driver0.set_current(c, power);
+                if (bar == External) return _led_driver1.set_current(c, power);
+                return false;
+            });
+    }
+
+    // Callback function for the heartbeat led timer
+    auto heartbeat_led() {
+        hb_led_state = !hb_led_state;
+        _policy->set_heartbeat_led(hb_led_state);
+    }
+
+    // Helper to get the StatusBarState given the bar id
+    auto get_statusbar_state(StatusBarID bar) -> StatusBarState& {
+        switch (bar) {
+            case Internal:
+                return _led_bar_internal;
+            case External:
+                return _led_bar_external;
+            default:
+                return _led_bar_internal;
+        }
+    }
+
+    auto set_status_bar(StatusBarID bar, StatusBarColor color, float power)
+        -> bool {
+        power = std::clamp(power, 0.0F, 1.0F);
+        auto status_bar = &get_statusbar_state(bar);
+        status_bar->color = color;
+        status_bar->power = power;
+
+        set_color_power(bar, color, power);
+        if (bar == Internal) {
+            _led_driver0.set_pwm(power);
+            return _led_driver0.send_update(*_policy);
+        } else if (bar == External) {
+            _led_driver1.set_pwm(power);
+            return _led_driver1.send_update(*_policy);
+        }
+        return false;
+    }
+
+    Queue& _message_queue;
+    Aggregator* _task_registry;
+    UIPolicy* _policy;
+    is31fl::IS31FL<LED_DRIVER0_I2C_ADDRESS> _led_driver0;
+    is31fl::IS31FL<LED_DRIVER1_I2C_ADDRESS> _led_driver1;
+    FreeRTOSTimer _ui_timer;
+
+    StatusBarState _led_bar_internal = led_bar_internal;
+    StatusBarState _led_bar_external = led_bar_external;
+    bool _initialized = false;
+    bool hb_led_state = false;
+};
+};  // namespace ui_task
diff --git a/stm32-modules/include/flex-stacker/systemwide.h b/stm32-modules/include/flex-stacker/systemwide.h
index 7c1abeba6..7bcf0faad 100644
--- a/stm32-modules/include/flex-stacker/systemwide.h
+++ b/stm32-modules/include/flex-stacker/systemwide.h
@@ -7,6 +7,18 @@ typedef enum MotorID {
     MOTOR_L,
 } MotorID;
 
+typedef enum StatusBarID {
+    Internal = 0,
+    External,
+} StatusBarID;
+
+typedef enum StatusBarColor {
+    White = 0,
+    Red,
+    Green,
+    Blue,
+} StatusBarColor;
+
 enum MotorSelection { Z, X, L, ALL };
 
 /* size of array for setting serial number */