Initial commit
This commit is contained in:
Cedarium Zhang
274
main/main.c
Normal file
274
main/main.c
Normal file
@@ -0,0 +1,274 @@
|
||||
/*
|
||||
* 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);
|
||||
}
|
||||
Reference in New Issue
Block a user