275 lines
8.3 KiB
C
275 lines
8.3 KiB
C
/*
|
|
* SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "driver/gpio.h"
|
|
#include "driver/uart.h"
|
|
#include "esp_log.h"
|
|
#include "stepper_motor.h"
|
|
|
|
static const char *TAG = "Direction Controller";
|
|
|
|
#define DIRECTION_TASK_STACK_SIZE (1024 * 4)
|
|
|
|
/* UART Configuration */
|
|
#define UART_PORT_NUM UART_NUM_0
|
|
#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
|
|
*/
|
|
static void print_help(void)
|
|
{
|
|
printf("\n========================================\n");
|
|
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");
|
|
printf("----------------------------------------\n");
|
|
printf("命令 (Commands):\n");
|
|
printf(" help - 显示帮助信息\n");
|
|
printf(" pos - 显示当前位置\n");
|
|
printf("========================================\n\n");
|
|
}
|
|
|
|
/**
|
|
* @brief Print current position
|
|
*/
|
|
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;
|
|
}
|
|
}
|
|
printf("当前位置: %s (%.1f°)\n", dir_name, s_current_angle);
|
|
}
|
|
|
|
/**
|
|
* @brief Direction control task - handles user input
|
|
*/
|
|
static void direction_control_task(void *arg)
|
|
{
|
|
ESP_LOGI(TAG, "Direction control task started");
|
|
|
|
print_help();
|
|
|
|
char input_buf[INPUT_BUF_SIZE];
|
|
int input_idx = 0;
|
|
uint8_t data;
|
|
|
|
printf("请输入方向 > ");
|
|
fflush(stdout);
|
|
|
|
while (1) {
|
|
/* Read one byte at a time */
|
|
int len = uart_read_bytes(UART_PORT_NUM, &data, 1, pdMS_TO_TICKS(100));
|
|
|
|
if (len > 0) {
|
|
/* Echo character */
|
|
uart_write_bytes(UART_PORT_NUM, (const char*)&data, 1);
|
|
|
|
/* Handle Enter key (CR or LF) */
|
|
if (data == '\r' || data == '\n') {
|
|
uart_write_bytes(UART_PORT_NUM, "\r\n", 2);
|
|
|
|
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 {
|
|
printf("未知方向: '%s'\n", cmd);
|
|
printf("输入 'help' 查看可用方向\n");
|
|
}
|
|
}
|
|
}
|
|
|
|
input_idx = 0;
|
|
printf("\n请输入方向 > ");
|
|
fflush(stdout);
|
|
}
|
|
/* Handle Backspace */
|
|
else if (data == '\b' || data == 0x7F) {
|
|
if (input_idx > 0) {
|
|
input_idx--;
|
|
uart_write_bytes(UART_PORT_NUM, " \b", 2);
|
|
}
|
|
}
|
|
/* Regular character */
|
|
else if (input_idx < INPUT_BUF_SIZE - 1) {
|
|
input_buf[input_idx++] = data;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief Initialize UART for user input
|
|
*/
|
|
static void uart_init(void)
|
|
{
|
|
uart_config_t uart_config = {
|
|
.baud_rate = 115200,
|
|
.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,
|
|
};
|
|
|
|
ESP_ERROR_CHECK(uart_driver_install(UART_PORT_NUM, UART_BUF_SIZE * 2, 0, 0, NULL, 0));
|
|
ESP_ERROR_CHECK(uart_param_config(UART_PORT_NUM, &uart_config));
|
|
}
|
|
|
|
void app_main(void)
|
|
{
|
|
ESP_LOGI(TAG, "========================================");
|
|
ESP_LOGI(TAG, " Direction Control System");
|
|
ESP_LOGI(TAG, " Default position: South (南)");
|
|
ESP_LOGI(TAG, "========================================");
|
|
|
|
/* Initialize peripherals */
|
|
stepper_motor_gpio_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°)");
|
|
|
|
/* Start direction control task */
|
|
xTaskCreate(direction_control_task, "direction_control_task",
|
|
DIRECTION_TASK_STACK_SIZE, NULL, 5, NULL);
|
|
}
|