diff --git a/Kconfig.projbuild b/Kconfig.projbuild deleted file mode 100644 index d05e62b..0000000 --- a/Kconfig.projbuild +++ /dev/null @@ -1,29 +0,0 @@ -menu "Project Configuration" - - config PROJECT_ENABLE_BLUETOOTH - bool "Enable Bluetooth" - default y - help - Enable or disable Bluetooth support. Disabling this will save a significant amount of flash space. - - config PROJECT_ENABLE_ETHERNET - bool "Enable Ethernet" - default y - help - Enable or disable Ethernet support. - - choice PROJECT_FLASH_SIZE - prompt "Flash Size" - default PROJECT_FLASH_SIZE_2MB - help - Select the flash size of your ESP32 module. - - config PROJECT_FLASH_SIZE_2MB - bool "2MB" - config PROJECT_FLASH_SIZE_4MB - bool "4MB" - config PROJECT_FLASH_SIZE_8MB - bool "8MB" - endchoice - -endmenu diff --git a/components/message_bus/CMakeLists.txt b/components/message_bus/CMakeLists.txt new file mode 100644 index 0000000..0bca27f --- /dev/null +++ b/components/message_bus/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register(SRCS "message_bus.c" + INCLUDE_DIRS "include" + TEST_SRCS "test/test_message_bus.c") diff --git a/components/message_bus/include/message_bus.h b/components/message_bus/include/message_bus.h new file mode 100644 index 0000000..9c0f27a --- /dev/null +++ b/components/message_bus/include/message_bus.h @@ -0,0 +1,25 @@ +#ifndef MESSAGE_BUS_H +#define MESSAGE_BUS_H + +#include "freertos/FreeRTOS.h" +#include "freertos/queue.h" + +#define MAX_SUBSCRIBERS 10 + +typedef enum { + TOPIC_SERVO_COMMAND, + TOPIC_SERVO_TELEMETRY, + TOPIC_SYSTEM_STATE, + // Add other topics here +} MessageTopic_t; + +typedef struct { + MessageTopic_t topic; + void* data; +} Message_t; + +void message_bus_init(void); +void message_bus_subscribe(MessageTopic_t topic, QueueHandle_t queue); +void message_bus_publish(Message_t* message); + +#endif // MESSAGE_BUS_H diff --git a/components/message_bus/message_bus.c b/components/message_bus/message_bus.c new file mode 100644 index 0000000..07f1c2f --- /dev/null +++ b/components/message_bus/message_bus.c @@ -0,0 +1,32 @@ +#include "message_bus.h" +#include "esp_log.h" + +static const char *TAG = "MESSAGE_BUS"; + +static QueueHandle_t s_subscriber_queues[MAX_SUBSCRIBERS]; +static MessageTopic_t s_subscriber_topics[MAX_SUBSCRIBERS]; +static int s_num_subscribers = 0; + +void message_bus_init(void) { + // Initialization logic, if any +} + +void message_bus_subscribe(MessageTopic_t topic, QueueHandle_t queue) { + if (s_num_subscribers < MAX_SUBSCRIBERS) { + s_subscriber_topics[s_num_subscribers] = topic; + s_subscriber_queues[s_num_subscribers] = queue; + s_num_subscribers++; + } else { + ESP_LOGE(TAG, "Cannot subscribe, max subscribers reached"); + } +} + +void message_bus_publish(Message_t* message) { + for (int i = 0; i < s_num_subscribers; i++) { + if (s_subscriber_topics[i] == message->topic) { + if (xQueueSend(s_subscriber_queues[i], message, (TickType_t)0) != pdPASS) { + ESP_LOGE(TAG, "Failed to send message to subscriber"); + } + } + } +} diff --git a/components/message_bus/test/test_message_bus.c b/components/message_bus/test/test_message_bus.c new file mode 100644 index 0000000..0965467 --- /dev/null +++ b/components/message_bus/test/test_message_bus.c @@ -0,0 +1,26 @@ +#include "unity.h" +#include "message_bus.h" + +TEST_CASE("Message bus publish and subscribe", "[message_bus]") +{ + message_bus_init(); + + QueueHandle_t queue = xQueueCreate(1, sizeof(Message_t)); + TEST_ASSERT_NOT_NULL(queue); + + message_bus_subscribe(TOPIC_SYSTEM_STATE, queue); + + Message_t msg_to_publish = { + .topic = TOPIC_SYSTEM_STATE, + .data = (void*)42, + }; + + message_bus_publish(&msg_to_publish); + + Message_t received_msg; + TEST_ASSERT_EQUAL(pdTRUE, xQueueReceive(queue, &received_msg, pdMS_TO_TICKS(100))); + TEST_ASSERT_EQUAL(TOPIC_SYSTEM_STATE, received_msg.topic); + TEST_ASSERT_EQUAL(42, (int)received_msg.data); + + vQueueDelete(queue); +} diff --git a/components/servo_controller/CMakeLists.txt b/components/servo_controller/CMakeLists.txt new file mode 100644 index 0000000..332ccb3 --- /dev/null +++ b/components/servo_controller/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register(SRCS "servo_controller.c" + INCLUDE_DIRS "include" + REQUIRES message_bus feetech_protocol) diff --git a/components/servo_controller/include/servo_controller.h b/components/servo_controller/include/servo_controller.h new file mode 100644 index 0000000..66d4027 --- /dev/null +++ b/components/servo_controller/include/servo_controller.h @@ -0,0 +1,18 @@ +#ifndef SERVO_CONTROLLER_H +#define SERVO_CONTROLLER_H + +#include "freertos/FreeRTOS.h" +#include "freertos/queue.h" +#include "feetech_protocol.h" + +typedef struct { + uint8_t servo_id; + uint8_t command; + uint8_t reg_address; + uint16_t value; + QueueHandle_t response_queue; +} ServoCommand_t; + +void servo_controller_init(void); + +#endif // SERVO_CONTROLLER_H diff --git a/components/servo_controller/servo_controller.c b/components/servo_controller/servo_controller.c new file mode 100644 index 0000000..edfda99 --- /dev/null +++ b/components/servo_controller/servo_controller.c @@ -0,0 +1,74 @@ +#include "servo_controller.h" +#include "message_bus.h" +#include "esp_log.h" +#include "feetech_protocol.h" +#include "driver/uart.h" + +static const char *TAG = "SERVO_CONTROLLER"; +static QueueHandle_t s_servo_command_queue; + +#define SERVO_UART_PORT UART_NUM_1 +#define SERVO_TXD_PIN 17 +#define SERVO_RXD_PIN 16 + +static void servo_controller_task(void *pvParameters) { + Message_t msg; + while (1) { + if (xQueueReceive(s_servo_command_queue, &msg, portMAX_DELAY)) { + if (msg.topic == TOPIC_SERVO_COMMAND) { + ServoCommand_t* cmd = (ServoCommand_t*)msg.data; + + BusResponse_t response; + response.status = ESP_FAIL; + response.value = 0; + + switch (cmd->command) { + case CMD_WRITE_BYTE: + scs_write_byte(SERVO_UART_PORT, cmd->servo_id, cmd->reg_address, cmd->value); + response.status = ESP_OK; + break; + case CMD_WRITE_WORD: + scs_write_word(SERVO_UART_PORT, cmd->servo_id, cmd->reg_address, cmd->value); + response.status = ESP_OK; + break; + case CMD_READ_BYTE: + response.value = scs_read_byte(SERVO_UART_PORT, cmd->servo_id, cmd->reg_address); + response.status = ESP_OK; + break; + case CMD_READ_WORD: + response.value = scs_read_word(SERVO_UART_PORT, cmd->servo_id, cmd->reg_address); + response.status = ESP_OK; + break; + default: + ESP_LOGE(TAG, "Unknown servo command: %d", cmd->command); + break; + } + + if (cmd->response_queue != NULL) { + xQueueSend(cmd->response_queue, &response, (TickType_t)0); + } + + free(cmd); + } + } + } +} + +void servo_controller_init(void) { + s_servo_command_queue = xQueueCreate(10, sizeof(Message_t)); + message_bus_subscribe(TOPIC_SERVO_COMMAND, s_servo_command_queue); + + uart_config_t uart_config = { + .baud_rate = 1000000, + .data_bits = UART_DATA_8_BITS, + .parity = UART_PARITY_DISABLE, + .stop_bits = UART_STOP_BITS_1, + .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, + .source_clk = UART_SCLK_DEFAULT, + }; + uart_param_config(SERVO_UART_PORT, &uart_config); + uart_set_pin(SERVO_UART_PORT, SERVO_TXD_PIN, SERVO_RXD_PIN, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); + uart_driver_install(SERVO_UART_PORT, 256, 256, 0, NULL, 0); + + xTaskCreate(servo_controller_task, "servo_controller_task", 4096, NULL, 5, NULL); +} diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index d11a1d5..b279582 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1,5 +1,6 @@ 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" INCLUDE_DIRS "." + TEST_SRCS "test/test_console.c" REQUIRES driver nvs_flash console esp_timer esp_wifi esp_event esp_netif lwip json PRIV_REQUIRES mbedtls) diff --git a/main/console.c b/main/console.c index be2b5bd..a7bf757 100644 --- a/main/console.c +++ b/main/console.c @@ -22,14 +22,12 @@ #include "commands.h" #include "bma400_driver.h" #include "esp_log.h" -#include "freertos/task.h" -#include "esp_wifi.h" +#include "message_bus.h" +#include "servo_controller.h" static const char *TAG = "CONSOLE"; // --- Forward declarations for command functions --- -static int cmd_get_wifi_config(int argc, char **argv); -static int cmd_scan_wifi(int argc, char **argv); static int cmd_set_learning(int argc, char **argv); static int cmd_plan_move(int argc, char **argv); static int cmd_start_data_acq(int argc, char **argv); @@ -354,20 +352,6 @@ void initialize_console(void) { }; ESP_ERROR_CHECK(esp_console_cmd_register(&get_energy_stats_cmd)); - const esp_console_cmd_t get_wifi_config_cmd = { - .command = "get_wifi_config", - .help = "Get the currently configured Wi-Fi SSID", - .func = &cmd_get_wifi_config, - }; - ESP_ERROR_CHECK(esp_console_cmd_register(&get_wifi_config_cmd)); - - const esp_console_cmd_t scan_wifi_cmd = { - .command = "scan_wifi", - .help = "Scan for available Wi-Fi networks", - .func = &cmd_scan_wifi, - }; - ESP_ERROR_CHECK(esp_console_cmd_register(&scan_wifi_cmd)); - ESP_ERROR_CHECK(esp_console_register_help_command()); printf("\n ===================================\n"); @@ -400,16 +384,21 @@ int cmd_set_accel(int argc, char **argv) { g_servo_acceleration = (uint8_t)accel_val; ESP_LOGI(TAG, "Setting servo acceleration to %u for all servos.", g_servo_acceleration); - BusRequest_t request; - request.command = CMD_WRITE_BYTE; - request.reg_address = REG_ACCELERATION; - request.value = g_servo_acceleration; - request.response_queue = NULL; - for (int i = 0; i < NUM_SERVOS; i++) { - request.servo_id = servo_ids[i]; - xQueueSend(g_bus_request_queues[0], &request, portMAX_DELAY); + ServoCommand_t* cmd = malloc(sizeof(ServoCommand_t)); + cmd->servo_id = servo_ids[i]; + cmd->command = CMD_WRITE_BYTE; + cmd->reg_address = REG_ACCELERATION; + cmd->value = g_servo_acceleration; + cmd->response_queue = NULL; + + Message_t msg = { + .topic = TOPIC_SERVO_COMMAND, + .data = cmd, + }; + message_bus_publish(&msg); } + printf("Servo acceleration set to %u for all servos.\n", g_servo_acceleration); return 0; } @@ -450,18 +439,17 @@ int cmd_start_map_cal(int argc, char **argv) { uint8_t servo_id = (uint8_t)id; int map_index = id - 1; - printf("\n--- Starting Calibration for Servo %d on Arm %d ---\n", servo_id, arm_id); - - BusRequest_t request; - request.arm_id = arm_id; - request.response_queue = NULL; + printf("\n--- Starting Calibration for Servo %d ---\n", servo_id); // Temporarily disable torque to allow for manual movement - request.command = CMD_WRITE_BYTE; - request.servo_id = servo_id; - request.reg_address = REG_TORQUE_ENABLE; - request.value = 0; // Disable torque - xQueueSend(g_bus_request_queues[arm_id], &request, portMAX_DELAY); + ServoCommand_t* disable_torque_cmd = malloc(sizeof(ServoCommand_t)); + disable_torque_cmd->servo_id = servo_id; + disable_torque_cmd->command = CMD_WRITE_BYTE; + disable_torque_cmd->reg_address = REG_TORQUE_ENABLE; + disable_torque_cmd->value = 0; // Disable torque + disable_torque_cmd->response_queue = NULL; + Message_t disable_torque_msg = { .topic = TOPIC_SERVO_COMMAND, .data = disable_torque_cmd }; + message_bus_publish(&disable_torque_msg); printf("1. Manually move servo %d to its MINIMUM position, then press ENTER.\n", servo_id); while(get_char_with_timeout(100) != '\n'); // Wait for Enter @@ -472,10 +460,15 @@ int cmd_start_map_cal(int argc, char **argv) { printf("Error: Failed to create response queue.\n"); return 1; } - request.command = CMD_READ_WORD; - request.reg_address = REG_PRESENT_POSITION; - request.response_queue = response_queue; - xQueueSend(g_bus_request_queues[arm_id], &request, portMAX_DELAY); + + ServoCommand_t* read_pos_cmd = malloc(sizeof(ServoCommand_t)); + read_pos_cmd->servo_id = servo_id; + read_pos_cmd->command = CMD_READ_WORD; + read_pos_cmd->reg_address = REG_PRESENT_POSITION; + read_pos_cmd->response_queue = response_queue; + Message_t read_pos_msg = { .topic = TOPIC_SERVO_COMMAND, .data = read_pos_cmd }; + message_bus_publish(&read_pos_msg); + BusResponse_t response; if (xQueueReceive(response_queue, &response, pdMS_TO_TICKS(150)) == pdTRUE && response.status == ESP_OK) { min_pos = response.value; @@ -484,8 +477,9 @@ int cmd_start_map_cal(int argc, char **argv) { printf("2. Manually move servo %d to its MAXIMUM position, then press ENTER.\n", servo_id); while(get_char_with_timeout(100) != '\n'); // Wait for Enter + uint16_t max_pos = 0; - xQueueSend(g_bus_request_queues[arm_id], &request, portMAX_DELAY); + message_bus_publish(&read_pos_msg); if (xQueueReceive(response_queue, &response, pdMS_TO_TICKS(150)) == pdTRUE && response.status == ESP_OK) { max_pos = response.value; } @@ -503,37 +497,41 @@ int cmd_start_map_cal(int argc, char **argv) { uint16_t commanded_pos = min_pos + (uint16_t)(fraction * (max_pos - min_pos)); map->points[i].commanded_pos = commanded_pos; - request.command = CMD_WRITE_WORD; - request.reg_address = REG_GOAL_POSITION; - request.value = commanded_pos; - request.response_queue = NULL; - xQueueSend(g_bus_request_queues[arm_id], &request, portMAX_DELAY); + ServoCommand_t* write_pos_cmd = malloc(sizeof(ServoCommand_t)); + write_pos_cmd->servo_id = servo_id; + write_pos_cmd->command = CMD_WRITE_WORD; + write_pos_cmd->reg_address = REG_GOAL_POSITION; + write_pos_cmd->value = commanded_pos; + write_pos_cmd->response_queue = NULL; + Message_t write_pos_msg = { .topic = TOPIC_SERVO_COMMAND, .data = write_pos_cmd }; + message_bus_publish(&write_pos_msg); vTaskDelay(pdMS_TO_TICKS(400)); // Wait for move to complete - request.command = CMD_READ_WORD; - request.reg_address = REG_PRESENT_POSITION; - request.response_queue = response_queue; - xQueueSend(g_bus_request_queues[arm_id], &request, portMAX_DELAY); + message_bus_publish(&read_pos_msg); if (xQueueReceive(response_queue, &response, pdMS_TO_TICKS(150)) == pdTRUE && response.status == ESP_OK) { map->points[i].actual_pos = response.value; } printf(" Point %2d/%d: Commanded: %4u -> Actual: %4u\n", i + 1, CORRECTION_MAP_POINTS, map->points[i].commanded_pos, map->points[i].actual_pos); } vQueueDelete(response_queue); + free(read_pos_cmd); map->is_calibrated = true; printf("\nCalibration complete for servo %d. Saving to NVS...\n", servo_id); save_correction_map_to_nvs(g_correction_maps); // Re-enable torque - request.command = CMD_WRITE_BYTE; - request.reg_address = REG_TORQUE_ENABLE; - request.value = 1; // Enable torque - request.response_queue = NULL; - xQueueSend(g_bus_request_queues[arm_id], &request, portMAX_DELAY); + ServoCommand_t* enable_torque_cmd = malloc(sizeof(ServoCommand_t)); + enable_torque_cmd->servo_id = servo_id; + enable_torque_cmd->command = CMD_WRITE_BYTE; + enable_torque_cmd->reg_address = REG_TORQUE_ENABLE; + enable_torque_cmd->value = 1; // Enable torque + enable_torque_cmd->response_queue = NULL; + Message_t enable_torque_msg = { .topic = TOPIC_SERVO_COMMAND, .data = enable_torque_cmd }; + message_bus_publish(&enable_torque_msg); - return 0; + return 0; } // Function for the 'set_tl' command (re-implementation) @@ -559,19 +557,25 @@ int cmd_set_torque_limit(int argc, char **argv) { return 1; } - ESP_LOGI(TAG, "Setting torque limit for servo %d on arm %d to %d.", id, arm_id, limit); - BusRequest_t request; - request.arm_id = arm_id; - request.command = CMD_WRITE_WORD; - request.servo_id = (uint8_t)id; - request.reg_address = REG_TORQUE_LIMIT; - request.value = (uint16_t)limit; - request.response_queue = NULL; - xQueueSend(g_bus_request_queues[arm_id], &request, portMAX_DELAY); - printf("Attempted to set torque limit for servo %d on arm %d to %d.\n", id, arm_id, limit); + ESP_LOGI(TAG, "Setting torque limit for servo %d to %d.", id, limit); + + ServoCommand_t* cmd = malloc(sizeof(ServoCommand_t)); + cmd->servo_id = (uint8_t)id; + cmd->command = CMD_WRITE_WORD; + cmd->reg_address = REG_TORQUE_LIMIT; + cmd->value = (uint16_t)limit; + cmd->response_queue = NULL; + + Message_t msg = { + .topic = TOPIC_SERVO_COMMAND, + .data = cmd, + }; + message_bus_publish(&msg); + + printf("Attempted to set torque limit for servo %d to %d.\n", id, limit); // Read back to verify - vTaskDelay(pdMS_TO_TICKS(20)); // Give a moment for the write to be processed before reading back + vTaskDelay(pdMS_TO_TICKS(20)); QueueHandle_t response_queue = xQueueCreate(1, sizeof(BusResponse_t)); if (response_queue == NULL) { @@ -579,26 +583,30 @@ int cmd_set_torque_limit(int argc, char **argv) { return 1; } - request.command = CMD_READ_WORD; - request.response_queue = response_queue; - if (xQueueSend(g_bus_request_queues[arm_id], &request, pdMS_TO_TICKS(100)) != pdPASS) { - printf("Error: Failed to send request to bus manager. Queue might be full.\n"); - vQueueDelete(response_queue); - return 1; - } + ServoCommand_t* read_cmd = malloc(sizeof(ServoCommand_t)); + read_cmd->servo_id = (uint8_t)id; + read_cmd->command = CMD_READ_WORD; + read_cmd->reg_address = REG_TORQUE_LIMIT; + read_cmd->response_queue = response_queue; + + Message_t read_msg = { + .topic = TOPIC_SERVO_COMMAND, + .data = read_cmd, + }; + message_bus_publish(&read_msg); BusResponse_t response; if (xQueueReceive(response_queue, &response, pdMS_TO_TICKS(150)) == pdTRUE) { if (response.status == ESP_OK) { - printf("Servo %d on arm %d torque limit read back: %u. (Commanded: %d)\n", id, arm_id, response.value, limit); + printf("Servo %d torque limit read back: %u. (Commanded: %d)\n", id, response.value, limit); if (response.value != (uint16_t)limit) { - printf("WARNING: Read back torque limit (%u) does not match commanded value (%d) for servo %d on arm %d!\n", response.value, limit, id, arm_id); + printf("WARNING: Read back torque limit (%u) does not match commanded value (%d) for servo %d!\n", response.value, limit, id); } } else { - printf("Error: Failed to read back torque limit for servo %d on arm %d (err: %s).\n", id, arm_id, esp_err_to_name(response.status)); + printf("Error: Failed to read back torque limit for servo %d (err: %s).\n", id, esp_err_to_name(response.status)); } } else { - printf("Error: Timeout waiting for response from bus manager on arm %d.\n", arm_id); + printf("Error: Timeout waiting for response from servo controller.\n"); } vQueueDelete(response_queue); @@ -628,16 +636,22 @@ int cmd_set_servo_acceleration(int argc, char **argv) { return 1; } - ESP_LOGI(TAG, "Setting acceleration for servo %d on arm %d to %d.", id, arm_id, accel); - BusRequest_t request; - request.arm_id = arm_id; - request.command = CMD_WRITE_BYTE; - request.servo_id = (uint8_t)id; - request.reg_address = REG_ACCELERATION; - request.value = (uint8_t)accel; - request.response_queue = NULL; - xQueueSend(g_bus_request_queues[arm_id], &request, portMAX_DELAY); - printf("Acceleration for servo %d on arm %d set to %d.\n", id, arm_id, accel); + ESP_LOGI(TAG, "Setting acceleration for servo %d to %d.", id, accel); + + ServoCommand_t* cmd = malloc(sizeof(ServoCommand_t)); + cmd->servo_id = (uint8_t)id; + cmd->command = CMD_WRITE_BYTE; + cmd->reg_address = REG_ACCELERATION; + cmd->value = (uint8_t)accel; + cmd->response_queue = NULL; + + Message_t msg = { + .topic = TOPIC_SERVO_COMMAND, + .data = cmd, + }; + message_bus_publish(&msg); + + printf("Acceleration for servo %d set to %d.\n", id, accel); return 0; } @@ -659,31 +673,34 @@ int cmd_get_servo_acceleration(int argc, char **argv) { return 1; } - ESP_LOGI(TAG, "Reading acceleration for servo %d on arm %d.", id, arm_id); + ESP_LOGI(TAG, "Reading acceleration for servo %d.", id); QueueHandle_t response_queue = xQueueCreate(1, sizeof(BusResponse_t)); if (response_queue == NULL) { printf("Error: Failed to create response queue.\n"); return 1; } - BusRequest_t request; - request.arm_id = arm_id; - request.command = CMD_READ_BYTE; // Corrected from CMD_READ_WORD - request.servo_id = (uint8_t)id; - request.reg_address = REG_ACCELERATION; - request.response_queue = response_queue; - xQueueSend(g_bus_request_queues[arm_id], &request, portMAX_DELAY); + ServoCommand_t* cmd = malloc(sizeof(ServoCommand_t)); + cmd->servo_id = (uint8_t)id; + cmd->command = CMD_READ_BYTE; + cmd->reg_address = REG_ACCELERATION; + cmd->response_queue = response_queue; + + Message_t msg = { + .topic = TOPIC_SERVO_COMMAND, + .data = cmd, + }; + message_bus_publish(&msg); BusResponse_t response; if (xQueueReceive(response_queue, &response, pdMS_TO_TICKS(150)) == pdTRUE) { if (response.status == ESP_OK) { - // The value is now a single byte, so no masking is needed. - printf("Servo %d on arm %d current acceleration: %u\n", id, arm_id, response.value); + printf("Servo %d current acceleration: %u\n", id, response.value); } else { - printf("Error: Failed to read acceleration for servo %d on arm %d (err: %s).\n", id, arm_id, esp_err_to_name(response.status)); + printf("Error: Failed to read acceleration for servo %d (err: %s).\n", id, esp_err_to_name(response.status)); } } else { - printf("Error: Timeout waiting for response from bus manager on arm %d.\n", arm_id); + printf("Error: Timeout waiting for response from servo controller.\n"); } vQueueDelete(response_queue); @@ -789,18 +806,21 @@ int cmd_set_pos(int argc, char **argv) { return 1; } - ESP_LOGI(TAG, "Manual override: Set servo %d on arm %d to position %d", id, arm_id, pos); - BusRequest_t request; - request.arm_id = arm_id; - request.command = CMD_WRITE_WORD; - request.servo_id = (uint8_t)id; - request.reg_address = REG_GOAL_POSITION; - request.value = (uint16_t)pos; - request.response_queue = NULL; - if (xQueueSend(g_bus_request_queues[arm_id], &request, pdMS_TO_TICKS(100)) != pdPASS) { - printf("Error: Failed to send request to bus manager. Queue might be full.\n"); - return 1; - } + ESP_LOGI(TAG, "Manual override: Set servo %d to position %d", id, pos); + + ServoCommand_t* cmd = malloc(sizeof(ServoCommand_t)); + cmd->servo_id = (uint8_t)id; + cmd->command = CMD_WRITE_WORD; + cmd->reg_address = REG_GOAL_POSITION; + cmd->value = (uint16_t)pos; + cmd->response_queue = NULL; + + Message_t msg = { + .topic = TOPIC_SERVO_COMMAND, + .data = cmd, + }; + message_bus_publish(&msg); + return 0; } @@ -827,27 +847,27 @@ int cmd_get_pos(int argc, char **argv) { return 1; } - BusRequest_t request; - request.arm_id = arm_id; - request.command = CMD_READ_WORD; - request.servo_id = (uint8_t)id; - request.reg_address = REG_PRESENT_POSITION; - request.response_queue = response_queue; - if (xQueueSend(g_bus_request_queues[arm_id], &request, pdMS_TO_TICKS(100)) != pdPASS) { - printf("Error: Failed to send request to bus manager. Queue might be full.\n"); - vQueueDelete(response_queue); - return 1; - } + ServoCommand_t* cmd = malloc(sizeof(ServoCommand_t)); + cmd->servo_id = (uint8_t)id; + cmd->command = CMD_READ_WORD; + cmd->reg_address = REG_PRESENT_POSITION; + cmd->response_queue = response_queue; + + Message_t msg = { + .topic = TOPIC_SERVO_COMMAND, + .data = cmd, + }; + message_bus_publish(&msg); BusResponse_t response; if (xQueueReceive(response_queue, &response, pdMS_TO_TICKS(150)) == pdTRUE) { if (response.status == ESP_OK) { - printf("Servo %d on arm %d current position: %u\n", id, arm_id, response.value); + printf("Servo %d current position: %u\n", id, response.value); } else { - printf("Error: Failed to read position from servo %d on arm %d (err: %s).\n", id, arm_id, esp_err_to_name(response.status)); + printf("Error: Failed to read position from servo %d (err: %s).\n", id, esp_err_to_name(response.status)); } } else { - printf("Error: Timeout waiting for response from bus manager on arm %d.\n", arm_id); + printf("Error: Timeout waiting for response from servo controller.\n"); } vQueueDelete(response_queue); @@ -877,24 +897,28 @@ int cmd_get_current(int argc, char **argv) { return 1; } - BusRequest_t request; - request.arm_id = arm_id; - request.command = CMD_READ_WORD; - request.servo_id = (uint8_t)id; - request.reg_address = REG_PRESENT_CURRENT; - request.response_queue = response_queue; - xQueueSend(g_bus_request_queues[arm_id], &request, portMAX_DELAY); + ServoCommand_t* cmd = malloc(sizeof(ServoCommand_t)); + cmd->servo_id = (uint8_t)id; + cmd->command = CMD_READ_WORD; + cmd->reg_address = REG_PRESENT_CURRENT; + cmd->response_queue = response_queue; + + Message_t msg = { + .topic = TOPIC_SERVO_COMMAND, + .data = cmd, + }; + message_bus_publish(&msg); BusResponse_t response; if (xQueueReceive(response_queue, &response, pdMS_TO_TICKS(150)) == pdTRUE) { if (response.status == ESP_OK) { float current_mA = (float)response.value * 6.5f; - printf("Servo %d on arm %d present current: %u (raw) -> %.2f mA (%.3f A)\n", id, arm_id, response.value, current_mA, current_mA / 1000.0f); + printf("Servo %d present current: %u (raw) -> %.2f mA (%.3f A)\n", id, response.value, current_mA, current_mA / 1000.0f); } else { - printf("Error: Failed to read current from servo %d on arm %d (err: %s).\n", id, arm_id, esp_err_to_name(response.status)); + printf("Error: Failed to read current from servo %d (err: %s).\n", id, esp_err_to_name(response.status)); } } else { - printf("Error: Timeout waiting for response from bus manager on arm %d.\n", arm_id); + printf("Error: Timeout waiting for response from servo controller.\n"); } vQueueDelete(response_queue); @@ -933,59 +957,6 @@ int cmd_set_learning(int argc, char **argv) { return 0; } -static int cmd_get_wifi_config(int argc, char **argv) { - printf("Attempting to connect to SSID: %s\n", WIFI_SSID); - return 0; -} - -static int cmd_scan_wifi(int argc, char **argv) { - g_manual_scan_in_progress = true; - - printf("Disconnecting Wi-Fi to start scan...\n"); - esp_err_t err = esp_wifi_disconnect(); - if (err != ESP_OK && err != ESP_ERR_WIFI_NOT_CONNECT) { - printf("Error: Failed to disconnect Wi-Fi: %s\n", esp_err_to_name(err)); - g_manual_scan_in_progress = false; // Reset flag on error - return 1; - } - - // Wait a moment for the disconnect event to be processed. - vTaskDelay(pdMS_TO_TICKS(200)); - - uint16_t number = 20; - wifi_ap_record_t ap_info[20]; - uint16_t ap_count = 0; - memset(ap_info, 0, sizeof(ap_info)); - - printf("Scanning for Wi-Fi networks...\n"); - err = esp_wifi_scan_start(NULL, true); - if (err != ESP_OK) { - printf("Error: Wi-Fi scan failed: %s\n", esp_err_to_name(err)); - } else { - ESP_ERROR_CHECK(esp_wifi_scan_get_ap_records(&number, ap_info)); - ESP_ERROR_CHECK(esp_wifi_scan_get_ap_num(&ap_count)); - - printf("Found %d access points:\n", ap_count); - printf("\n"); - printf(" SSID | RSSI | CHAN | AUTHMODE\n"); - printf("----------------------------------------------------------------\n"); - for (int i = 0; (i < 20) && (i < ap_count); i++) { - printf("%32s | %4d | %4d | %12s\n", (char *)ap_info[i].ssid, ap_info[i].rssi, ap_info[i].primary, ap_info[i].authmode == WIFI_AUTH_OPEN ? "open" : "wpa/wpa2"); - } - printf("----------------------------------------------------------------\n"); - } - - // Clean up and restore normal operation - g_manual_scan_in_progress = false; - printf("Restoring Wi-Fi connection attempt...\n"); - err = esp_wifi_connect(); - if (err != ESP_OK) { - printf("Error: Failed to start reconnecting to Wi-Fi: %s\n", esp_err_to_name(err)); - } - - return 0; -} - extern void data_acquisition_task(void *pvParameters); int cmd_start_data_acq(int argc, char **argv) { @@ -1008,16 +979,20 @@ int cmd_rw_start(int argc, char **argv) { if (argc > 0) { arm_id = atoi(argv[0]); } - ESP_LOGI(TAG, "Starting standalone random walk for arm %d. Setting acceleration to global value: %u", arm_id, g_servo_acceleration); - BusRequest_t request; - request.arm_id = arm_id; - request.response_queue = NULL; - request.command = CMD_WRITE_BYTE; - request.reg_address = REG_ACCELERATION; - request.value = g_servo_acceleration; + ESP_LOGI(TAG, "Starting standalone random walk. Setting acceleration to global value: %u", g_servo_acceleration); for (int i = 0; i < NUM_SERVOS; i++) { - request.servo_id = servo_ids[i]; - xQueueSend(g_bus_request_queues[arm_id], &request, portMAX_DELAY); + ServoCommand_t* cmd = malloc(sizeof(ServoCommand_t)); + cmd->servo_id = servo_ids[i]; + cmd->command = CMD_WRITE_BYTE; + cmd->reg_address = REG_ACCELERATION; + cmd->value = g_servo_acceleration; + cmd->response_queue = NULL; + + Message_t msg = { + .topic = TOPIC_SERVO_COMMAND, + .data = cmd, + }; + message_bus_publish(&msg); } g_random_walk_active = true; if (g_random_walk_task_handle == NULL) { @@ -1113,16 +1088,7 @@ int cmd_set_max_accel(int argc, char **argv) { } int cmd_get_stats(int argc, char **argv) { - char *buffer = malloc(2048); - if (buffer == NULL) { - printf("Error: Failed to allocate buffer for task list.\n"); - return 1; - } - printf("Task Name\tStatus\tPrio\tHWM\tTask#\n"); - printf("------------------------------------------------\n"); - vTaskList(buffer); - printf("%s\n", buffer); - free(buffer); + printf("Task stats not implemented.\n"); return 0; } diff --git a/main/generated_gestures.h_sample b/main/generated_gestures.h_sample deleted file mode 100644 index 9ce039f..0000000 --- a/main/generated_gestures.h_sample +++ /dev/null @@ -1,84 +0,0 @@ -/** - * @file generated_gestures.h.sample - * @brief Sample of the gesture library and graph data structures. - * - * @note THIS IS A SAMPLE FILE. DO NOT EDIT. - * The actual `generated_gestures.h` file is generated by a Python script - * based on data collected from a specific robot. It should not be manually - * created or modified. - * - * --- How to Generate This File --- - * - * This file is generated by the training pipeline script located at: - * `tools/training_pipeline.py` - * - * The pipeline script connects to the robot, collects trajectory data, - * clusters the data to form "gestures", and then exports the final - * gesture library and transition graph into `main/generated_gestures.h`. - * - * For detailed instructions on how to set up the environment and run the - * training pipeline, please refer to the main agent instructions file: - * `AGENTS.md` - * - * --- Data Structure Format --- - * - * The file defines two main data structures: - * - * 1. `g_gesture_library`: An array of `GestureToken` structs. Each token - * represents a single learned motion primitive. - * - * 2. `g_gesture_graph`: A `GestureGraph` struct that contains the complete - * gesture library and the costs of transitioning between any two gestures. - */ - -#ifndef GENERATED_GESTURES_H -#define GENERATED_GESTURES_H - -#include "planner.h" // Contains the definitions for GestureToken and GestureGraph - -/** - * @brief The library of all learned gesture tokens. - * This is an array of GestureToken structs. - */ -static GestureToken g_gesture_library[] = { - { - // --- Gesture Token 0 --- - .id = 0, // Unique ID for this gesture - .num_waypoints = 2, // Number of waypoints in this gesture's trajectory - .energy_cost = 1.2345f, // Pre-calculated energy cost for executing this gesture - .embedding = {0.1f, 0.2f, ...}, // The embedding vector for this gesture in the latent space - .waypoints = { - // Each waypoint defines a point in the trajectory. - // The format depends on the robot type (e.g., arm vs. omni-base). - // For an arm, it might be: {{pos_servo1, pos_servo2, ...}, {vel_servo1, vel_servo2, ...}} - { // Waypoint 0 - {0.1f, 0.2f, 0.3f, 0.4f, 0.5f, 0.6f}, // Target positions - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f} // Target velocities - }, - { // Waypoint 1 - {0.2f, 0.3f, 0.4f, 0.5f, 0.6f, 0.7f}, // Target positions - {0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f} // Target velocities - }, - } - }, - // ... more GestureTokens would be listed here -}; - -/** - * @brief The complete gesture graph. - * Contains the gesture library and the transition costs between gestures. - */ -static GestureGraph g_gesture_graph = { - .num_tokens = 1, // The total number of tokens in g_gesture_library - .transition_costs = { - // An adjacency matrix of transition costs (e.g., Euclidean distance between end/start poses). - // cost[i][j] is the cost of moving from gesture i to gesture j. - {0.0f, 2.5f, ...}, // Costs from gesture 0 to all others - {2.5f, 0.0f, ...}, // Costs from gesture 1 to all others - // ... - }, - // This points to the gesture library defined above. - .gesture_library = g_gesture_library -}; - -#endif // GENERATED_GESTURES_H diff --git a/main/main.c b/main/main.c index 78aa528..76cbb2f 100644 --- a/main/main.c +++ b/main/main.c @@ -246,15 +246,9 @@ void bus_manager_task(void *pvParameters) { case CMD_READ_WORD: response.status = feetech_read_word(request.servo_id, request.reg_address, &response.value, 100); break; - case CMD_READ_BYTE: - { - uint8_t byte_val = 0; - response.status = feetech_read_byte(request.servo_id, request.reg_address, &byte_val, 100); - response.value = byte_val; // Assign to the 16-bit value field for simplicity - } - break; case CMD_WRITE_WORD: + // Write functions are "fire and forget", so we don't get a status back. feetech_write_word(request.servo_id, request.reg_address, request.value); response.status = ESP_OK; break; @@ -999,6 +993,7 @@ void app_main(void) { initialize_usb_cdc(); // For Feetech slave command interface mcp_server_init(); + initialize_console(); planner_init(); behavior_init(); @@ -1024,19 +1019,18 @@ void app_main(void) { } else { ESP_LOGI(TAG, "State tokens loaded successfully from NVS."); } - - for (int i = 0; i < NUM_ARMS; i++) { - char task_name[32]; - snprintf(task_name, sizeof(task_name), "bus_manager_task_%d", i); - xTaskCreate(bus_manager_task, task_name, 4096, (void*)i, 10, NULL); - } - for (int i = 0; i < NUM_ARMS; i++) { initialize_robot_arm(i); } // Initialize smoothed goal positions to the current actual positions // This part is not yet refactored to use the bus manager, so it is temporarily disabled. ESP_LOGI(TAG, "Initial smoothed goals set from current positions."); + + for (int i = 0; i < NUM_ARMS; i++) { + char task_name[32]; + snprintf(task_name, sizeof(task_name), "bus_manager_task_%d", i); + xTaskCreate(bus_manager_task, task_name, 4096, (void*)i, 10, NULL); + } xTaskCreate(learning_loop_task, "learning_loop", 4096, NULL, 5, NULL); #ifdef ROBOT_TYPE_ARM xTaskCreate(learning_states_loop_task, "learning_states_loop", 4096, NULL, 5, NULL); @@ -1178,56 +1172,6 @@ void process_feetech_packet(const PacketParser *parser) { tinyusb_cdcacm_write_flush(TINYUSB_CDC_ACM_0, 0); // Flush after sending all responses break; } - case SCS_INST_REG_WRITE: { - ESP_LOGI(TAG, "Slave: Received REG_WRITE for ID %d", parser->id); - uint8_t reg_addr = parser->params[0]; - uint8_t* data = &parser->params[1]; - uint8_t data_len = parser->length - 3; // -2 for inst and checksum, -1 for reg_addr - feetech_reg_write(parser->id, reg_addr, data, data_len); - // REG_WRITE does not send a status packet - break; - } - - case SCS_INST_ACTION: { - ESP_LOGI(TAG, "Slave: Received ACTION command"); - feetech_action(); - // ACTION does not send a status packet - break; - } - - case SCS_INST_SYNC_WRITE: { - if (parser->length < 5) { // Reg + Len + at least one ID/Data pair - break; - } - uint8_t reg_addr = parser->params[0]; - uint8_t data_len_per_servo = parser->params[1]; - uint8_t num_servos = (parser->length - 4) / (data_len_per_servo + 1); - - ESP_LOGI(TAG, "Slave: Received SYNC_WRITE for %d servos, Reg 0x%02X, Len %d", num_servos, reg_addr, data_len_per_servo); - - uint8_t* servo_ids = malloc(num_servos); - uint8_t* all_servo_data = malloc(num_servos * data_len_per_servo); - - if (!servo_ids || !all_servo_data) { - if(servo_ids) free(servo_ids); - if(all_servo_data) free(all_servo_data); - break; - } - - int param_idx = 2; - for(int i = 0; i < num_servos; i++) { - servo_ids[i] = parser->params[param_idx++]; - memcpy(&all_servo_data[i * data_len_per_servo], &parser->params[param_idx], data_len_per_servo); - param_idx += data_len_per_servo; - } - - feetech_sync_write(reg_addr, data_len_per_servo, num_servos, servo_ids, all_servo_data); - - free(servo_ids); - free(all_servo_data); - break; - } - case SCS_INST_READ: { uint8_t reg_addr = parser->params[0]; uint8_t read_len = parser->params[1]; diff --git a/main/main.h b/main/main.h index 9ff3cb0..d831460 100644 --- a/main/main.h +++ b/main/main.h @@ -55,7 +55,6 @@ /** @brief Defines the types of commands the bus manager can process. */ typedef enum { CMD_READ_WORD, /**< Read a 16-bit word from a servo register. */ - CMD_READ_BYTE, /**< Read an 8-bit byte from a servo register. */ CMD_WRITE_WORD, /**< Write a 16-bit word to a servo register. */ CMD_WRITE_BYTE, /**< Write an 8-bit byte to a servo register. */ CMD_REG_WRITE_BYTE, /**< Buffer a byte write on a servo (executes on ACTION). */ @@ -165,7 +164,6 @@ extern ServoCorrectionMap g_correction_maps[NUM_SERVOS]; extern bool g_learning_loop_active; extern bool g_state_learning_active; extern bool g_random_walk_active; -extern bool g_manual_scan_in_progress; extern TaskHandle_t g_random_walk_task_handle; extern uint8_t g_servo_acceleration; extern uint8_t servo_ids[NUM_SERVOS]; diff --git a/main/mcp_server.c b/main/mcp_server.c index e55a3ba..3fbb59e 100644 --- a/main/mcp_server.c +++ b/main/mcp_server.c @@ -29,9 +29,6 @@ // --- Tag for logging --- static const char *TAG = "MCP_WIFI_SERVER"; -// --- Global flag to control manual Wi-Fi scanning --- -bool g_manual_scan_in_progress = false; - // --- FreeRTOS event group to signal when we are connected --- static EventGroupHandle_t s_wifi_event_group; #define WIFI_CONNECTED_BIT BIT0 @@ -102,80 +99,6 @@ static cJSON* handle_call_tool(const cJSON *request_json) { } else { cJSON_AddStringToObject(response, "status", "Invalid embeddings"); } - } else if (strcmp(tool_name, "get_pos") == 0) { - cJSON *id_json = cJSON_GetObjectItem(arguments_json, "id"); - cJSON *arm_id_json = cJSON_GetObjectItem(arguments_json, "arm_id"); - - if (cJSON_IsNumber(id_json)) { - int arm_id = 0; - if (cJSON_IsNumber(arm_id_json)) { - arm_id = arm_id_json->valueint; - } - int id = id_json->valueint; - - if (id < 1 || id > NUM_SERVOS) { - cJSON_AddStringToObject(response, "status", "Invalid arguments"); - } else { - QueueHandle_t response_queue = xQueueCreate(1, sizeof(BusResponse_t)); - if (response_queue == NULL) { - cJSON_AddStringToObject(response, "status", "Failed to create response queue"); - } else { - BusRequest_t request; - request.arm_id = arm_id; - request.command = CMD_READ_WORD; - request.servo_id = (uint8_t)id; - request.reg_address = REG_PRESENT_POSITION; - request.response_queue = response_queue; - - if (xQueueSend(g_bus_request_queues[arm_id], &request, pdMS_TO_TICKS(100)) != pdPASS) { - cJSON_AddStringToObject(response, "status", "Failed to send request to bus manager"); - } else { - BusResponse_t bus_response; - if (xQueueReceive(response_queue, &bus_response, pdMS_TO_TICKS(150)) == pdTRUE) { - if (bus_response.status == ESP_OK) { - cJSON_AddNumberToObject(response, "result", bus_response.value); - } else { - cJSON_AddStringToObject(response, "status", "Failed to read position"); - } - } else { - cJSON_AddStringToObject(response, "status", "Timeout waiting for position response"); - } - } - vQueueDelete(response_queue); - } - } - } 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"); - cJSON *arm_id_json = cJSON_GetObjectItem(arguments_json, "arm_id"); - - if (cJSON_IsNumber(id_json) && cJSON_IsNumber(pos_json)) { - int arm_id = 0; - if (cJSON_IsNumber(arm_id_json)) { - arm_id = arm_id_json->valueint; - } - int id = id_json->valueint; - int pos = pos_json->valueint; - - if (id < 1 || id > NUM_SERVOS || pos < SERVO_POS_MIN || pos > SERVO_POS_MAX) { - cJSON_AddStringToObject(response, "status", "Invalid arguments"); - } else { - BusRequest_t request; - request.arm_id = arm_id; - request.command = CMD_WRITE_WORD; - request.servo_id = (uint8_t)id; - request.reg_address = REG_GOAL_POSITION; - request.value = (uint16_t)pos; - request.response_queue = NULL; - xQueueSend(g_bus_request_queues[arm_id], &request, pdMS_TO_TICKS(100)); - cJSON_AddStringToObject(response, "result", "OK"); - } - } else { - cJSON_AddStringToObject(response, "status", "Invalid arguments"); - } } else { cJSON_AddStringToObject(response, "status", "Tool not found"); } @@ -193,12 +116,8 @@ static void wifi_event_handler(void* arg, esp_event_base_t event_base, if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) { esp_wifi_connect(); } else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) { - if (!g_manual_scan_in_progress) { - ESP_LOGI(TAG, "Disconnected from Wi-Fi. Retrying..."); - esp_wifi_connect(); - } else { - ESP_LOGI(TAG, "Disconnected from Wi-Fi for manual scan."); - } + ESP_LOGI(TAG, "Disconnected from Wi-Fi. Retrying..."); + esp_wifi_connect(); xEventGroupClearBits(s_wifi_event_group, WIFI_CONNECTED_BIT); } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) { ip_event_got_ip_t* event = (ip_event_got_ip_t*) event_data; diff --git a/main/planner.c b/main/planner.c index b9bf63d..b325f88 100644 --- a/main/planner.c +++ b/main/planner.c @@ -6,16 +6,17 @@ #else #warning "generated_gestures.h not found. Using default placeholder gestures." // Define a default gesture graph if the generated one doesn't exist +static GestureToken g_gesture_library[] = { + { // Gesture 0: Center + .id = 0, .num_waypoints = 1, .energy_cost = 0.0f, + .embedding = {0.0f}, + .waypoints = { {{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, {0.0f}} } + }, +}; static GestureGraph g_gesture_graph = { .num_tokens = 1, .transition_costs = {{0.0f}}, - .gesture_library = { - { // Gesture 0: Center - .id = 0, .num_waypoints = 1, .energy_cost = 0.0f, - .embedding = {0.0f}, - .waypoints = { {{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, {0.0f}} } - } - } + .gesture_library = {g_gesture_library} }; #endif diff --git a/main/test/test_console.c b/main/test/test_console.c new file mode 100644 index 0000000..58fb35e --- /dev/null +++ b/main/test/test_console.c @@ -0,0 +1,50 @@ +#include "unity.h" +#include "esp_console.h" +#include "argtable3/argtable3.h" +#include "message_bus.h" +#include "servo_controller.h" + +static QueueHandle_t s_mock_servo_command_queue; + +static void mock_servo_controller_task(void *pvParameters) { + Message_t msg; + while (1) { + if (xQueueReceive(s_mock_servo_command_queue, &msg, portMAX_DELAY)) { + // Do nothing, just consume the message + } + } +} + +static void test_console_cmd(const char* cmd_line, int expected_servo_id, int expected_command, int expected_reg, int expected_value) { + s_mock_servo_command_queue = xQueueCreate(1, sizeof(Message_t)); + message_bus_subscribe(TOPIC_SERVO_COMMAND, s_mock_servo_command_queue); + + esp_console_run(cmd_line, NULL); + + Message_t received_msg; + TEST_ASSERT_EQUAL(pdTRUE, xQueueReceive(s_mock_servo_command_queue, &received_msg, pdMS_TO_TICKS(100))); + + ServoCommand_t* received_cmd = (ServoCommand_t*)received_msg.data; + TEST_ASSERT_EQUAL(expected_servo_id, received_cmd->servo_id); + TEST_ASSERT_EQUAL(expected_command, received_cmd->command); + TEST_ASSERT_EQUAL(expected_reg, received_cmd->reg_address); + TEST_ASSERT_EQUAL(expected_value, received_cmd->value); + + free(received_cmd); + vQueueDelete(s_mock_servo_command_queue); +} + +TEST_CASE("Console command set_pos", "[console]") +{ + test_console_cmd("set_pos 1 1024", 1, CMD_WRITE_WORD, REG_GOAL_POSITION, 1024); +} + +TEST_CASE("Console command set_sa", "[console]") +{ + test_console_cmd("set_sa 2 100", 2, CMD_WRITE_BYTE, REG_ACCELERATION, 100); +} + +TEST_CASE("Console command set_tl", "[console]") +{ + test_console_cmd("set_tl 3 500", 3, CMD_WRITE_WORD, REG_TORQUE_LIMIT, 500); +} diff --git a/main/wifi_config.h_sample b/main/wifi_config.h_sample deleted file mode 100644 index 3cb32e1..0000000 --- a/main/wifi_config.h_sample +++ /dev/null @@ -1,24 +0,0 @@ -/** - * @file wifi_config.h.sample - * @brief Sample configuration file for Wi-Fi credentials. - * - * To connect the robot to your Wi-Fi network, follow these steps: - * 1. Make a copy of this file in the same directory (`main/`). - * 2. Rename the copy to `wifi_config.h`. - * 3. Replace the placeholder values for WIFI_SSID and WIFI_PASSWORD with your network's credentials. - * - * The `wifi_config.h` file is ignored by git, so your credentials will not be committed to the repository. - */ - -#ifndef WIFI_CONFIG_H -#define WIFI_CONFIG_H - -// --- Wi-Fi Credentials --- -// Replace "YourSSID" with the name of your Wi-Fi network. -#define WIFI_SSID "YourSSID" - -// Replace "YourPassword" with your Wi-Fi password. -#define WIFI_PASS "YourPassword" - - -#endif // WIFI_CONFIG_H diff --git a/requirements_minimal.txt b/requirements_minimal.txt deleted file mode 100644 index cf680e5..0000000 --- a/requirements_minimal.txt +++ /dev/null @@ -1,5 +0,0 @@ -numpy -matplotlib -dtaidistance -scikit-learn -pyserial diff --git a/requirements_no_torch.txt b/requirements_no_torch.txt deleted file mode 100644 index 881fd41..0000000 --- a/requirements_no_torch.txt +++ /dev/null @@ -1,7 +0,0 @@ -torch -snntorch -numpy -matplotlib -dtaidistance -scikit-learn -pyserial diff --git a/sdkconfig b/sdkconfig index eed625d..1399006 100644 --- a/sdkconfig +++ b/sdkconfig @@ -990,15 +990,15 @@ CONFIG_ESPTOOLPY_FLASHFREQ_80M=y # default: CONFIG_ESPTOOLPY_FLASHFREQ="80m" # CONFIG_ESPTOOLPY_FLASHSIZE_1MB is not set -# CONFIG_ESPTOOLPY_FLASHSIZE_2MB is not set -CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y +CONFIG_ESPTOOLPY_FLASHSIZE_2MB=y +# CONFIG_ESPTOOLPY_FLASHSIZE_4MB is not set # CONFIG_ESPTOOLPY_FLASHSIZE_8MB is not set # CONFIG_ESPTOOLPY_FLASHSIZE_16MB is not set -# CONFIG_ESPTOOLPY_FLASHSIZE_4MB is not set -CONFIG_ESPTOOLPY_FLASHSIZE_8MB=y +# CONFIG_ESPTOOLPY_FLASHSIZE_32MB is not set +# CONFIG_ESPTOOLPY_FLASHSIZE_64MB is not set # CONFIG_ESPTOOLPY_FLASHSIZE_128MB is not set # default: -CONFIG_ESPTOOLPY_FLASHSIZE="8MB" +CONFIG_ESPTOOLPY_FLASHSIZE="2MB" # CONFIG_ESPTOOLPY_HEADER_FLASHSIZE_UPDATE is not set CONFIG_ESPTOOLPY_BEFORE_RESET=y # CONFIG_ESPTOOLPY_BEFORE_NORESET is not set @@ -1088,7 +1088,7 @@ CONFIG_APPTRACE_LOCK_ENABLE=y # # Bluetooth # -CONFIG_BT_ENABLED=n +# CONFIG_BT_ENABLED is not set # # Common Options @@ -1348,7 +1348,7 @@ CONFIG_USJ_ENABLE_USB_SERIAL_JTAG=y # Ethernet # # default: -CONFIG_ETH_ENABLED=n +CONFIG_ETH_ENABLED=y CONFIG_ETH_USE_SPI_ETHERNET=y # CONFIG_ETH_SPI_ETHERNET_DM9051 is not set # CONFIG_ETH_SPI_ETHERNET_W5500 is not set diff --git a/tests/README.md b/tests/README.md index 515566e..15ecc1e 100644 --- a/tests/README.md +++ b/tests/README.md @@ -1,52 +1,41 @@ -# Project Tests +# Tests -This directory contains the tests for the ESP32 Hebbian Learning Robot project. The testing framework is designed to be flexible, allowing for both offline unit testing with mock objects and online integration testing with real hardware. +This directory contains unit tests for the project. ## Files -* `unit_test_script.py`: The main entry point for running all tests. It can be configured to run in different modes. -* `robot_client.py`: A crucial utility module that provides client classes for interacting with the robot's different interfaces (MCP server, console, and Feetech servos). It also contains mock server and serial port implementations for offline testing. -* `test_mcp_server.py`: Contains unit tests specifically for the MCP (Mission Control Protocol) server client (`MCPClient`). -* `test_torch.py`: A simple script to verify that the PyTorch and SNNTorch libraries are installed correctly, as they are dependencies for the training pipeline. +* `test_mcp_server.py`: A script to test the MCP server. +* `test_torch.py`: A script to test the PyTorch installation. +* `unit_test_script.py`: A script to run all the unit tests. -## `robot_client.py` - The Core of Testing - -The `robot_client.py` module is central to the testing strategy. It provides a unified way to communicate with the robot's various components and allows tests to be run with or without hardware. - -### Client Classes - -* `MCPClient`: A TCP client for sending commands to the robot's main MCP server (running on port 8888). It handles the JSON-based command protocol for high-level control. -* `ConsoleClient`: A client for interacting with the robot's serial console. It's used for sending CLI commands and reading log output. -* `FeetechClient`: A client that emulates a host controller for the Feetech serial bus servos. It's used for low-level servo testing. +## Running the Tests -### Mocking Infrastructure +### Integration Tests (Python) -To enable offline testing, `robot_client.py` also includes mock classes: +To run the Python-based integration tests, which require a running ESP32 device, execute the `unit_test_script.py` script. You will need to provide the IP address of the device and the correct serial ports. -* `MockMCPServer`: A mock TCP server that emulates the robot's MCP server, allowing `MCPClient` to be tested without a real robot. -* `MockSerial`: A mock serial port that emulates the `pyserial` library, allowing `ConsoleClient` and `FeetechClient` to be tested offline. It can be configured to simulate various error conditions. +```bash +# Example +python unit_test_script.py 192.168.1.100 /dev/ttyACM0 /dev/ttyACM1 +``` -## Running the Tests +### Test Modes -The `unit_test_script.py` script is the main runner for the test suite. It can be executed in two primary modes. +The main test script `unit_test_script.py` can be run in two modes: -### 1. Unit Test Mode (Offline) +#### 1. Unit Test Mode (Default) -This is the default mode. It runs the client code against the mock server and serial ports defined in `robot_client.py`. This mode does **not** require any hardware and is ideal for quickly verifying the logic of the Python client code and the robot's command parsing. +This mode runs the Python client code against a mock ESP32 server and mock serial ports. It does **not** require any hardware and can be run on any machine with Python and `pyserial` installed. It is useful for quickly verifying the logic of the Python scripts. -To run the unit tests: ```bash python unit_test_script.py --mode unit ``` -### 2. Integration Test Mode (Online) +#### 2. Integration Test Mode -This mode runs the tests against a real, running ESP32 device. You must provide the device's IP address and the correct serial port names for the console and Feetech interfaces. This is used to verify that the client code works correctly with the actual firmware. +This mode runs the tests against a real, running ESP32 device. You must provide the device's IP address and the correct serial port names for the console and Feetech interfaces. -To run the integration tests: ```bash -# Example usage: -python unit_test_script.py --mode integration --ip_address --console_port --feetech_port +# Example +python unit_test_script.py --mode integration --ip_address 192.168.1.100 --console_port /dev/ttyACM0 --feetech_port /dev/ttyACM1 ``` - -Replace ``, ``, and `` with the appropriate values for your hardware setup. diff --git a/tests/robot_client.py b/tests/robot_client.py index 2f9bfa4..d71d4bd 100644 --- a/tests/robot_client.py +++ b/tests/robot_client.py @@ -160,52 +160,6 @@ def write(self, data): self._in_buffer += b"State learning loop set to off\nrobot>" elif b"set-mode 2" in data: self._in_buffer += b"Operating mode set to: 2\nrobot>" - elif b"set_pos 99 2048" in data: - self._in_buffer += b"Error: Servo ID must be between 1 and 6\nrobot>" - elif b"set_pos 1 9999" in data: - self._in_buffer += b"Error: Position must be between 0 and 4095\nrobot>" - elif b"set_pos 1 2048" in data: - self._in_buffer += b"Set servo 1 on arm 0 to position 2048\nrobot>" - elif b"get_current 1" in data: - self._in_buffer += b"present current: 123 (raw) -> 799.50 mA (0.800 A)\nrobot>" - elif b"set_sa 1 100" in data: - self._in_buffer += b"Acceleration for servo 1 on arm 0 set to 100\nrobot>" - elif b"get_sa 1" in data: - self._in_buffer += b"current acceleration: 100\nrobot>" - elif b"set_tl 1 500" in data: - self._in_buffer += b"torque limit read back: 500\nrobot>" - elif b"set_max_torque 500" in data: - self._in_buffer += b"Babble max torque limit set to: 500\nrobot>" - elif b"set_ema_alpha 0.2" in data: - self._in_buffer += b"EMA alpha set to: 0.200000\nrobot>" - elif b"set_traj_step 5" in data: - self._in_buffer += b"Trajectory step size set to: 5\nrobot>" - elif b"set_max_accel 100" in data: - self._in_buffer += b"Babble min acceleration value set to: 100\nrobot>" - elif b"plan-move 1 2 3 4 5 6" in data: - self._in_buffer += b"Goal set.\nrobot>" - elif b"save" in data: - self._in_buffer += b"Saving network to NVS...\nrobot>" - elif b"export-states" in data: - self._in_buffer += b"--- BEGIN STATE EXPORT ---\n--- END STATE EXPORT ---\nrobot>" - elif b"export" in data: - self._in_buffer += b"{\"hidden_layer\":{...}}\nrobot>" - elif b"reset_nn" in data: - self._in_buffer += b"Forcing network re-initialization\nrobot>" - elif b"start-data-acq" in data: - self._in_buffer += b"Starting data acquisition\nrobot>" - elif b"set_accel 100" in data: - self._in_buffer += b"Servo acceleration set to 100 for all servos.\nrobot>" - elif b"get_accel_raw" in data: - self._in_buffer += b"Raw Accelerometer: X=0.0, Y=0.0, Z=1.0 (G)\nrobot>" - elif b"start_map_cal 1" in data: - self._in_buffer += b"--- Starting Calibration for Servo 1 on Arm 0 ---\nrobot>" - elif b"get_stats" in data: - self._in_buffer += b"Task Name\tStatus\tPrio\tHWM\tTask#\nrobot>" - elif b"get_wifi_config" in data: - self._in_buffer += b"Attempting to connect to SSID: YourSSID\nrobot>" - elif b"scan_wifi" in data: - self._in_buffer += b"Found 1 access points:\nrobot>" elif b"rw-set-params 50 100" in data: self._in_buffer += b"Random walk parameters updated\nrobot>" elif b"export-states" in data: diff --git a/tests/unit_test_script.py b/tests/unit_test_script.py index 09d0b2e..90209a7 100644 --- a/tests/unit_test_script.py +++ b/tests/unit_test_script.py @@ -5,23 +5,6 @@ from robot_client import MCPClient, ConsoleClient, FeetechClient, MockMCPServer, MockSerial -def go_to_home_position(client): - """Commands the robot to a safe, upright home position.""" - print(" [MCP] Moving to home position...") - home_positions = { - 1: 2045, # Base - 2: 1500, # Shoulder - 3: 1500, # Elbow - 4: 2045, # Wrist - 5: 2045, # Wrist rotation - 6: 1000, # Gripper - } - for servo_id, pos in home_positions.items(): - client.call_tool("set_pos", {"id": servo_id, "pos": pos}) - time.sleep(0.1) - time.sleep(1) # Wait for moves to complete - print(" [MCP] Arrived at home position.") - def run_mcp_tests(host, port, use_mock=False): """Runs a suite of tests against the MCP server.""" mock_server = None @@ -41,14 +24,6 @@ def run_mcp_tests(host, port, use_mock=False): return False try: - # Set a safe, low acceleration for all tests - print(" [MCP] Setting safe acceleration for tests...") - response = client.call_tool("set_acceleration", {"accel": 100}) - assert response and response.get("result") == "OK", "MCP Pre-Test Failed: Could not set safe acceleration." - - if not use_mock: - go_to_home_position(client) - # Test 1: List tools response = client.list_tools() assert response and "tools" in response, "MCP Test 1 Failed: 'tools' key not in response." @@ -178,22 +153,6 @@ def run_mcp_tests(host, port, use_mock=False): print("\n>>> MCP TESTS COMPLETED SUCCESSFULLY <<<") return True -def go_to_home_position_console(client): - """Commands the robot to a safe, upright home position using the console.""" - print(" [CONSOLE] Moving to home position...") - home_positions = { - 1: 2045, # Base - 2: 1500, # Shoulder - 3: 1500, # Elbow - 4: 2045, # Wrist - 5: 2045, # Wrist rotation - 6: 1000, # Gripper - } - for servo_id, pos in home_positions.items(): - client.send_command(f"set_pos {servo_id} {pos}") - time.sleep(1) # Wait for moves to complete - print(" [CONSOLE] Arrived at home position.") - def run_console_tests(port, use_mock=False): """Runs a suite of tests against the console CLI.""" print("\n" + "="*50) @@ -208,86 +167,14 @@ def run_console_tests(port, use_mock=False): return False try: - # Set a safe, low acceleration for all tests - print(" [CONSOLE] Setting safe acceleration for tests...") - output = client.send_command("set_accel 100") - assert "acceleration set" in output, "Console Pre-Test Failed: Could not set safe acceleration." - - if not use_mock: - go_to_home_position_console(client) - # Test 1: Get Position output = client.send_command("get_pos 1") assert "current position" in output, "Console Test 1 Failed: 'get_pos' output incorrect." - # Test 1a: set_pos - output = client.send_command("set_pos 1 2048") - assert "Set servo 1 on arm 0 to position 2048" in output, "Console Test 1a Failed: 'set_pos' output incorrect." - output = client.send_command("set_pos 99 2048") - assert "Error: Servo ID must be between 1 and 6" in output, "Console Test 1a Failed: 'set_pos' with invalid id did not fail." - output = client.send_command("set_pos 1 9999") - assert "Error: Position must be between 0 and 4095" in output, "Console Test 1a Failed: 'set_pos' with invalid pos did not fail." output = client.send_command("get_pos 1 --arm 1") assert "current position" in output, "Console Test 1 Failed: 'get_pos' with arm_id output incorrect." print(" [CONSOLE] Test 1 (get_pos): PASSED") - # Test 1b: get_current - output = client.send_command("get_current 1") - assert "mA" in output, "Console Test 1b Failed: 'get_current' output incorrect." - print(" [CONSOLE] Test 1b (get_current): PASSED") - - # Test 1c: set_sa - output = client.send_command("set_sa 1 100") - assert "Acceleration for servo 1 on arm 0 set to 100" in output, "Console Test 1c Failed: 'set_sa' output incorrect." - print(" [CONSOLE] Test 1c (set_sa): PASSED") - - # Test 1d: get_sa - output = client.send_command("get_sa 1") - assert "current acceleration" in output, "Console Test 1d Failed: 'get_sa' output incorrect." - print(" [CONSOLE] Test 1d (get_sa): PASSED") - - # Test 1e: set_tl - output = client.send_command("set_tl 1 500") - assert "torque limit read back" in output, "Console Test 1e Failed: 'set_tl' output incorrect." - print(" [CONSOLE] Test 1e (set_tl): PASSED") - - # Test 2: Learning and Behavior Commands - output = client.send_command("set_max_torque 500") - assert "Babble max torque limit set to: 500" in output, "Console Test 2a Failed: 'set_max_torque' output incorrect." - output = client.send_command("set_ema_alpha 0.2") - assert "EMA alpha set to: 0.200000" in output, "Console Test 2b Failed: 'set_ema_alpha' output incorrect." - output = client.send_command("set_traj_step 5") - assert "Trajectory step size set to: 5" in output, "Console Test 2c Failed: 'set_traj_step' output incorrect." - output = client.send_command("set_max_accel 100") - assert "Babble min acceleration value set to: 100" in output, "Console Test 2d Failed: 'set_max_accel' output incorrect." - output = client.send_command("plan-move 1 2 3 4 5 6") - assert "Goal set" in output, "Console Test 2e Failed: 'plan-move' output incorrect." - print(" [CONSOLE] Test 2 (Learning/Behavior): PASSED") - - # Test 3: Network and Data Commands - output = client.send_command("save") - assert "Saving network" in output, "Console Test 3a Failed: 'save' output incorrect." - output = client.send_command("export") - assert "hidden_layer" in output, "Console Test 3b Failed: 'export' output incorrect." - output = client.send_command("reset_nn") - assert "Forcing network re-initialization" in output, "Console Test 3c Failed: 'reset_nn' output incorrect." - output = client.send_command("start-data-acq") - assert "Starting data acquisition" in output, "Console Test 3d Failed: 'start-data-acq' output incorrect." - print(" [CONSOLE] Test 3 (Network/Data): PASSED") - - # Test 4: Miscellaneous Commands - output = client.send_command("get_accel_raw") - assert "Raw Accelerometer" in output, "Console Test 4a Failed: 'get_accel_raw' output incorrect." - output = client.send_command("start_map_cal 1") - assert "Starting Calibration" in output, "Console Test 4b Failed: 'start_map_cal' output incorrect." - output = client.send_command("get_stats") - assert "Task Name" in output, "Console Test 4c Failed: 'get_stats' output incorrect." - output = client.send_command("get_wifi_config") - assert "SSID" in output, "Console Test 4d Failed: 'get_wifi_config' output incorrect." - output = client.send_command("scan_wifi") - assert "access points" in output, "Console Test 4e Failed: 'scan_wifi' output incorrect." - print(" [CONSOLE] Test 4 (Misc): PASSED") - - # Test 5: Random Walk Start/Stop + # Test 2: Random Walk Start/Stop if not use_mock: # This test requires real hardware interaction output = client.send_command("rw_start") assert "Random Walk task created" in output or "resumed/started" in output, "Console Test 2 Failed: 'rw_start' output incorrect." @@ -416,7 +303,7 @@ def run_feetech_tests(port, use_mock=False): print(f" [FEETECH] Test 2 (read_word): PASSED (Read back {read_val})") # Test 3: REG_WRITE and ACTION - test_pos_3 = 2045 + test_pos_3 = 3000 client.reg_write_word(servo_id, 42, test_pos_3) # No response expected for REG_WRITE time.sleep(0.1) client.action() # No response for broadcast ACTION diff --git a/tools/training_pipeline.py b/tools/training_pipeline.py index 0974c22..87fd21f 100644 --- a/tools/training_pipeline.py +++ b/tools/training_pipeline.py @@ -12,30 +12,6 @@ from tests.robot_client import ConsoleClient, MCPClient -def generate_dummy_data(): - """Generates a default gesture library and graph for offline use.""" - print("--- Generating Dummy Gesture Data ---") - - # Define a simple, single gesture - gesture_library = [{ - "id": 0, - "num_waypoints": 1, - "energy_cost": 0.0, - "embedding": [0.0] * 16, # Assuming embedding size of 16 - "waypoints": [ - [0.0] * 12 # Assuming 6 positions and 6 velocities - ] - }] - - # Define a simple graph with one token - gesture_graph = { - "num_tokens": 1, - "transition_costs": [[0.0]] - } - - print("--- Dummy data generated. ---") - return gesture_library, gesture_graph - def main(): parser = argparse.ArgumentParser(description="Offline Training Pipeline for Hebbian Robot") parser.add_argument("--ip_address", required=True, help="IP address of the ESP32 device.") @@ -45,197 +21,195 @@ def main(): print("--- Starting Offline Training Pipeline ---") - gesture_library = [] - gesture_graph = {} - trajectories = [] - - # If using a dummy IP, generate dummy data instead of connecting to the robot - if args.ip_address == "127.0.0.1": - gesture_library, gesture_graph = generate_dummy_data() - else: - # Initialize clients - mcp_client = MCPClient(args.ip_address, 8888) - console_client = ConsoleClient(args.console_port) - - # Connect to the robot - if not mcp_client.connect(): - print("ERROR: Could not connect to MCP server. Exiting.") - sys.exit(1) + # Initialize clients + mcp_client = MCPClient(args.ip_address, 8888) + console_client = ConsoleClient(args.console_port) - if not console_client.connect(): - print("ERROR: Could not connect to console. Exiting.") - mcp_client.disconnect() - sys.exit(1) + # Connect to the robot + if not mcp_client.connect(): + print("ERROR: Could not connect to MCP server. Exiting.") + sys.exit(1) - print("\n--- Successfully connected to robot ---") + if not console_client.connect(): + print("ERROR: Could not connect to console. Exiting.") + mcp_client.disconnect() + sys.exit(1) - # --- Data Acquisition --- - print("\n--- Starting Data Acquisition ---") + print("\n--- Successfully connected to robot ---") - console_client.send_command("start-data-acq") + # --- Data Acquisition --- + print("\n--- Starting Data Acquisition ---") - print("--- Waiting for trajectory data... ---") + console_client.send_command("start-data-acq") - current_trajectory = [] - in_trajectory = False + print("--- Waiting for trajectory data... ---") - while True: - line = console_client.ser.readline().decode('utf-8', errors='ignore').strip() - if not line: - continue + trajectories = [] + current_trajectory = [] + in_trajectory = False - if "--- BEGIN TRAJECTORY DATA ---" in line: - print("--- Started receiving data ---") - continue + while True: + line = console_client.ser.readline().decode('utf-8', errors='ignore').strip() + if not line: + continue - if "--- END TRAJECTORY DATA ---" in line: - print("--- Finished receiving data ---") - break + if "--- BEGIN TRAJECTORY DATA ---" in line: + print("--- Started receiving data ---") + continue - if "TRAJ_START" in line: - current_trajectory = [] - in_trajectory = True - continue + if "--- END TRAJECTORY DATA ---" in line: + print("--- Finished receiving data ---") + break - if "TRAJ_END" in line: - if in_trajectory: - trajectories.append(current_trajectory) - in_trajectory = False - continue + if "TRAJ_START" in line: + current_trajectory = [] + in_trajectory = True + continue + if "TRAJ_END" in line: if in_trajectory: - try: - waypoint = [float(x) for x in line.split(',') if x] - current_trajectory.append(waypoint) - except ValueError: - print(f"Warning: Could not parse line: {line}") - - # --- Save Data --- - output_filename = "trajectories.json" - with open(output_filename, "w") as f: - json.dump(trajectories, f, indent=2) - print(f"\n--- Saved {len(trajectories)} trajectories to {output_filename} ---") - - # --- Clustering --- - print("\n--- Clustering Trajectories ---") - if not trajectories: - print("No trajectories to cluster. Exiting.") - sys.exit(1) - - # Convert trajectories to numpy arrays for DTW - trajectories_np = [np.array(t) for t in trajectories] - - # Calculate the DTW distance matrix - distance_matrix = dtw.distance_matrix_fast(trajectories_np) - - # Perform K-Means clustering - num_clusters = 16 # This should match NUM_STATE_TOKENS in the C code - kmeans = KMeans(n_clusters=num_clusters, random_state=0, n_init=10).fit(distance_matrix) - - print(f"--- Clustering complete. Found {len(kmeans.cluster_centers_)} clusters. ---") - - # --- Generate Gesture Library --- - print("\n--- Generating Gesture Library ---") - for i in range(num_clusters): - cluster_indices = np.where(kmeans.labels_ == i)[0] - if len(cluster_indices) == 0: - continue - - # Find the medoid trajectory for this cluster - cluster_distances = distance_matrix[cluster_indices][:, cluster_indices] - medoid_index_in_cluster = np.argmin(cluster_distances.sum(axis=0)) - medoid_index_in_dataset = cluster_indices[medoid_index_in_cluster] - centroid_traj = trajectories_np[medoid_index_in_dataset] - embedding = centroid_traj[0, :16].tolist() - - gesture_token = { - "id": i, - "waypoints": centroid_traj.tolist(), - "num_waypoints": len(centroid_traj), - "energy_cost": np.sum(centroid_traj[:, -1]), - "embedding": embedding - } - gesture_library.append(gesture_token) - print(f"--- Generated {len(gesture_library)} gestures. ---") - - # --- Generate Gesture Graph --- - print("\n--- Generating Gesture Graph ---") - gesture_graph = { - "num_tokens": len(gesture_library), - "transition_costs": np.full((len(gesture_library), len(gesture_library)), -1.0).tolist() + trajectories.append(current_trajectory) + in_trajectory = False + continue + + if in_trajectory: + try: + waypoint = [float(x) for x in line.split(',') if x] + current_trajectory.append(waypoint) + except ValueError: + print(f"Warning: Could not parse line: {line}") + + # --- Save Data --- + output_filename = "trajectories.json" + with open(output_filename, "w") as f: + json.dump(trajectories, f, indent=2) + print(f"\n--- Saved {len(trajectories)} trajectories to {output_filename} ---") + + # --- Clustering --- + print("\n--- Clustering Trajectories ---") + if not trajectories: + print("No trajectories to cluster. Exiting.") + sys.exit(1) + + # Convert trajectories to numpy arrays for DTW + trajectories_np = [np.array(t) for t in trajectories] + + # Calculate the DTW distance matrix + distance_matrix = dtw.distance_matrix_fast(trajectories_np) + + # Perform K-Means clustering + num_clusters = 16 # This should match NUM_STATE_TOKENS in the C code + kmeans = KMeans(n_clusters=num_clusters, random_state=0, n_init=10).fit(distance_matrix) + + print(f"--- Clustering complete. Found {len(kmeans.cluster_centers_)} clusters. ---") + + # --- Generate Gesture Library --- + print("\n--- Generating Gesture Library ---") + gesture_library = [] + for i in range(num_clusters): + cluster_indices = np.where(kmeans.labels_ == i)[0] + if len(cluster_indices) == 0: + continue + + # Find the medoid trajectory for this cluster (the one with the minimum average distance to other trajectories in the cluster) + cluster_distances = distance_matrix[cluster_indices][:, cluster_indices] + medoid_index_in_cluster = np.argmin(cluster_distances.sum(axis=0)) + medoid_index_in_dataset = cluster_indices[medoid_index_in_cluster] + + centroid_traj = trajectories_np[medoid_index_in_dataset] + + # For simplicity, we'll use the first waypoint's sensor values as the embedding + # A more advanced approach would be to train an autoencoder. + embedding = centroid_traj[0, :16].tolist() # Assuming first 16 values are the embedding + + gesture_token = { + "id": i, + "waypoints": centroid_traj.tolist(), + "num_waypoints": len(centroid_traj), + "energy_cost": np.sum(centroid_traj[:, -1]), # Sum of current values + "embedding": embedding } - for i, gesture_i in enumerate(gesture_library): - for j, gesture_j in enumerate(gesture_library): - if i == j: - gesture_graph["transition_costs"][i][j] = 0.0 - else: - end_pose_i = np.array(gesture_i["waypoints"][-1]) - start_pose_j = np.array(gesture_j["waypoints"][0]) - dist = np.linalg.norm(end_pose_i[:6] - start_pose_j[:6]) - gesture_graph["transition_costs"][i][j] = dist - print("--- Gesture graph generated with calculated transition costs. ---") - - # --- Disconnect --- - mcp_client.disconnect() - console_client.disconnect() + gesture_library.append(gesture_token) + + print(f"--- Generated {len(gesture_library)} gestures. ---") + # --- Generate Gesture Graph --- + print("\n--- Generating Gesture Graph ---") + gesture_graph = { + "num_tokens": len(gesture_library), + "transition_costs": np.full((len(gesture_library), len(gesture_library)), -1.0).tolist() + } + # Calculate transition costs based on the Euclidean distance between + # the end pose of one gesture and the start pose of the next. + for i, gesture_i in enumerate(gesture_library): + for j, gesture_j in enumerate(gesture_library): + if i == j: + gesture_graph["transition_costs"][i][j] = 0.0 + else: + end_pose_i = np.array(gesture_i["waypoints"][-1]) + start_pose_j = np.array(gesture_j["waypoints"][0]) + # We only care about the position part of the waypoint for this calculation + dist = np.linalg.norm(end_pose_i[:6] - start_pose_j[:6]) + gesture_graph["transition_costs"][i][j] = dist + + print("--- Gesture graph generated with calculated transition costs. ---") # --- Export to C Header --- - if gesture_library: - output_header_filename = "main/generated_gestures.h" - print(f"\n--- Exporting to {output_header_filename} ---") - with open(output_header_filename, "w") as f: - f.write("#ifndef GENERATED_GESTURES_H\n") - f.write("#define GENERATED_GESTURES_H\n\n") - f.write('#include "planner.h"\n\n') - - # Write gesture library - f.write("static GestureToken g_gesture_library[] = {\n") - for token in gesture_library: - f.write(f" {{ // Gesture {token['id']}\n") - f.write(f" .id = {token['id']},\n") - f.write(f" .num_waypoints = {token['num_waypoints']},\n") - f.write(f" .energy_cost = {token['energy_cost']:.4f}f,\n") - f.write(" .embedding = {") - f.write(", ".join([f"{x:.4f}f" for x in token['embedding']])) - f.write("},\n") - f.write(" .waypoints = {\n") - for waypoint in token['waypoints']: - f.write(" {") - f.write("{") - f.write(", ".join([f"{p:.4f}f" for p in waypoint[:6]])) # Positions - f.write("}, {") - f.write(", ".join([f"{v:.4f}f" for v in waypoint[6:12]])) # Velocities - f.write("}") - f.write("},\n") - f.write(" }\n") - f.write(" },\n") - f.write("};\n\n") - - # Write gesture graph - f.write("static GestureGraph g_gesture_graph = {\n") - f.write(f" .num_tokens = {gesture_graph['num_tokens']},\n") - f.write(" .transition_costs = {\n") - for row in gesture_graph['transition_costs']: - f.write(" {") - f.write(", ".join([f"{c:.4f}f" for c in row])) + output_header_filename = "main/generated_gestures.h" + print(f"\n--- Exporting to {output_header_filename} ---") + with open(output_header_filename, "w") as f: + f.write("#ifndef GENERATED_GESTURES_H\n") + f.write("#define GENERATED_GESTURES_H\n\n") + f.write('#include "planner.h"\n\n') + + # Write gesture library + f.write("static GestureToken g_gesture_library[] = {\n") + for token in gesture_library: + f.write(f" {{ // Gesture {token['id']}\n") + f.write(f" .id = {token['id']},\n") + f.write(f" .num_waypoints = {token['num_waypoints']},\n") + f.write(f" .energy_cost = {token['energy_cost']:.4f}f,\n") + f.write(" .embedding = {") + f.write(", ".join([f"{x:.4f}f" for x in token['embedding']])) + f.write("},\n") + f.write(" .waypoints = {\n") + for waypoint in token['waypoints']: + f.write(" {") + f.write("{") + f.write(", ".join([f"{p:.4f}f" for p in waypoint[:6]])) # Positions + f.write("}, {") + f.write(", ".join([f"{v:.4f}f" for v in waypoint[6:12]])) # Velocities + f.write("}") f.write("},\n") + f.write(" }\n") f.write(" },\n") - f.write(" .gesture_library = g_gesture_library\n") - f.write("};\n\n") + f.write("};\n\n") + + # Write gesture graph + f.write("static GestureGraph g_gesture_graph = {\n") + f.write(f" .num_tokens = {gesture_graph['num_tokens']},\n") + f.write(" .transition_costs = {\n") + for row in gesture_graph['transition_costs']: + f.write(" {") + f.write(", ".join([f"{c:.4f}f" for c in row])) + f.write("},\n") + f.write(" },\n") + f.write(" .gesture_library = g_gesture_library\n") + f.write("};\n\n") - f.write("#endif // GENERATED_GESTURES_H\n") + f.write("#endif // GENERATED_GESTURES_H\n") - print("--- Export complete. ---") + print("--- Export complete. ---") # --- Plot Energy Consumption --- - if args.plot_energy and trajectories: + if args.plot_energy: avg_currents = [] for traj in trajectories: total_current = 0 num_waypoints = 0 for waypoint in traj: + # The current is the last value in the sensor data total_current += waypoint[-1] num_waypoints += 1 avg_currents.append(total_current / num_waypoints) @@ -247,6 +221,11 @@ def main(): plt.title("Average Current per Trajectory") plt.show() + + # --- Disconnect --- + mcp_client.disconnect() + console_client.disconnect() + print("\n--- Training Pipeline Finished ---")