feat: Implement inter-ESP32 communication via ESP-NOW

main/CMakeLists.txt

@@ -1,4 +1,4 @@
-idf_component_register(SRCS "main.c" "feetech_protocol.c" "bma400_driver.c" "led_indicator.c" "nvs_storage.c" "mcp_server.c" "console.c" "config.c" "planner.c" "behavior.c" "omni_base.c" "synsense_driver.c"
+idf_component_register(SRCS "main.c" "feetech_protocol.c" "bma400_driver.c" "led_indicator.c" "nvs_storage.c" "mcp_server.c" "console.c" "config.c" "planner.c" "behavior.c" "omni_base.c" "synsense_driver.c" "inter_esp_comm.c"
                     INCLUDE_DIRS "."
 
                     REQUIRES driver nvs_flash console esp_timer esp_wifi esp_event esp_netif lwip json

main/inter_esp_comm.c

@@ -0,0 +1,89 @@
+#include "inter_esp_comm.h"
+#include "esp_now.h"
+#include "esp_wifi.h"
+#include "esp_log.h"
+#include "string.h"
+#include "main.h"
+#include "feetech_protocol.h"
+
+static const char *TAG = "INTER_ESP_COMM";
+extern QueueHandle_t g_bus_request_queues[NUM_ARMS];
+
+// MAC address of the peer ESP32
+static uint8_t peer_mac_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; // Broadcast address
+
+// Send callback
+static void esp_now_send_cb(const uint8_t *mac_addr, esp_now_send_status_t status)
+{
+    if (status == ESP_NOW_SEND_SUCCESS) {
+        ESP_LOGI(TAG, "Message sent successfully");
+    } else {
+        ESP_LOGE(TAG, "Failed to send message");
+    }
+}
+
+// Receive callback
+static void esp_now_recv_cb(const uint8_t *mac_addr, const uint8_t *data, int len)
+{
+    inter_esp_message_t received_msg;
+    if (len == sizeof(inter_esp_message_t)) {
+        memcpy(&received_msg, data, sizeof(inter_esp_message_t));
+        ESP_LOGI(TAG, "Received message: command=%d, value=%f", received_msg.command, received_msg.value);
+
+        // Handle the received message
+        if (received_msg.command == 1) { // Example command: set servo position
+            int arm_id = 0; // Assuming arm 0 for now
+            int servo_id = 1; // Assuming servo 1 for now
+            uint16_t position = (uint16_t)received_msg.value;
+
+            BusRequest_t request;
+            request.arm_id = arm_id;
+            request.command = CMD_WRITE_WORD;
+            request.servo_id = (uint8_t)servo_id;
+            request.reg_address = REG_GOAL_POSITION;
+            request.value = position;
+            request.response_queue = NULL;
+            xQueueSend(g_bus_request_queues[arm_id], &request, pdMS_TO_TICKS(100));
+        }
+    } else {
+        ESP_LOGW(TAG, "Received message with incorrect length");
+    }
+}
+
+esp_err_t inter_esp_comm_init(void)
+{
+    // Initialize Wi-Fi
+    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
+    ESP_ERROR_CHECK(esp_wifi_init(&cfg));
+    ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM));
+    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
+    ESP_ERROR_CHECK(esp_wifi_start());
+
+    // Initialize ESP-NOW
+    ESP_ERROR_CHECK(esp_now_init());
+    ESP_ERROR_CHECK(esp_now_register_send_cb(esp_now_send_cb));
+    ESP_ERROR_CHECK(esp_now_register_recv_cb(esp_now_recv_cb));
+
+    // Add peer
+    esp_now_peer_info_t *peer = malloc(sizeof(esp_now_peer_info_t));
+    if (peer == NULL) {
+        ESP_LOGE(TAG, "Failed to allocate memory for peer info");
+        return ESP_FAIL;
+    }
+    memset(peer, 0, sizeof(esp_now_peer_info_t));
+    peer->channel = 0;
+    peer->ifidx = ESP_IF_WIFI_STA;
+    peer->encrypt = false;
+    memcpy(peer->peer_addr, peer_mac_addr, ESP_NOW_ETH_ALEN);
+    ESP_ERROR_CHECK(esp_now_add_peer(peer));
+    free(peer);
+
+    ESP_LOGI(TAG, "ESP-NOW initialized successfully");
+
+    return ESP_OK;
+}
+
+esp_err_t inter_esp_comm_send_message(const inter_esp_message_t* msg)
+{
+    return esp_now_send(peer_mac_addr, (const uint8_t*) msg, sizeof(inter_esp_message_t));
+}

main/inter_esp_comm.h

@@ -0,0 +1,18 @@
+#ifndef INTER_ESP_COMM_H
+#define INTER_ESP_COMM_H
+
+#include "esp_err.h"
+
+// Define a structure for the messages to be exchanged
+typedef struct {
+    int command;
+    float value;
+} inter_esp_message_t;
+
+// Function to initialize ESP-NOW communication
+esp_err_t inter_esp_comm_init(void);
+
+// Function to send a message to the peer ESP32
+esp_err_t inter_esp_comm_send_message(const inter_esp_message_t* msg);
+
+#endif // INTER_ESP_COMM_H

main/main.c

@@ -31,6 +31,7 @@ const unsigned char dummy_synsense_config[] = {0xDE, 0xAD, 0xBE, 0xEF};
 #include "tinyusb.h"
 #include "tusb_cdc_acm.h"
 #include "mcp_server.h"
+#include "inter_esp_comm.h"
 #include "argtable3/argtable3.h"
 #include "commands.h"
 
@@ -998,6 +999,7 @@ void app_main(void) {
     synsense_load_configuration(dummy_synsense_config, sizeof(dummy_synsense_config));
     initialize_usb_cdc(); // For Feetech slave command interface
     mcp_server_init();
+    inter_esp_comm_init();
 
     planner_init();
     behavior_init();

main/mcp_server.c

@@ -20,6 +20,7 @@
 #include "commands.h"
 #include "planner.h"
 #include "behavior.h"
+#include "inter_esp_comm.h"
 
 // --- Wi-Fi & Server Configuration ---
 #define MCP_TCP_PORT   8888
@@ -147,6 +148,23 @@ static cJSON* handle_call_tool(const cJSON *request_json) {
         } else {
             cJSON_AddStringToObject(response, "status", "Invalid arguments");
         }
+    } else if (strcmp(tool_name, "send_test_message") == 0) {
+        cJSON *command_json = cJSON_GetObjectItem(arguments_json, "command");
+        cJSON *value_json = cJSON_GetObjectItem(arguments_json, "value");
+
+        if (cJSON_IsNumber(command_json) && cJSON_IsNumber(value_json)) {
+            inter_esp_message_t msg;
+            msg.command = command_json->valueint;
+            msg.value = (float)value_json->valuedouble;
+
+            if (inter_esp_comm_send_message(&msg) == ESP_OK) {
+                cJSON_AddStringToObject(response, "status", "OK");
+            } else {
+                cJSON_AddStringToObject(response, "status", "Failed to send message");
+            }
+        } else {
+            cJSON_AddStringToObject(response, "status", "Invalid arguments");
+        }
     } else if (strcmp(tool_name, "set_pos") == 0) {
         cJSON *id_json = cJSON_GetObjectItem(arguments_json, "id");
         cJSON *pos_json = cJSON_GetObjectItem(arguments_json, "pos");
@@ -524,6 +542,12 @@ static cJSON* handle_list_tools(void) {
     cJSON_AddStringToObject(execute_behavior_tool, "description", "Executes a sequence of goal embeddings.");
     cJSON_AddItemToArray(tools, execute_behavior_tool);
 
+    // Tool: send_test_message
+    cJSON *send_test_message_tool = cJSON_CreateObject();
+    cJSON_AddStringToObject(send_test_message_tool, "name", "send_test_message");
+    cJSON_AddStringToObject(send_test_message_tool, "description", "Sends a test message to the other ESP32.");
+    cJSON_AddItemToArray(tools, send_test_message_tool);
+
     return root;
 }