From a709cdb56a8696ee5ac3acfe7e2b952664ded15d Mon Sep 17 00:00:00 2001 From: Bairan Zhang Date: Fri, 30 Jan 2026 17:08:40 +0800 Subject: [PATCH] feat: add calibration switch support --- .../direction_controller.c | 158 +++++++++++++++++- .../include/direction_controller.h | 39 ++++- main/main.c | 38 ++++- 3 files changed, 228 insertions(+), 7 deletions(-) diff --git a/components/direction_controller/direction_controller.c b/components/direction_controller/direction_controller.c index ccb422e..8ea3533 100644 --- a/components/direction_controller/direction_controller.c +++ b/components/direction_controller/direction_controller.c @@ -6,6 +6,9 @@ #include #include +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "driver/gpio.h" #include "esp_log.h" #include "stepper_motor.h" #include "direction_controller.h" @@ -29,6 +32,8 @@ static size_t s_direction_count = sizeof(s_default_directions) / sizeof(s_defaul 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) { @@ -88,6 +93,17 @@ static bool equals_trimmed(const char *input, const char *token, bool case_insen 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; @@ -120,11 +136,14 @@ void direction_controller_init(const direction_controller_config_t *config) 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 = initial_angle; + s_current_angle = normalize_angle(initial_angle); s_rotate_speed_us = rotate_speed_us; if (init_stepper_gpio) { @@ -152,6 +171,12 @@ direction_controller_cmd_t direction_controller_parse_command(const char *input, 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) { @@ -185,7 +210,8 @@ esp_err_t direction_controller_rotate_to_angle(float target_angle) direction_controller_init(NULL); } - float rotation = calculate_shortest_rotation(s_current_angle, target_angle); + 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"); @@ -193,10 +219,10 @@ esp_err_t direction_controller_rotate_to_angle(float target_angle) } ESP_LOGI(TAG, "Rotating from %.1f° to %.1f° (rotation: %.1f°)", - s_current_angle, target_angle, rotation); + s_current_angle, normalized_target, rotation); stepper_rotate_angle_with_accel(rotation, s_rotate_speed_us); - s_current_angle = target_angle; + s_current_angle = normalized_target; stepper_motor_power_off(); ESP_LOGI(TAG, "Rotation complete, current position: %.1f°", s_current_angle); @@ -221,7 +247,7 @@ float direction_controller_get_current_angle(void) void direction_controller_set_current_angle(float angle) { - s_current_angle = angle; + s_current_angle = normalize_angle(angle); } const direction_controller_direction_t *direction_controller_get_current_direction(void) @@ -260,3 +286,125 @@ void direction_controller_set_rotate_speed_us(int rotate_speed_us) 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; +} diff --git a/components/direction_controller/include/direction_controller.h b/components/direction_controller/include/direction_controller.h index c036817..98aa10e 100644 --- a/components/direction_controller/include/direction_controller.h +++ b/components/direction_controller/include/direction_controller.h @@ -8,6 +8,7 @@ #include #include +#include "driver/gpio.h" #include "esp_err.h" #ifdef __cplusplus @@ -33,8 +34,25 @@ typedef struct { 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 */ + const struct direction_controller_calibration_t *calibration; /* Optional calibration config */ } direction_controller_config_t; +/** + * @brief Calibration switch configuration + */ +typedef struct direction_controller_calibration_t { + bool enabled; /* Enable calibration */ + gpio_num_t gpio_num; /* Switch GPIO */ + int active_level; /* Active logic level (0 or 1) */ + bool pullup_en; /* Enable internal pull-up (if supported) */ + bool pulldown_en; /* Enable internal pull-down (if supported) */ + float calibration_angle; /* Angle to set when switch is active */ + float step_angle_deg; /* Step angle per search iteration */ + float max_sweep_deg; /* Maximum total sweep angle */ + bool search_cw; /* Search direction: true=clockwise, false=counterclockwise */ + int settle_delay_ms; /* Delay after each step to allow switch settle */ +} direction_controller_calibration_t; + /** * @brief Parsed command types */ @@ -42,7 +60,8 @@ typedef enum { DIRECTION_CONTROLLER_CMD_UNKNOWN = 0, DIRECTION_CONTROLLER_CMD_HELP, DIRECTION_CONTROLLER_CMD_POSITION, - DIRECTION_CONTROLLER_CMD_DIRECTION + DIRECTION_CONTROLLER_CMD_DIRECTION, + DIRECTION_CONTROLLER_CMD_CALIBRATE } direction_controller_cmd_t; /** @@ -86,6 +105,14 @@ esp_err_t direction_controller_rotate_to_angle(float target_angle); */ esp_err_t direction_controller_rotate_to_direction(const char *input); +/** + * @brief Calibrate direction using a switch + * + * @param config Optional calibration configuration, NULL uses config set in init + * @return ESP_OK on success + */ +esp_err_t direction_controller_calibrate(const direction_controller_calibration_t *config); + /** * @brief Get current angle */ @@ -121,6 +148,16 @@ int direction_controller_get_rotate_speed_us(void); */ void direction_controller_set_rotate_speed_us(int rotate_speed_us); +/** + * @brief Set calibration configuration + */ +void direction_controller_set_calibration(const direction_controller_calibration_t *config); + +/** + * @brief Get calibration configuration (NULL if not configured) + */ +const direction_controller_calibration_t *direction_controller_get_calibration(void); + #ifdef __cplusplus } #endif diff --git a/main/main.c b/main/main.c index ce70132..1c292cc 100644 --- a/main/main.c +++ b/main/main.c @@ -48,6 +48,7 @@ static void print_help(void) printf("命令 (Commands):\n"); printf(" help - 显示帮助信息\n"); printf(" pos - 显示当前位置\n"); + printf(" cal - 校准正北位置\n"); printf("========================================\n\n"); } @@ -119,6 +120,21 @@ static void direction_control_task(void *arg) case DIRECTION_CONTROLLER_CMD_POSITION: print_current_position(); break; + case DIRECTION_CONTROLLER_CMD_CALIBRATE: { + printf("开始校准...\n"); + esp_err_t err = direction_controller_calibrate(NULL); + if (err == ESP_OK) { + printf("校准完成\n"); + print_current_position(); + } else if (err == ESP_ERR_TIMEOUT) { + printf("校准失败: 未检测到微动开关\n"); + } else if (err == ESP_ERR_INVALID_STATE) { + printf("校准失败: 未配置校准开关\n"); + } else { + printf("校准失败: 错误码 %d\n", err); + } + break; + } case DIRECTION_CONTROLLER_CMD_DIRECTION: if (dir != NULL) { printf("目标方向: %s (%s, %.1f°)\n", @@ -181,7 +197,27 @@ void app_main(void) ESP_LOGI(TAG, "========================================"); /* Initialize peripherals */ - direction_controller_init(NULL); + direction_controller_calibration_t calibration = { + .enabled = true, + .gpio_num = GPIO_NUM_39, + .active_level = 0, + .pullup_en = false, + .pulldown_en = false, + .calibration_angle = 180.0f, + .step_angle_deg = 5.0f, + .max_sweep_deg = 360.0f, + .search_cw = true, + .settle_delay_ms = 10 + }; + direction_controller_config_t config = { + .directions = NULL, + .direction_count = 0, + .initial_angle = 0.0f, + .rotate_speed_us = 0, + .init_stepper_gpio = true, + .calibration = &calibration + }; + direction_controller_init(&config); uart_init(); ESP_LOGI(TAG, "Initial position: South (0°)");