Robothon Task Board 2025 Updates

idf/taskboard/extra_ros_packages/robothon_taskboard_msgs/msg/SensorMeasurement.msg

@@ -3,6 +3,7 @@ uint8 SENSOR_MEASUREMENT_TYPE_BOOL = 1
 uint8 SENSOR_MEASUREMENT_TYPE_ANALOG = 2
 uint8 SENSOR_MEASUREMENT_TYPE_VECTOR3 = 3
 uint8 SENSOR_MEASUREMENT_TYPE_INTEGER = 4
+uint8 SENSOR_MEASUREMENT_TYPE_EMPTY = 5
 
 uint8 type
 

idf/taskboard/extra_ros_packages/robothon_taskboard_msgs/msg/TaskStatus.msg

@@ -5,4 +5,5 @@ bool waiting_precondition
 builtin_interfaces/Time elapsed_time
 TaskStep[] steps
 bool[] status
+float32[] score
 builtin_interfaces/Time[] finish_times

idf/taskboard/extra_ros_packages/robothon_taskboard_msgs/msg/TaskStep.msg

@@ -2,6 +2,7 @@ uint8 TASK_STEP_TYPE_UNKNOWN = 0
 uint8 TASK_STEP_TYPE_EQUAL = 1
 uint8 TASK_STEP_TYPE_EQUAL_RANDOM = 2
 uint8 TASK_STEP_TYPE_GREATER_EQUAL = 3
+uint8 TASK_STEP_TYPE_TRACE_SHAPE = 4
 
 string sensor_name
 uint8 type

idf/taskboard/main/Kconfig.projbuild

@@ -6,7 +6,7 @@ menu "M5STACK controller"
 
 choice CONTROLLER_HARDWARE
     prompt "M5STACK hardware"
-    default M5STACK_STICKC_PLUS2
+    default M5STACK_CORE2
     config M5STACK_STICKC_PLUS2
         bool "M5STACK_STICKC_PLUS2"
     config M5STACK_CORE2

idf/taskboard/main/hal/ClueScreenController.hpp

@@ -32,6 +32,18 @@ struct ClueScreenController
             const float& current_value,
             const float& target_value) = 0;
 
+     /*
+      * @brief Displays a path clue without feedback
+      *
+      * @param expected_path Vector of SensorMeasurement objects representing the expected path
+      * @param measured_path Vector of SensorMeasurement objects representing the measured path
+      */
+     virtual void print_task_clue_path(
+            const std::vector<SensorMeasurement>& expected_path,
+            const std::vector<SensorMeasurement>& measured_path) = 0;
+
+    virtual void print_task_clue_goal(const char* clue_text) = 0;
+
     /**
      * @brief Clears the screen of any displayed clues
      */

idf/taskboard/main/hal/HardwareLowLevelController.hpp

@@ -4,7 +4,10 @@
 
 #pragma once
 
+#include <sdkconfig.h>
+
 #include <hal/PbHubController.hpp>
+#include <hal/PaHubToPbHubController.hpp>
 
 /**
  * @struct HardwareLowLevelController
@@ -13,6 +16,13 @@
  */
 struct HardwareLowLevelController
 {
+
+#if CONFIG_M5STACK_CORE2
+    PaHubToPbHubController& pb_hub_controller_1;    ///< Reference to PbHub I/O expansion controller
+    PaHubToPbHubController& pb_hub_controller_2;    ///< Reference to PbHub I/O expansion controller
+#else
     PbHubController& pb_hub_controller;    ///< Reference to PbHub I/O expansion controller
-    m5::M5Unified& m5_unified;             ///< Reference to M5Stack core functionality controller
-};
+#endif
+
+    m5::M5Unified& m5_unified;                     ///< Reference to M5Stack core functionality controller
+};

idf/taskboard/main/hal/NonVolatileStorage.hpp

@@ -103,10 +103,11 @@ struct NonVolatileStorage
             return;
         }
 
-        // Write CSV line: timestamp, task name, is_human
-        fprintf(file, "%s,%lld,%d,%s,",
+        // Write CSV line: timestamp, protocol name, protocol time, protocol score, is_human, protocol id
+        fprintf(file, "%s,%lld,%f,%d,%s,",
                 task.name().c_str(),
-                task.elapsed_time(),
+                task.total_task_time(),
+                task.final_score(),
                 is_human ? 1 : 0,
                 task.unique_id().c_str());
 
@@ -130,8 +131,8 @@ struct NonVolatileStorage
         // Ensure data is written to file
         fflush(file);
 
-        ESP_LOGI(TAG, "Added new register: %s, %lld, %d",
-                task.name().c_str(), task.elapsed_time(), is_human ? 1 : 0);
+        ESP_LOGI(TAG, "Added new register: %s, %lld, %f, %d",
+                task.name().c_str(), task.total_task_time(), task.final_score(), is_human ? 1 : 0);
 
         fclose(file);
     }

idf/taskboard/main/hal/PaHubToPbHubController.hpp

@@ -0,0 +1,183 @@
+/**
+ * Robothon Task Board Firmware
+ */
+
+#pragma once
+
+#include <M5Unified.h>
+
+#include <hal/PbHubController.hpp>
+
+#include <esp_log.h>
+
+#include <cstdint>
+
+/**
+ * @struct PaHubController
+ * 
+ * @brief Controller for I2C communication with PaHub device
+ * 
+ */
+struct PaHubController
+{
+    const char* TAG = "PaHubController";    ///< Logging tag
+
+    static constexpr uint32_t I2C_FREQ = 400000;        ///< I2C bus frequency in Hz
+    static constexpr uint8_t PAHUB_I2C_ADDR = 0x70;   ///< Default I2C address for PaHub
+
+    /**
+     * @brief Constructs a new PaHubController object
+     *
+     * @details Initializes I2C communication with default address
+     */
+    PaHubController() noexcept
+        : i2c_addr_(PAHUB_I2C_ADDR)
+    {
+        bool init = M5.Ex_I2C.begin();
+
+        if (init)
+        {
+            ESP_LOGI(TAG, "PaHubController::PaHubController() on address %d", i2c_addr_);
+        }
+        else
+        {
+            ESP_LOGE(TAG, "PaHubController::PaHubController() failed");
+        }
+    }
+
+    /**
+     * @brief Selects the TCA9548A I2C multiplexer channel
+     *
+     * @param bus Target bus number
+     *
+     * @return true if operation successful, false otherwise
+     */
+    bool TCA9548A_select(uint8_t bus)
+    {
+        bool status = true;
+
+        if (bus != last_channel_selected_)
+        {
+            status = status && M5.Ex_I2C.start(i2c_addr_, false, I2C_FREQ);
+            status = status && M5.Ex_I2C.write(1 << bus);
+            status = status && M5.Ex_I2C.stop();
+            if (status)
+            {
+                last_channel_selected_ = bus;
+                ESP_LOGI(TAG, "TCA9548A_select() bus %d selected", bus);
+            }
+        }
+
+        return status;
+    }
+
+private:
+
+    const uint8_t i2c_addr_ = 0;   ///< I2C address of PaHub device
+    uint8_t last_channel_selected_ = -1;   ///< Last selected channel
+
+};
+
+
+
+/**
+ * @struct PaHubToPbHubController
+ *
+ * @brief Controller for I2C communication with PbHub device through PaHub
+ *
+ */
+struct PaHubToPbHubController :
+    public PbHubController
+{
+    const char* TAG = "PaHubToPbHubController";    ///< Logging tag
+
+    static constexpr uint8_t PBHUB_I2C_ADDR = 0x61;   ///< Default I2C address for PbHub
+
+    /**
+     * @brief Constructs a new PaHubToPbHubController object
+     *
+     * @details Initializes I2C communication with default address
+     */
+    PaHubToPbHubController(PaHubController& pa_hub_controller,uint8_t pb_hub_channel) noexcept
+        :   i2c_addr_(PBHUB_I2C_ADDR),
+            pa_hub_controller_(pa_hub_controller),
+            pb_hub_channel_(pb_hub_channel)
+    {
+        bool init = M5.Ex_I2C.begin();
+
+        if (init)
+        {
+            ESP_LOGI(TAG, "PaHubToPbHubController::PaHubToPbHubController() on address %d", i2c_addr_);
+        }
+        else
+        {
+            ESP_LOGE(TAG, "PaHubToPbHubController::PaHubToPbHubController() failed");
+        }
+    }
+
+private:
+
+    /**
+     * @brief Performs I2C read operation
+     *
+     * @param channel Target channel
+     * @param operation Operation to perform
+     * @param data Buffer for read data
+     * @param length Number of bytes to read
+     *
+     * @return true if operation successful, false otherwise
+     */
+    bool read_operation(
+            const Channel channel,
+            const Operation operation,
+            uint8_t* data,
+            const size_t length) override
+    {
+        bool status = true;
+
+        status = status && pa_hub_controller_.TCA9548A_select(pb_hub_channel_);
+
+        status = status && PbHubController::read_operation(channel, operation, data, length);
+
+        if (!status)
+        {
+            ESP_LOGE(TAG, "PaHubToPbHubController::read_digital() failed");
+        }
+
+        return status;
+    }
+
+    /**
+     * @brief Performs I2C write operation
+     *
+     * @param channel Target channel
+     * @param operation Operation to perform
+     * @param data Data to write
+     * @param length Number of bytes to write
+     *
+     * @return true if operation successful, false otherwise
+     */
+    bool write_operation(
+            const Channel channel,
+            const Operation operation,
+            const uint8_t* data,
+            const size_t length) override
+    {
+        bool status = true;
+
+        status = status && pa_hub_controller_.TCA9548A_select(pb_hub_channel_);
+
+        status = status && PbHubController::write_operation(channel, operation, data, length);
+
+        if (!status)
+        {
+            ESP_LOGE(TAG, "PaHubToPbHubController::write_digital() failed");
+        }
+
+        return status;
+    }
+
+    const uint8_t i2c_addr_ = 0;        ///< I2C address of PbHub device
+    PaHubController& pa_hub_controller_;   ///< Reference to PaHub controller
+    const uint8_t pb_hub_channel_ = 0;   ///< Channel of PaHub where PbHub is located
+};

idf/taskboard/main/hal/PbHubController.hpp

@@ -160,7 +160,7 @@ struct PbHubController
         read_operation(channel, Operation::WRITE_IO1, reinterpret_cast<uint8_t*>(&data), sizeof(data));
     }
 
-private:
+protected:
 
     /**
      * @brief Performs I2C read operation
@@ -172,25 +172,25 @@ struct PbHubController
      *
      * @return true if operation successful, false otherwise
      */
-    bool read_operation(
+    virtual bool read_operation(
             const Channel channel,
             const Operation operation,
             uint8_t* data,
             const size_t length)
     {
         bool status = true;
-        status &= M5.Ex_I2C.start(i2c_addr_, false, I2C_FREQ);
-        status &= M5.Ex_I2C.write(static_cast<uint8_t>(channel) + static_cast<uint8_t>(operation));
-        status &= M5.Ex_I2C.stop();
+        status = status && M5.Ex_I2C.start(i2c_addr_, false, I2C_FREQ);
+        status = status && M5.Ex_I2C.write(static_cast<uint8_t>(channel) + static_cast<uint8_t>(operation));
+        status = status && M5.Ex_I2C.stop();
 
-        status &= M5.Ex_I2C.start(i2c_addr_, true, I2C_FREQ);
+        status = status && M5.Ex_I2C.start(i2c_addr_, true, I2C_FREQ);
 
         for (size_t i = 0; i < length; i++)
         {
-            status &= M5.Ex_I2C.read(&data[i], 1);
+            status = status && M5.Ex_I2C.read(&data[i], 1);
         }
 
-        status &= M5.Ex_I2C.stop();
+        status = status && M5.Ex_I2C.stop();
 
         if (!status)
         {
@@ -210,17 +210,17 @@ struct PbHubController
      *
      * @return true if operation successful, false otherwise
      */
-    bool write_operation(
+    virtual bool write_operation(
             const Channel channel,
             const Operation operation,
             const uint8_t* data,
             const size_t length)
     {
         bool status = true;
-        status &= M5.Ex_I2C.start(i2c_addr_, false, I2C_FREQ);
-        status &= M5.Ex_I2C.write(static_cast<uint8_t>(channel) + static_cast<uint8_t>(operation));
-        status &= M5.Ex_I2C.write(data, length);
-        status &= M5.Ex_I2C.stop();
+        status = status && M5.Ex_I2C.start(i2c_addr_, false, I2C_FREQ);
+        status = status && M5.Ex_I2C.write(static_cast<uint8_t>(channel) + static_cast<uint8_t>(operation));
+        status = status && M5.Ex_I2C.write(data, length);
+        status = status && M5.Ex_I2C.stop();
 
         if (!status)
         {
@@ -230,5 +230,7 @@ struct PbHubController
         return status;
     }
 
+private:
+
     const uint8_t i2c_addr_ = 0;   ///< I2C address of PbHub device
 };