Bairan.Zhang/stepper_base
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add calibration switch support

Browse Source
This commit is contained in:
张柏然
2026-01-30 17:08:40 +08:00
parent 512c64ecff
commit a709cdb56a
3 changed files with 228 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

158
components/direction_controller/direction_controller.c
View File

@@ -6,6 +6,9 @@
#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"
@@ -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;
}

39
components/direction_controller/include/direction_controller.h
View File

@@ -8,6 +8,7 @@
#include <stdbool.h>
#include <stddef.h>
#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

38
main/main.c
View File

@@ -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°)");