From e437cbf8d2d8773798d6ef51e1dafe86221ff5ea Mon Sep 17 00:00:00 2001 From: Bairan Zhang Date: Fri, 30 Jan 2026 16:10:51 +0800 Subject: [PATCH] feat: add direction controller component --- .../direction_controller/CMakeLists.txt | 4 + .../direction_controller.c | 262 ++++++++++++++++++ .../include/direction_controller.h | 126 +++++++++ main/CMakeLists.txt | 4 +- main/main.c | 190 ++++--------- 5 files changed, 449 insertions(+), 137 deletions(-) create mode 100644 components/direction_controller/CMakeLists.txt create mode 100644 components/direction_controller/direction_controller.c create mode 100644 components/direction_controller/include/direction_controller.h diff --git a/components/direction_controller/CMakeLists.txt b/components/direction_controller/CMakeLists.txt new file mode 100644 index 0000000..6c1c0a3 --- /dev/null +++ b/components/direction_controller/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register(SRCS "direction_controller.c" + INCLUDE_DIRS "include" + REQUIRES "stepper_motor" + PRIV_REQUIRES "driver" "log") diff --git a/components/direction_controller/direction_controller.c b/components/direction_controller/direction_controller.c new file mode 100644 index 0000000..ccb422e --- /dev/null +++ b/components/direction_controller/direction_controller.c @@ -0,0 +1,262 @@ +/* + * SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include "esp_log.h" +#include "stepper_motor.h" +#include "direction_controller.h" + +static const char *TAG = "Direction Controller"; + +/* Default direction list (South = 0°, clockwise positive) */ +static const direction_controller_direction_t s_default_directions[] = { + {"南", "S", 0.0f}, + {"西南", "SW", 45.0f}, + {"西", "W", 90.0f}, + {"西北", "NW", 135.0f}, + {"北", "N", 180.0f}, + {"东北", "NE", -135.0f}, + {"东", "E", -90.0f}, + {"东南", "SE", -45.0f}, +}; + +static const direction_controller_direction_t *s_directions = s_default_directions; +static size_t s_direction_count = sizeof(s_default_directions) / sizeof(s_default_directions[0]); +static float s_current_angle = 0.0f; +static int s_rotate_speed_us = STEPPER_SPEED_FAST; +static bool s_initialized = false; + +static void trimmed_span(const char *input, const char **start_out, size_t *len_out) +{ + const char *start = input; + const char *end = NULL; + + if (input == NULL) { + *start_out = NULL; + *len_out = 0; + return; + } + + while (*start != '\0' && isspace((unsigned char)*start)) { + start++; + } + + end = start + strlen(start); + while (end > start && isspace((unsigned char)*(end - 1))) { + end--; + } + + *start_out = start; + *len_out = (size_t)(end - start); +} + +static bool equals_trimmed(const char *input, const char *token, bool case_insensitive) +{ + const char *start = NULL; + size_t len = 0; + size_t token_len = 0; + + if (input == NULL || token == NULL) { + return false; + } + + trimmed_span(input, &start, &len); + token_len = strlen(token); + + if (len == 0 || len != token_len) { + return false; + } + + for (size_t i = 0; i < len; i++) { + char a = start[i]; + char b = token[i]; + + if (case_insensitive) { + a = (char)tolower((unsigned char)a); + b = (char)tolower((unsigned char)b); + } + + if (a != b) { + return false; + } + } + + return true; +} + +static float calculate_shortest_rotation(float current, float target) +{ + float diff = target - current; + + while (diff > 180.0f) { + diff -= 360.0f; + } + while (diff < -180.0f) { + diff += 360.0f; + } + + return diff; +} + +void direction_controller_init(const direction_controller_config_t *config) +{ + const direction_controller_direction_t *directions = s_default_directions; + size_t direction_count = sizeof(s_default_directions) / sizeof(s_default_directions[0]); + float initial_angle = 0.0f; + int rotate_speed_us = STEPPER_SPEED_FAST; + bool init_stepper_gpio = true; + + if (config != NULL) { + if (config->directions != NULL && config->direction_count > 0) { + directions = config->directions; + direction_count = config->direction_count; + } + initial_angle = config->initial_angle; + if (config->rotate_speed_us > 0) { + rotate_speed_us = config->rotate_speed_us; + } + init_stepper_gpio = config->init_stepper_gpio; + } + + s_directions = directions; + s_direction_count = direction_count; + s_current_angle = initial_angle; + s_rotate_speed_us = rotate_speed_us; + + if (init_stepper_gpio) { + stepper_motor_gpio_init(); + stepper_motor_power_off(); + } + s_initialized = true; + + ESP_LOGI(TAG, "Direction controller initialized (angle=%.1f, speed=%dus)", + s_current_angle, s_rotate_speed_us); +} + +direction_controller_cmd_t direction_controller_parse_command(const char *input, + const direction_controller_direction_t **out_direction) +{ + if (out_direction != NULL) { + *out_direction = NULL; + } + + if (equals_trimmed(input, "help", true) || equals_trimmed(input, "?", false)) { + return DIRECTION_CONTROLLER_CMD_HELP; + } + + if (equals_trimmed(input, "pos", true)) { + return DIRECTION_CONTROLLER_CMD_POSITION; + } + + const direction_controller_direction_t *direction = direction_controller_find_direction(input); + if (direction != NULL) { + if (out_direction != NULL) { + *out_direction = direction; + } + return DIRECTION_CONTROLLER_CMD_DIRECTION; + } + + return DIRECTION_CONTROLLER_CMD_UNKNOWN; +} + +const direction_controller_direction_t *direction_controller_find_direction(const char *input) +{ + if (input == NULL) { + return NULL; + } + + for (size_t i = 0; i < s_direction_count; i++) { + if (equals_trimmed(input, s_directions[i].name_cn, false) || + equals_trimmed(input, s_directions[i].name_en, true)) { + return &s_directions[i]; + } + } + + return NULL; +} + +esp_err_t direction_controller_rotate_to_angle(float target_angle) +{ + if (!s_initialized) { + direction_controller_init(NULL); + } + + float rotation = calculate_shortest_rotation(s_current_angle, target_angle); + + if (rotation == 0.0f) { + ESP_LOGI(TAG, "Already at target position"); + return ESP_OK; + } + + ESP_LOGI(TAG, "Rotating from %.1f° to %.1f° (rotation: %.1f°)", + s_current_angle, target_angle, rotation); + + stepper_rotate_angle_with_accel(rotation, s_rotate_speed_us); + s_current_angle = target_angle; + stepper_motor_power_off(); + + ESP_LOGI(TAG, "Rotation complete, current position: %.1f°", s_current_angle); + return ESP_OK; +} + +esp_err_t direction_controller_rotate_to_direction(const char *input) +{ + const direction_controller_direction_t *direction = direction_controller_find_direction(input); + + if (direction == NULL) { + return ESP_ERR_NOT_FOUND; + } + + return direction_controller_rotate_to_angle(direction->angle); +} + +float direction_controller_get_current_angle(void) +{ + return s_current_angle; +} + +void direction_controller_set_current_angle(float angle) +{ + s_current_angle = angle; +} + +const direction_controller_direction_t *direction_controller_get_current_direction(void) +{ + for (size_t i = 0; i < s_direction_count; i++) { + if (s_directions[i].angle == s_current_angle) { + return &s_directions[i]; + } + } + + return NULL; +} + +size_t direction_controller_get_direction_count(void) +{ + return s_direction_count; +} + +const direction_controller_direction_t *direction_controller_get_direction(size_t index) +{ + if (index >= s_direction_count) { + return NULL; + } + + return &s_directions[index]; +} + +int direction_controller_get_rotate_speed_us(void) +{ + return s_rotate_speed_us; +} + +void direction_controller_set_rotate_speed_us(int rotate_speed_us) +{ + if (rotate_speed_us > 0) { + s_rotate_speed_us = rotate_speed_us; + } +} diff --git a/components/direction_controller/include/direction_controller.h b/components/direction_controller/include/direction_controller.h new file mode 100644 index 0000000..c036817 --- /dev/null +++ b/components/direction_controller/include/direction_controller.h @@ -0,0 +1,126 @@ +/* + * SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#pragma once + +#include +#include +#include "esp_err.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* ========== Direction Definitions ========== */ +/** + * @brief Direction definition entry + */ +typedef struct { + const char *name_cn; /* Chinese name */ + const char *name_en; /* English abbreviation */ + float angle; /* Angle relative to South */ +} direction_controller_direction_t; + +/** + * @brief Direction controller configuration + */ +typedef struct { + const direction_controller_direction_t *directions; /* Custom direction list, NULL to use default */ + size_t direction_count; /* Number of directions */ + float initial_angle; /* Initial angle relative to South */ + int rotate_speed_us; /* Rotation speed (microseconds per step) */ + bool init_stepper_gpio; /* Initialize stepper GPIO in init */ +} direction_controller_config_t; + +/** + * @brief Parsed command types + */ +typedef enum { + DIRECTION_CONTROLLER_CMD_UNKNOWN = 0, + DIRECTION_CONTROLLER_CMD_HELP, + DIRECTION_CONTROLLER_CMD_POSITION, + DIRECTION_CONTROLLER_CMD_DIRECTION +} direction_controller_cmd_t; + +/** + * @brief Initialize direction controller + * + * @param config Optional configuration, NULL uses defaults + */ +void direction_controller_init(const direction_controller_config_t *config); + +/** + * @brief Parse a command string + * + * @param input Input string (null-terminated) + * @param out_direction Output matched direction when command type is DIRECTION_CONTROLLER_CMD_DIRECTION + * @return Parsed command type + */ +direction_controller_cmd_t direction_controller_parse_command(const char *input, + const direction_controller_direction_t **out_direction); + +/** + * @brief Find direction by name + * + * @param input Direction name (Chinese or English abbreviation) + * @return Matched direction pointer, or NULL if not found + */ +const direction_controller_direction_t *direction_controller_find_direction(const char *input); + +/** + * @brief Rotate to specified angle + * + * @param target_angle Target angle relative to South + * @return ESP_OK on success + */ +esp_err_t direction_controller_rotate_to_angle(float target_angle); + +/** + * @brief Rotate to specified direction by name + * + * @param input Direction name (Chinese or English abbreviation) + * @return ESP_OK on success, ESP_ERR_NOT_FOUND if name is unknown + */ +esp_err_t direction_controller_rotate_to_direction(const char *input); + +/** + * @brief Get current angle + */ +float direction_controller_get_current_angle(void); + +/** + * @brief Set current angle (does not move motor) + */ +void direction_controller_set_current_angle(float angle); + +/** + * @brief Get current direction if it matches a defined direction + */ +const direction_controller_direction_t *direction_controller_get_current_direction(void); + +/** + * @brief Get direction count + */ +size_t direction_controller_get_direction_count(void); + +/** + * @brief Get direction by index + */ +const direction_controller_direction_t *direction_controller_get_direction(size_t index); + +/** + * @brief Get rotation speed (microseconds per step) + */ +int direction_controller_get_rotate_speed_us(void); + +/** + * @brief Set rotation speed (microseconds per step) + */ +void direction_controller_set_rotate_speed_us(int rotate_speed_us); + +#ifdef __cplusplus +} +#endif diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index cf2c455..3a0cdff 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1,2 +1,4 @@ idf_component_register(SRCS "main.c" - INCLUDE_DIRS ".") + INCLUDE_DIRS "." + REQUIRES "direction_controller" + PRIV_REQUIRES "driver") diff --git a/main/main.c b/main/main.c index 33a787e..ce70132 100644 --- a/main/main.c +++ b/main/main.c @@ -4,14 +4,14 @@ * SPDX-License-Identifier: Apache-2.0 */ +#include #include #include #include "freertos/FreeRTOS.h" #include "freertos/task.h" -#include "driver/gpio.h" #include "driver/uart.h" #include "esp_log.h" -#include "stepper_motor.h" +#include "direction_controller.h" static const char *TAG = "Direction Controller"; @@ -22,103 +22,6 @@ static const char *TAG = "Direction Controller"; #define UART_BUF_SIZE 256 #define INPUT_BUF_SIZE 64 -/* Current angle tracking (relative to South = 0°) */ -static float s_current_angle = 0.0f; - -/* Direction definitions (angles relative to South = 0°) */ -typedef struct { - const char *name_cn; /* Chinese name */ - const char *name_en; /* English abbreviation */ - float angle; /* Angle relative to South */ -} direction_t; - -/* - * Direction angle mapping (South = 0°, clockwise positive) - * - * 北 (180°) - * | - * 西北(135°) | 东北(-135°/225°) - * | - * 西(90°) ------+------ 东(-90°/270°) - * | - * 西南(45°) | 东南(-45°/315°) - * | - * 南 (0°) [HOME] - */ -static const direction_t directions[] = { - {"南", "S", 0.0f}, /* South - Home position */ - {"西南", "SW", 45.0f}, /* Southwest */ - {"西", "W", 90.0f}, /* West */ - {"西北", "NW", 135.0f}, /* Northwest */ - {"北", "N", 180.0f}, /* North */ - {"东北", "NE", -135.0f}, /* Northeast */ - {"东", "E", -90.0f}, /* East */ - {"东南", "SE", -45.0f}, /* Southeast */ -}; -#define NUM_DIRECTIONS (sizeof(directions) / sizeof(directions[0])) - -/** - * @brief Calculate shortest rotation angle to target - * - * @param current Current angle - * @param target Target angle - * @return Shortest rotation angle (positive = clockwise, negative = counter-clockwise) - */ -static float calculate_shortest_rotation(float current, float target) -{ - float diff = target - current; - - /* Normalize to -180 ~ 180 range */ - while (diff > 180.0f) { - diff -= 360.0f; - } - while (diff < -180.0f) { - diff += 360.0f; - } - - return diff; -} - -/** - * @brief Rotate to specified direction - * - * @param target_angle Target angle (relative to South) - */ -static void rotate_to_angle(float target_angle) -{ - float rotation = calculate_shortest_rotation(s_current_angle, target_angle); - - if (rotation == 0.0f) { - ESP_LOGI(TAG, "Already at target position"); - return; - } - - ESP_LOGI(TAG, "Rotating from %.1f° to %.1f° (rotation: %.1f°)", - s_current_angle, target_angle, rotation); - - stepper_rotate_angle_with_accel(rotation, STEPPER_SPEED_FAST); - s_current_angle = target_angle; - stepper_motor_power_off(); - - ESP_LOGI(TAG, "Rotation complete, current position: %.1f°", s_current_angle); -} - -/** - * @brief Find direction by name - * - * @param input User input string - * @return Pointer to direction_t if found, NULL otherwise - */ -static const direction_t* find_direction(const char *input) -{ - for (int i = 0; i < NUM_DIRECTIONS; i++) { - if (strcmp(input, directions[i].name_cn) == 0 || - strcasecmp(input, directions[i].name_en) == 0) { - return &directions[i]; - } - } - return NULL; -} /** * @brief Print help message with available directions @@ -129,14 +32,18 @@ static void print_help(void) printf(" 方向控制系统 (Direction Control)\n"); printf("========================================\n"); printf("可用方向 (Available directions):\n"); - printf(" 南/S - South (0°) [默认位置]\n"); - printf(" 西南/SW - Southwest (45°)\n"); - printf(" 西/W - West (90°)\n"); - printf(" 西北/NW - Northwest (135°)\n"); - printf(" 北/N - North (180°)\n"); - printf(" 东北/NE - Northeast (-135°)\n"); - printf(" 东/E - East (-90°)\n"); - printf(" 东南/SE - Southeast (-45°)\n"); + size_t direction_count = direction_controller_get_direction_count(); + for (size_t i = 0; i < direction_count; i++) { + const direction_controller_direction_t *dir = direction_controller_get_direction(i); + if (dir == NULL) { + continue; + } + if (dir->angle == 0.0f) { + printf(" %s/%s - %.1f° [默认位置]\n", dir->name_cn, dir->name_en, dir->angle); + } else { + printf(" %s/%s - %.1f°\n", dir->name_cn, dir->name_en, dir->angle); + } + } printf("----------------------------------------\n"); printf("命令 (Commands):\n"); printf(" help - 显示帮助信息\n"); @@ -149,14 +56,13 @@ static void print_help(void) */ static void print_current_position(void) { - const char *dir_name = "未知"; - for (int i = 0; i < NUM_DIRECTIONS; i++) { - if (directions[i].angle == s_current_angle) { - dir_name = directions[i].name_cn; - break; - } + const direction_controller_direction_t *dir = direction_controller_get_current_direction(); + float angle = direction_controller_get_current_angle(); + if (dir != NULL) { + printf("当前位置: %s (%.1f°)\n", dir->name_cn, angle); + } else { + printf("当前位置: 未知 (%.1f°)\n", angle); } - printf("当前位置: %s (%.1f°)\n", dir_name, s_current_angle); } /** @@ -189,27 +95,43 @@ static void direction_control_task(void *arg) if (input_idx > 0) { input_buf[input_idx] = '\0'; - + /* Trim leading/trailing whitespace */ char *cmd = input_buf; - while (*cmd == ' ') cmd++; - char *end = cmd + strlen(cmd) - 1; - while (end > cmd && *end == ' ') *end-- = '\0'; - - /* Process command */ - if (strcmp(cmd, "help") == 0 || strcmp(cmd, "?") == 0) { - print_help(); - } else if (strcmp(cmd, "pos") == 0) { - print_current_position(); - } else { - const direction_t *dir = find_direction(cmd); - if (dir != NULL) { - printf("目标方向: %s (%s, %.1f°)\n", - dir->name_cn, dir->name_en, dir->angle); - rotate_to_angle(dir->angle); - } else { + while (*cmd != '\0' && isspace((unsigned char)*cmd)) { + cmd++; + } + char *end = cmd + strlen(cmd); + while (end > cmd && isspace((unsigned char)*(end - 1))) { + end--; + } + *end = '\0'; + + if (*cmd != '\0') { + const direction_controller_direction_t *dir = NULL; + direction_controller_cmd_t cmd_type = + direction_controller_parse_command(cmd, &dir); + + switch (cmd_type) { + case DIRECTION_CONTROLLER_CMD_HELP: + print_help(); + break; + case DIRECTION_CONTROLLER_CMD_POSITION: + print_current_position(); + break; + case DIRECTION_CONTROLLER_CMD_DIRECTION: + if (dir != NULL) { + printf("目标方向: %s (%s, %.1f°)\n", + dir->name_cn, dir->name_en, dir->angle); + if (direction_controller_rotate_to_angle(dir->angle) != ESP_OK) { + printf("旋转失败\n"); + } + } + break; + default: printf("未知方向: '%s'\n", cmd); printf("输入 'help' 查看可用方向\n"); + break; } } } @@ -259,13 +181,9 @@ void app_main(void) ESP_LOGI(TAG, "========================================"); /* Initialize peripherals */ - stepper_motor_gpio_init(); + direction_controller_init(NULL); uart_init(); - /* Set initial position to South (0°) */ - s_current_angle = 0.0f; - stepper_motor_power_off(); - ESP_LOGI(TAG, "Initial position: South (0°)"); /* Start direction control task */