Compare commits
2 Commits
917f42d7ab
...
512c64ecff
| Author | SHA1 | Date | |
|---|---|---|---|
| 512c64ecff | |||
| e437cbf8d2 |
@@ -26,6 +26,7 @@ Run these from an ESP-IDF environment with `IDF_PATH` set:
|
|||||||
- Validate behavior on hardware after changes; include manual test steps and results in PR descriptions.
|
- Validate behavior on hardware after changes; include manual test steps and results in PR descriptions.
|
||||||
|
|
||||||
## Commit & Pull Request Guidelines
|
## Commit & Pull Request Guidelines
|
||||||
|
- Commit Messages must follow the [Conventional Commits](https://www.conventionalcommits.org/) specification.
|
||||||
- Git history shows short, imperative summaries without scopes (e.g., "Initial commit").
|
- Git history shows short, imperative summaries without scopes (e.g., "Initial commit").
|
||||||
- PRs should include: a short description, hardware/board tested, and any notable serial output or behavior changes. Attach photos/videos when motion behavior changes.
|
- PRs should include: a short description, hardware/board tested, and any notable serial output or behavior changes. Attach photos/videos when motion behavior changes.
|
||||||
|
|
||||||
|
|||||||
4
components/direction_controller/CMakeLists.txt
Normal file
4
components/direction_controller/CMakeLists.txt
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
idf_component_register(SRCS "direction_controller.c"
|
||||||
|
INCLUDE_DIRS "include"
|
||||||
|
REQUIRES "stepper_motor"
|
||||||
|
PRIV_REQUIRES "driver" "log")
|
||||||
262
components/direction_controller/direction_controller.c
Normal file
262
components/direction_controller/direction_controller.c
Normal file
@@ -0,0 +1,262 @@
|
|||||||
|
/*
|
||||||
|
* SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD
|
||||||
|
*
|
||||||
|
* SPDX-License-Identifier: Apache-2.0
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <ctype.h>
|
||||||
|
#include <string.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
}
|
||||||
126
components/direction_controller/include/direction_controller.h
Normal file
126
components/direction_controller/include/direction_controller.h
Normal file
@@ -0,0 +1,126 @@
|
|||||||
|
/*
|
||||||
|
* SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD
|
||||||
|
*
|
||||||
|
* SPDX-License-Identifier: Apache-2.0
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stddef.h>
|
||||||
|
#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
|
||||||
@@ -1,2 +1,4 @@
|
|||||||
idf_component_register(SRCS "main.c"
|
idf_component_register(SRCS "main.c"
|
||||||
INCLUDE_DIRS ".")
|
INCLUDE_DIRS "."
|
||||||
|
REQUIRES "direction_controller"
|
||||||
|
PRIV_REQUIRES "driver")
|
||||||
|
|||||||
190
main/main.c
190
main/main.c
@@ -4,14 +4,14 @@
|
|||||||
* SPDX-License-Identifier: Apache-2.0
|
* SPDX-License-Identifier: Apache-2.0
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include <ctype.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include "freertos/FreeRTOS.h"
|
#include "freertos/FreeRTOS.h"
|
||||||
#include "freertos/task.h"
|
#include "freertos/task.h"
|
||||||
#include "driver/gpio.h"
|
|
||||||
#include "driver/uart.h"
|
#include "driver/uart.h"
|
||||||
#include "esp_log.h"
|
#include "esp_log.h"
|
||||||
#include "stepper_motor.h"
|
#include "direction_controller.h"
|
||||||
|
|
||||||
static const char *TAG = "Direction Controller";
|
static const char *TAG = "Direction Controller";
|
||||||
|
|
||||||
@@ -22,103 +22,6 @@ static const char *TAG = "Direction Controller";
|
|||||||
#define UART_BUF_SIZE 256
|
#define UART_BUF_SIZE 256
|
||||||
#define INPUT_BUF_SIZE 64
|
#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
|
* @brief Print help message with available directions
|
||||||
@@ -129,14 +32,18 @@ static void print_help(void)
|
|||||||
printf(" 方向控制系统 (Direction Control)\n");
|
printf(" 方向控制系统 (Direction Control)\n");
|
||||||
printf("========================================\n");
|
printf("========================================\n");
|
||||||
printf("可用方向 (Available directions):\n");
|
printf("可用方向 (Available directions):\n");
|
||||||
printf(" 南/S - South (0°) [默认位置]\n");
|
size_t direction_count = direction_controller_get_direction_count();
|
||||||
printf(" 西南/SW - Southwest (45°)\n");
|
for (size_t i = 0; i < direction_count; i++) {
|
||||||
printf(" 西/W - West (90°)\n");
|
const direction_controller_direction_t *dir = direction_controller_get_direction(i);
|
||||||
printf(" 西北/NW - Northwest (135°)\n");
|
if (dir == NULL) {
|
||||||
printf(" 北/N - North (180°)\n");
|
continue;
|
||||||
printf(" 东北/NE - Northeast (-135°)\n");
|
}
|
||||||
printf(" 东/E - East (-90°)\n");
|
if (dir->angle == 0.0f) {
|
||||||
printf(" 东南/SE - Southeast (-45°)\n");
|
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("----------------------------------------\n");
|
||||||
printf("命令 (Commands):\n");
|
printf("命令 (Commands):\n");
|
||||||
printf(" help - 显示帮助信息\n");
|
printf(" help - 显示帮助信息\n");
|
||||||
@@ -149,14 +56,13 @@ static void print_help(void)
|
|||||||
*/
|
*/
|
||||||
static void print_current_position(void)
|
static void print_current_position(void)
|
||||||
{
|
{
|
||||||
const char *dir_name = "未知";
|
const direction_controller_direction_t *dir = direction_controller_get_current_direction();
|
||||||
for (int i = 0; i < NUM_DIRECTIONS; i++) {
|
float angle = direction_controller_get_current_angle();
|
||||||
if (directions[i].angle == s_current_angle) {
|
if (dir != NULL) {
|
||||||
dir_name = directions[i].name_cn;
|
printf("当前位置: %s (%.1f°)\n", dir->name_cn, angle);
|
||||||
break;
|
} 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) {
|
if (input_idx > 0) {
|
||||||
input_buf[input_idx] = '\0';
|
input_buf[input_idx] = '\0';
|
||||||
|
|
||||||
/* Trim leading/trailing whitespace */
|
/* Trim leading/trailing whitespace */
|
||||||
char *cmd = input_buf;
|
char *cmd = input_buf;
|
||||||
while (*cmd == ' ') cmd++;
|
while (*cmd != '\0' && isspace((unsigned char)*cmd)) {
|
||||||
char *end = cmd + strlen(cmd) - 1;
|
cmd++;
|
||||||
while (end > cmd && *end == ' ') *end-- = '\0';
|
}
|
||||||
|
char *end = cmd + strlen(cmd);
|
||||||
/* Process command */
|
while (end > cmd && isspace((unsigned char)*(end - 1))) {
|
||||||
if (strcmp(cmd, "help") == 0 || strcmp(cmd, "?") == 0) {
|
end--;
|
||||||
print_help();
|
}
|
||||||
} else if (strcmp(cmd, "pos") == 0) {
|
*end = '\0';
|
||||||
print_current_position();
|
|
||||||
} else {
|
if (*cmd != '\0') {
|
||||||
const direction_t *dir = find_direction(cmd);
|
const direction_controller_direction_t *dir = NULL;
|
||||||
if (dir != NULL) {
|
direction_controller_cmd_t cmd_type =
|
||||||
printf("目标方向: %s (%s, %.1f°)\n",
|
direction_controller_parse_command(cmd, &dir);
|
||||||
dir->name_cn, dir->name_en, dir->angle);
|
|
||||||
rotate_to_angle(dir->angle);
|
switch (cmd_type) {
|
||||||
} else {
|
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("未知方向: '%s'\n", cmd);
|
||||||
printf("输入 'help' 查看可用方向\n");
|
printf("输入 'help' 查看可用方向\n");
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -259,13 +181,9 @@ void app_main(void)
|
|||||||
ESP_LOGI(TAG, "========================================");
|
ESP_LOGI(TAG, "========================================");
|
||||||
|
|
||||||
/* Initialize peripherals */
|
/* Initialize peripherals */
|
||||||
stepper_motor_gpio_init();
|
direction_controller_init(NULL);
|
||||||
uart_init();
|
uart_init();
|
||||||
|
|
||||||
/* Set initial position to South (0°) */
|
|
||||||
s_current_angle = 0.0f;
|
|
||||||
stepper_motor_power_off();
|
|
||||||
|
|
||||||
ESP_LOGI(TAG, "Initial position: South (0°)");
|
ESP_LOGI(TAG, "Initial position: South (0°)");
|
||||||
|
|
||||||
/* Start direction control task */
|
/* Start direction control task */
|
||||||
|
|||||||
Reference in New Issue
Block a user