411 lines
11 KiB
C
411 lines
11 KiB
C
/*
|
|
* SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <ctype.h>
|
|
#include <string.h>
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "driver/gpio.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 direction_controller_calibration_t s_calibration = {0};
|
|
static bool s_calibration_configured = 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 normalize_angle(float angle)
|
|
{
|
|
while (angle > 180.0f) {
|
|
angle -= 360.0f;
|
|
}
|
|
while (angle <= -180.0f) {
|
|
angle += 360.0f;
|
|
}
|
|
return angle;
|
|
}
|
|
|
|
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;
|
|
if (config->calibration != NULL) {
|
|
direction_controller_set_calibration(config->calibration);
|
|
}
|
|
}
|
|
|
|
s_directions = directions;
|
|
s_direction_count = direction_count;
|
|
s_current_angle = normalize_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;
|
|
}
|
|
|
|
if (equals_trimmed(input, "cal", true) ||
|
|
equals_trimmed(input, "calibrate", true) ||
|
|
equals_trimmed(input, "校准", false)) {
|
|
return DIRECTION_CONTROLLER_CMD_CALIBRATE;
|
|
}
|
|
|
|
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 normalized_target = normalize_angle(target_angle);
|
|
float rotation = calculate_shortest_rotation(s_current_angle, normalized_target);
|
|
|
|
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, normalized_target, rotation);
|
|
|
|
stepper_rotate_angle_with_accel(rotation, s_rotate_speed_us);
|
|
s_current_angle = normalized_target;
|
|
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 = normalize_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;
|
|
}
|
|
}
|
|
|
|
static esp_err_t configure_switch_gpio(const direction_controller_calibration_t *config)
|
|
{
|
|
if (config == NULL) {
|
|
return ESP_ERR_INVALID_ARG;
|
|
}
|
|
|
|
if (config->gpio_num < 0 || config->gpio_num >= GPIO_NUM_MAX) {
|
|
return ESP_ERR_INVALID_ARG;
|
|
}
|
|
|
|
gpio_config_t io_conf = {
|
|
.pin_bit_mask = (1ULL << config->gpio_num),
|
|
.mode = GPIO_MODE_INPUT,
|
|
.pull_up_en = config->pullup_en ? GPIO_PULLUP_ENABLE : GPIO_PULLUP_DISABLE,
|
|
.pull_down_en = config->pulldown_en ? GPIO_PULLDOWN_ENABLE : GPIO_PULLDOWN_DISABLE,
|
|
.intr_type = GPIO_INTR_DISABLE
|
|
};
|
|
|
|
return gpio_config(&io_conf);
|
|
}
|
|
|
|
static bool is_switch_active(const direction_controller_calibration_t *config)
|
|
{
|
|
int level = gpio_get_level(config->gpio_num);
|
|
int active = (config->active_level != 0) ? 1 : 0;
|
|
return level == active;
|
|
}
|
|
|
|
static float clamp_positive_default(float value, float default_value)
|
|
{
|
|
if (value > 0.0f) {
|
|
return value;
|
|
}
|
|
return default_value;
|
|
}
|
|
|
|
esp_err_t direction_controller_calibrate(const direction_controller_calibration_t *config)
|
|
{
|
|
const direction_controller_calibration_t *cfg = config;
|
|
|
|
if (!s_initialized) {
|
|
direction_controller_init(NULL);
|
|
}
|
|
|
|
if (cfg == NULL) {
|
|
if (!s_calibration_configured) {
|
|
ESP_LOGW(TAG, "Calibration not configured");
|
|
return ESP_ERR_INVALID_STATE;
|
|
}
|
|
cfg = &s_calibration;
|
|
}
|
|
|
|
if (!cfg->enabled) {
|
|
ESP_LOGW(TAG, "Calibration disabled");
|
|
return ESP_ERR_INVALID_STATE;
|
|
}
|
|
|
|
esp_err_t err = configure_switch_gpio(cfg);
|
|
if (err != ESP_OK) {
|
|
ESP_LOGE(TAG, "Failed to configure calibration switch GPIO");
|
|
return err;
|
|
}
|
|
|
|
if (is_switch_active(cfg)) {
|
|
s_current_angle = normalize_angle(cfg->calibration_angle);
|
|
stepper_motor_power_off();
|
|
ESP_LOGI(TAG, "Calibration switch active, angle set to %.1f°", s_current_angle);
|
|
return ESP_OK;
|
|
}
|
|
|
|
float step_angle = clamp_positive_default(cfg->step_angle_deg, 5.0f);
|
|
float max_sweep = clamp_positive_default(cfg->max_sweep_deg, 360.0f);
|
|
float total_sweep = 0.0f;
|
|
float direction = cfg->search_cw ? 1.0f : -1.0f;
|
|
int settle_delay_ms = cfg->settle_delay_ms;
|
|
|
|
ESP_LOGI(TAG, "Calibration started (step=%.1f°, max=%.1f°, dir=%s)",
|
|
step_angle, max_sweep, cfg->search_cw ? "CW" : "CCW");
|
|
|
|
while (total_sweep < max_sweep) {
|
|
float delta = direction * step_angle;
|
|
stepper_rotate_angle_with_accel(delta, s_rotate_speed_us);
|
|
s_current_angle = normalize_angle(s_current_angle + delta);
|
|
total_sweep += step_angle;
|
|
|
|
if (settle_delay_ms > 0) {
|
|
vTaskDelay(pdMS_TO_TICKS(settle_delay_ms));
|
|
}
|
|
|
|
if (is_switch_active(cfg)) {
|
|
s_current_angle = normalize_angle(cfg->calibration_angle);
|
|
stepper_motor_power_off();
|
|
ESP_LOGI(TAG, "Calibration complete, angle set to %.1f°", s_current_angle);
|
|
return ESP_OK;
|
|
}
|
|
}
|
|
|
|
stepper_motor_power_off();
|
|
ESP_LOGW(TAG, "Calibration failed: switch not triggered within %.1f°", max_sweep);
|
|
return ESP_ERR_TIMEOUT;
|
|
}
|
|
|
|
void direction_controller_set_calibration(const direction_controller_calibration_t *config)
|
|
{
|
|
if (config == NULL) {
|
|
s_calibration_configured = false;
|
|
memset(&s_calibration, 0, sizeof(s_calibration));
|
|
return;
|
|
}
|
|
|
|
s_calibration = *config;
|
|
s_calibration_configured = true;
|
|
}
|
|
|
|
const direction_controller_calibration_t *direction_controller_get_calibration(void)
|
|
{
|
|
if (!s_calibration_configured) {
|
|
return NULL;
|
|
}
|
|
return &s_calibration;
|
|
}
|