From 14f2ff94d0547ed47b526b84459a884fa85acf3b Mon Sep 17 00:00:00 2001 From: kirl <18827922006@163.com> Date: Sat, 1 Aug 2026 05:05:25 +0800 Subject: [PATCH] release v0.9 --- components/actuator/actuator.c | 199 ++++++++++++++++++++--------- components/actuator/actuator.h | 52 ++++++++ components/key/CMakeLists.txt | 3 + components/key/key.c | 31 +++++ components/key/key.h | 32 +++++ components/protocol/protocol.c | 8 ++ components/protocol/protocol.h | 6 + components/web_ctrl/CMakeLists.txt | 3 + components/web_ctrl/web_ctrl.c | 1 + main/CMakeLists.txt | 5 +- main/main.c | 58 +++++++++ 11 files changed, 337 insertions(+), 61 deletions(-) create mode 100644 components/key/CMakeLists.txt create mode 100644 components/key/key.c create mode 100644 components/key/key.h diff --git a/components/actuator/actuator.c b/components/actuator/actuator.c index ee01e62..dd143a9 100644 --- a/components/actuator/actuator.c +++ b/components/actuator/actuator.c @@ -12,6 +12,7 @@ #include "freertos/task.h" #include "esp_log.h" #include +#include static const char *TAG = "ACTUATOR"; @@ -23,8 +24,16 @@ static float s_scale_x = ACTUATOR_SCALE_X; static float s_scale_y = ACTUATOR_SCALE_Y; static float s_scale_angle = ACTUATOR_SCALE_ANGLE; +/* Z 旋转轴心 → 工具末端 偏移补偿 (mm) */ +static float s_offset_x_mm = ACTUATOR_OFFSET_X_MM; +static float s_offset_y_mm = ACTUATOR_OFFSET_Y_MM; + static bool s_initialized = false; +/* 最近一次执行的 Cycle 快照 */ +static edc_cycle_t s_last_cycle; +static bool s_last_cycle_valid = false; + /* ================================================================ * 内部: 帧间间隔 * @@ -103,6 +112,67 @@ void actuator_set_scale(float x_scale, float y_scale, float angle_scale) s_scale_x, s_scale_y, s_scale_angle); } +/* ================================================================ + * Z 旋转轴心 ↔ 工具末端 偏移补偿 + * + * 终端执行机构与 Z 旋转机构不在同一条纵轴上, + * 旋转角度 θ 时, 工具末端 XY 位置 = 旋转中心 + R(θ)·(dx, dy) + * + * 要将工具末端放到 target 位置, 旋转中心需要去: + * center = target - R(θ)·(dx, dy) + * + * 即: + * X_motor = X_tgt - (dx·cosθ - dy·sinθ) + * Y_motor = Y_tgt - (dx·sinθ + dy·cosθ) + * ================================================================ */ + +void actuator_set_offset(float offset_x_mm, float offset_y_mm) +{ + s_offset_x_mm = offset_x_mm; + s_offset_y_mm = offset_y_mm; + ESP_LOGI(TAG, "偏移补偿: dx=%.2fmm dy=%.2fmm", (double)offset_x_mm, (double)offset_y_mm); +} + +void actuator_get_offset(float *offset_x_mm, float *offset_y_mm) +{ + if (offset_x_mm) *offset_x_mm = s_offset_x_mm; + if (offset_y_mm) *offset_y_mm = s_offset_y_mm; +} + +void actuator_apply_offset_compensation(uint16_t *target_x, uint16_t *target_y, + float angle_deg) +{ + if (!target_x || !target_y) return; + + /* 偏移量为零时跳过, 避免浮点运算 */ + if (s_offset_x_mm == 0.0f && s_offset_y_mm == 0.0f) return; + + float theta_rad = angle_deg * (float)(M_PI / 180.0); + float cos_t = cosf(theta_rad); + float sin_t = sinf(theta_rad); + + /* R(θ)·(dx, dy): 旋转后的偏移向量 */ + float rot_dx = s_offset_x_mm * cos_t - s_offset_y_mm * sin_t; + float rot_dy = s_offset_x_mm * sin_t + s_offset_y_mm * cos_t; + + /* center = target - rotated_offset, 四舍五入 */ + float cx = (float)(*target_x) - rot_dx; + float cy = (float)(*target_y) - rot_dy; + + /* 钳位到有效范围并写回 */ + if (cx < 0.0f) cx = 0.0f; + if (cy < 0.0f) cy = 0.0f; + *target_x = (uint16_t)(cx + 0.5f); + *target_y = (uint16_t)(cy + 0.5f); + + ESP_LOGD(TAG, "补偿: (%.1f°) offset(%.2f,%.2f)→rot(%.2f,%.2f) " + "tgt→ctr (%u,%u)", + (double)angle_deg, + (double)s_offset_x_mm, (double)s_offset_y_mm, + (double)rot_dx, (double)rot_dy, + *target_x, *target_y); +} + /* ================================================================ * 内部: 单轴绝对位置运动 * ================================================================ */ @@ -148,77 +218,83 @@ static void _actuator_move_fragment(const edc_fragment_t *frag) uint16_t vel_angle = 50; uint8_t acc_angle = 100; + /* ---- 提取 source / target 坐标 (可被补偿修改) ---- */ + uint16_t src_x = frag->source.x; + uint16_t src_y = frag->source.y; + uint16_t src_angle = frag->source.angle_x10; + uint16_t tgt_x = frag->target.x; + uint16_t tgt_y = frag->target.y; + uint16_t tgt_angle = frag->target.angle_x10; + + /* Z 旋转轴心偏移补偿 (仅对 target 做) */ + float tgt_angle_deg = (float)tgt_angle * 0.1f; + actuator_apply_offset_compensation(&tgt_x, &tgt_y, tgt_angle_deg); + /* source 坐标 → 脉冲数 */ - uint32_t src_pulses_x = (uint32_t)(frag->source.x * s_scale_x); - uint32_t src_pulses_y = (uint32_t)(frag->source.y * s_scale_y); - uint32_t src_pulses_angle = (uint32_t)(frag->source.angle_x10 * s_scale_angle); + uint32_t src_pulses_x = (uint32_t)(src_x * s_scale_x); + uint32_t src_pulses_y = (uint32_t)(src_y * s_scale_y); + uint32_t src_pulses_angle = (uint32_t)(src_angle * s_scale_angle); /* target 坐标 → 脉冲数 */ - uint32_t dst_pulses_x = (uint32_t)(frag->target.x * s_scale_x); - uint32_t dst_pulses_y = (uint32_t)(frag->target.y * s_scale_y); - uint32_t dst_pulses_angle = (uint32_t)(frag->target.angle_x10 * s_scale_angle); + uint32_t dst_pulses_x = (uint32_t)(tgt_x * s_scale_x); + uint32_t dst_pulses_y = (uint32_t)(tgt_y * s_scale_y); + uint32_t dst_pulses_angle = (uint32_t)(tgt_angle * s_scale_angle); - ESP_LOGI(TAG, "Fragment: SRC(X=%lu Y=%lu A=%lu) → DST(X=%lu Y=%lu A=%lu)", + ESP_LOGI(TAG, "Fragment: SRC(X=%lu Y=%lu A=%lu) -> DST(X=%lu Y=%lu A=%lu comp x=%u y=%u)", src_pulses_x, src_pulses_y, src_pulses_angle, - dst_pulses_x, dst_pulses_y, dst_pulses_angle); + dst_pulses_x, dst_pulses_y, dst_pulses_angle, + tgt_x, tgt_y); /* ---- 1. 磁铁掉电 ---- */ magnet_set(false); /* ---- 2. 运动到 source 位置 ---- */ - /* Angle / Z 旋转轴 */ _actuator_move_abs(ACTUATOR_ADDR_ANGLE, ACTUATOR_DIR_ANGLE, src_pulses_angle, vel_angle, acc_angle, false); _frame_gap(); - - /* X 轴 */ _actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X, src_pulses_x, vel_xy, acc_xy, false); _frame_gap(); - - /* Y 轴 (双电机同步) */ _actuator_move_y_sync(src_pulses_y, vel_xy, acc_xy); - /* ---- 3. 等待 2s 运动到位 (红灯快闪) ---- */ - _wait_flash(2000, 128, 0, 0, 200); + /* ---- 3. 等待 4s 运动到位 (红灯快闪) ---- */ + _wait_flash(4000, 128, 0, 0, 200); /* ---- 4. 舵机放下 ---- */ servo_set_down(); - _wait_flash(500, 128, 0, 0, 200); + _wait_flash(1000, 128, 0, 0, 200); /* ---- 5. 磁铁吸合 ---- */ magnet_set(true); - _wait_flash(500, 128, 0, 0, 200); + _wait_flash(1000, 128, 0, 0, 200); /* ---- 6. 舵机抬起 ---- */ servo_set_up(); - _wait_flash(500, 128, 0, 0, 200); + _wait_flash(1000, 128, 0, 0, 200); /* ---- 7. 运动到 target 位置 ---- */ _actuator_move_abs(ACTUATOR_ADDR_ANGLE, ACTUATOR_DIR_ANGLE, dst_pulses_angle, vel_angle, acc_angle, false); _frame_gap(); - _actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X, dst_pulses_x, vel_xy, acc_xy, false); _frame_gap(); - _actuator_move_y_sync(dst_pulses_y, vel_xy, acc_xy); - /* ---- 8. 等待 2s 运动到位 (红灯快闪) ---- */ - _wait_flash(2000, 128, 0, 0, 200); + /* ---- 8. 等待 4s 运动到位 (红灯快闪) ---- */ + _wait_flash(4000, 128, 0, 0, 200); /* ---- 9. 舵机放下 ---- */ servo_set_down(); - _wait_flash(500, 128, 0, 0, 200); + _wait_flash(1000, 128, 0, 0, 200); /* ---- 10. 磁铁掉电 ---- */ magnet_set(false); - _wait_flash(500, 128, 0, 0, 200); + _wait_flash(1000, 128, 0, 0, 200); /* ---- 11. 舵机抬起 ---- */ servo_set_up(); - _wait_flash(500, 128, 0, 0, 200); + _wait_flash(1000, 128, 0, 0, 200); } /* ================================================================ @@ -241,6 +317,10 @@ int actuator_execute_cycle(const edc_cycle_t *cycle) return -1; } + /* 保存快照 (执行前先存, 即使执行中也可查询) */ + memcpy(&s_last_cycle, cycle, sizeof(edc_cycle_t)); + s_last_cycle_valid = true; + ESP_LOGI(TAG, "=== Cycle #%u 开始 (%u fragments) ===", cycle->cycle_id, cycle->frag_count); @@ -253,6 +333,37 @@ int actuator_execute_cycle(const edc_cycle_t *cycle) return 0; } +/* ================================================================ + * 协议回调: 收到 Cycle → 执行 → DONE + * ================================================================ */ + +void edc_proto_on_cycle(const edc_cycle_t *cycle, edc_channel_t ch) +{ + static uint16_t s_last_exec_id = 0xFFFF; + + ESP_LOGI(TAG, "Cycle #%u via %s, %u frags", + cycle->cycle_id, ch == EDC_CH_I2C ? "I2C" : "UART", + cycle->frag_count); + + /* I2C / UART 冗余下发, 同一 cycle_id 只执行一次 */ + if (cycle->cycle_id == s_last_exec_id) { + ESP_LOGD(TAG, "Cycle #%u 重复, 跳过执行", cycle->cycle_id); + return; + } + s_last_exec_id = cycle->cycle_id; + + /* 执行电机运动 (阻塞, 含拾放全流程) */ + ESP_LOGI(TAG, "开始执行 Cycle #%u ...", cycle->cycle_id); + actuator_execute_cycle(cycle); + + /* 全部 Fragment 执行完成后回复 DONE */ + edc_proto_send_done(cycle->cycle_id); + ESP_LOGI(TAG, "Cycle #%u 执行完毕, DONE 已发送", cycle->cycle_id); + + /* 设置 RGB 状态为 DONE, 主循环将持续绿灯慢闪 */ + rgb_set_state(RGB_STATE_DONE); +} + /* ================================================================ * 急停 * ================================================================ */ @@ -429,13 +540,9 @@ void actuator_motor_stop(uint8_t addr) } /* ================================================================ - * 协议回调: 收到 Cycle → 执行 → DONE + * 最近 Cycle 快照查询 * ================================================================ */ -static edc_cycle_t s_last_cycle; -static bool s_last_cycle_valid; -static portMUX_TYPE s_cycle_lock = portMUX_INITIALIZER_UNLOCKED; - const edc_cycle_t *actuator_get_last_cycle(void) { return s_last_cycle_valid ? &s_last_cycle : NULL; @@ -446,35 +553,7 @@ bool actuator_is_last_cycle_valid(void) return s_last_cycle_valid; } -void edc_proto_on_cycle(const edc_cycle_t *cycle, edc_channel_t ch) +void actuator_clear_last_cycle(void) { - static uint16_t s_last_exec_id = 0xFFFF; - - /* 保存快照 */ - portENTER_CRITICAL(&s_cycle_lock); - memcpy(&s_last_cycle, cycle, sizeof(edc_cycle_t)); - s_last_cycle_valid = true; - portEXIT_CRITICAL(&s_cycle_lock); - - ESP_LOGI(TAG, "Cycle #%u via %s, %u frags", - cycle->cycle_id, ch == EDC_CH_I2C ? "I2C" : "UART", - cycle->frag_count); - - /* I2C / UART 冗余下发, 同一 cycle_id 只执行一次 */ - if (cycle->cycle_id == s_last_exec_id) { - ESP_LOGD(TAG, "Cycle #%u 重复, 跳过执行", cycle->cycle_id); - return; - } - s_last_exec_id = cycle->cycle_id; - - /* 执行电机运动 (阻塞, 含拾放全流程) */ - ESP_LOGI(TAG, "开始执行 Cycle #%u ...", cycle->cycle_id); - actuator_execute_cycle(cycle); - - /* 全部 Fragment 执行完成后回复 DONE */ - edc_proto_send_done(cycle->cycle_id); - ESP_LOGI(TAG, "Cycle #%u 执行完毕, DONE 已发送", cycle->cycle_id); - - /* 设置 RGB 状态为 DONE, 主循环将持续绿灯慢闪 */ - rgb_set_state(RGB_STATE_DONE); + s_last_cycle_valid = false; } diff --git a/components/actuator/actuator.h b/components/actuator/actuator.h index f36bae0..223afb3 100644 --- a/components/actuator/actuator.h +++ b/components/actuator/actuator.h @@ -118,6 +118,26 @@ extern "C" { /** 控制模式: 2 = 闭环位置模式 */ #define ACTUATOR_CTRL_MODE 2 +/* ================================================================ + * Z 旋转轴心 ↔ 工具末端 偏移补偿 + * + * 终端执行机构 (工具末端) 与 Z 旋转机构不在同一条纵轴上, + * 旋转角度 θ 时, 工具末端的 XY 位置会绕旋转中心偏移。 + * + * 补偿公式 (将工具末端定位到 target): + * X_motor = X_tgt - (dx * cosθ - dy * sinθ) + * Y_motor = Y_tgt - (dx * sinθ + dy * cosθ) + * + * dx, dy 为工具末端相对于 Z 旋转中心的 XY 偏移 (mm), + * 待机械测量后填入。 + * ================================================================ */ +#ifndef ACTUATOR_OFFSET_X_MM +#define ACTUATOR_OFFSET_X_MM 8.0f /* angle=0° 时工具末端偏右 ~8mm */ +#endif +#ifndef ACTUATOR_OFFSET_Y_MM +#define ACTUATOR_OFFSET_Y_MM -30.0f /* angle=0° 时工具末端偏上 ~30mm */ +#endif + /** * XY 平面左上零点坐标系 运动方向: * X 轴正向 (→): CW (dir=0) ── 电机2 @@ -146,6 +166,32 @@ void actuator_init(void); */ void actuator_set_scale(float x_scale, float y_scale, float angle_scale); +/** + * @brief 设置 Z 旋转轴心 ↔ 工具末端 XY 偏移量 (mm) + * @param offset_x_mm 工具末端相对于旋转中心的 X 偏移 (mm) + * @param offset_y_mm 工具末端相对于旋转中心的 Y 偏移 (mm) + * @note 补偿会在每次 fragment 执行时自动应用; + * 未调用时默认使用 ACTUATOR_OFFSET_X_MM / ACTUATOR_OFFSET_Y_MM + */ +void actuator_set_offset(float offset_x_mm, float offset_y_mm); + +/** + * @brief 获取当前 XY 偏移量 + * @param offset_x_mm [out] X 偏移 (mm), 可为 NULL + * @param offset_y_mm [out] Y 偏移 (mm), 可为 NULL + */ +void actuator_get_offset(float *offset_x_mm, float *offset_y_mm); + +/** + * @brief 对目标坐标应用 Z 旋转偏移补偿 + * @param target_x [in/out] 目标 X 坐标 (mm) + * @param target_y [in/out] 目标 Y 坐标 (mm) + * @param angle_deg 当前旋转角度 (度) + * @note 内部使用, 也可由上层在手动模式调用 + */ +void actuator_apply_offset_compensation(uint16_t *target_x, uint16_t *target_y, + float angle_deg); + /** * @brief 执行一个完整 Cycle (遍历所有 Fragment, 逐段运动) * @param cycle 协议下发的 Cycle 数据 @@ -241,6 +287,12 @@ const edc_cycle_t *actuator_get_last_cycle(void); * @brief 最近 Cycle 是否有效 */ bool actuator_is_last_cycle_valid(void); + +/** + * @brief 清除最近 Cycle 记录 (用于等待新 Cycle) + */ +void actuator_clear_last_cycle(void); + #ifdef __cplusplus } #endif diff --git a/components/key/CMakeLists.txt b/components/key/CMakeLists.txt new file mode 100644 index 0000000..5452bf9 --- /dev/null +++ b/components/key/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register(SRCS "key.c" + INCLUDE_DIRS "." + REQUIRES esp_driver_gpio driver freertos) diff --git a/components/key/key.c b/components/key/key.c new file mode 100644 index 0000000..10da91d --- /dev/null +++ b/components/key/key.c @@ -0,0 +1,31 @@ +/** + * @file key.c + * @brief 单 GPIO 按键控制实现 (低电平有效, 软件消抖) + */ + +#include "key.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + +static int s_gpio = -1; + +void key_init(int gpio) +{ + s_gpio = gpio; + + gpio_reset_pin(s_gpio); + gpio_set_direction(s_gpio, GPIO_MODE_INPUT); + gpio_set_pull_mode(s_gpio, GPIO_PULLUP_ONLY); +} + +bool key_is_pressed(void) +{ + if (s_gpio < 0) return false; + + /* 低电平有效 + 简单消抖: 连续两次读到低电平才算按下 */ + if (gpio_get_level(s_gpio) == 0) { + vTaskDelay(pdMS_TO_TICKS(20)); + return (gpio_get_level(s_gpio) == 0); + } + return false; +} diff --git a/components/key/key.h b/components/key/key.h new file mode 100644 index 0000000..5ac68c7 --- /dev/null +++ b/components/key/key.h @@ -0,0 +1,32 @@ +/** + * @file key.h + * @brief 单 GPIO 按键控制 (低电平有效, 软件消抖) + */ + +#ifndef KEY_H +#define KEY_H + +#include +#include "driver/gpio.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief 初始化按键 (内部上拉, 低电平有效) + * @param gpio GPIO 引脚号, 如 GPIO_NUM_8 + */ +void key_init(int gpio); + +/** + * @brief 读取按键状态 (已消抖) + * @return true=按下, false=释放 + */ +bool key_is_pressed(void); + +#ifdef __cplusplus +} +#endif + +#endif /* KEY_H */ diff --git a/components/protocol/protocol.c b/components/protocol/protocol.c index c2cd7d0..975b129 100644 --- a/components/protocol/protocol.c +++ b/components/protocol/protocol.c @@ -452,6 +452,14 @@ void edc_proto_send_request(void) ESP_LOGI(TAG, "REQUEST sent"); } +void edc_proto_send_recognize(void) +{ + if (!s_uart_enabled) return; + uart_send_frame(EDC_CMD_RECOGNIZE, NULL, 0); + s_stat.request_sent++; + ESP_LOGI(TAG, "RECOGNIZE sent"); +} + /* ================================================================ * 统计 * ================================================================ */ diff --git a/components/protocol/protocol.h b/components/protocol/protocol.h index 7354518..602b242 100644 --- a/components/protocol/protocol.h +++ b/components/protocol/protocol.h @@ -76,6 +76,7 @@ typedef enum { EDC_CMD_CYCLE_DATA = 0x01, /* MPU → MCU: 下发 Cycle 数据 */ EDC_CMD_CYCLE_DONE = 0x02, /* MCU → MPU: Cycle 执行完成 */ EDC_CMD_CYCLE_REQUEST = 0x03, /* MCU → MPU: 请求下一个 Cycle */ + EDC_CMD_RECOGNIZE = 0x04, /* MCU → MPU: 触发视觉识别 */ EDC_CMD_ACK = 0x80, /* 通用应答 */ EDC_CMD_NACK = 0xFF, /* 错误 */ } edc_cmd_t; @@ -209,6 +210,11 @@ void edc_proto_send_done(uint16_t cycle_id); */ void edc_proto_send_request(void); +/** + * @brief 发送 RECOGNIZE 触发视觉识别 (通过 UART) + */ +void edc_proto_send_recognize(void); + /** * @brief 获取统计信息 */ diff --git a/components/web_ctrl/CMakeLists.txt b/components/web_ctrl/CMakeLists.txt index 43e7747..b5f2bd1 100644 --- a/components/web_ctrl/CMakeLists.txt +++ b/components/web_ctrl/CMakeLists.txt @@ -1,3 +1,6 @@ idf_component_register(SRCS "web_ctrl.c" INCLUDE_DIRS "." REQUIRES emm_v5 magnet servo rgb esp_http_server esp_wifi nvs_flash protocol actuator) + +# 调试开关: 启用 WiFi + Web 服务器 +target_compile_definitions(${COMPONENT_LIB} PRIVATE EDC_DEBUG_WEB) diff --git a/components/web_ctrl/web_ctrl.c b/components/web_ctrl/web_ctrl.c index c0ef9ca..6d0b845 100644 --- a/components/web_ctrl/web_ctrl.c +++ b/components/web_ctrl/web_ctrl.c @@ -181,6 +181,7 @@ button:active{opacity:.7} +
diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 5bc56dd..12d5f67 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1,3 +1,6 @@ idf_component_register(SRCS "main.c" INCLUDE_DIRS "." - PRIV_REQUIRES emm_v5 magnet servo web_ctrl protocol rgb actuator) + PRIV_REQUIRES emm_v5 magnet servo key web_ctrl protocol rgb actuator) + +# 调试开关: 启用 WiFi + Web 服务器 (消除 IDE 未定义警告) +target_compile_definitions(${COMPONENT_LIB} PRIVATE EDC_DEBUG_WEB) diff --git a/main/main.c b/main/main.c index c41863c..575c2c1 100644 --- a/main/main.c +++ b/main/main.c @@ -12,6 +12,7 @@ * ESP32 GPIO2 -> I2C SDA (← MPU) * ESP32 GPIO3 -> I2C SCL * ESP32 GPIO27 -> RGB LED (WS2812) + * ESP32 GPIO8 -> 按钮 (低电平有效, 触发识别) * ESP32 GND -> 电机 GND / MPU GND * * 注: 日志控制台已切换到 USB Serial/JTAG (COM 口监视), @@ -27,12 +28,63 @@ #include "protocol.h" #include "rgb.h" #include "actuator.h" +#include "key.h" #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include "esp_log.h" static const char *TAG = "MAIN"; +/* ---- 按钮识别任务 ---- */ +#define RECOGNIZE_TIMEOUT_MS 20000 +#define RECOGNIZE_POLL_MS 100 + +static void button_recognize_task(void *pvParam) +{ + while (1) { + if (key_is_pressed()) { + ESP_LOGI(TAG, "按钮按下, 归零后再发送 RECOGNIZE ..."); + + /* ---- 归零: 舵机抬起 + Z 回 0° ---- */ + actuator_servo_up(); + actuator_motor_angle(ACTUATOR_ADDR_ANGLE, 0.0f, 50, 100, true); + vTaskDelay(pdMS_TO_TICKS(500)); + + /* ---- XY 回零: X=0, Y=330 (vel=1500 acc=200) ---- */ + actuator_motor_mm(ACTUATOR_ADDR_X, 0.0f, 1500, 200, true); + actuator_motor_mm(ACTUATOR_ADDR_Y1, 330.0f, 1500, 200, true); + vTaskDelay(pdMS_TO_TICKS(2000)); + + ESP_LOGI(TAG, "归零完成, 发送 RECOGNIZE ..."); + + /* 清除旧 Cycle 记录 */ + actuator_clear_last_cycle(); + + /* 发送识别指令 */ + edc_proto_send_recognize(); + + /* 等待 MPU 回传 Cycle, 超时则发送 REQUEST */ + bool got_cycle = false; + int loops = RECOGNIZE_TIMEOUT_MS / RECOGNIZE_POLL_MS; + for (int i = 0; i < loops; i++) { + vTaskDelay(pdMS_TO_TICKS(RECOGNIZE_POLL_MS)); + if (actuator_is_last_cycle_valid()) { + got_cycle = true; + ESP_LOGI(TAG, "RECOGNIZE 成功, 已收到 Cycle"); + break; + } + } + + if (!got_cycle) { + ESP_LOGW(TAG, "RECOGNIZE 超时 (%d ms), 发送 CYCLE_REQUEST", + RECOGNIZE_TIMEOUT_MS); + edc_proto_send_request(); + } + } + vTaskDelay(pdMS_TO_TICKS(50)); + } +} + void app_main(void) { /* 初始化 RGB LED (GPIO27, 最先启动, 指示系统状态) */ @@ -45,12 +97,18 @@ void app_main(void) /* 初始化电磁铁 (GPIO5) */ magnet_init(GPIO_NUM_5); + /* 初始化按钮 (GPIO8, 低电平有效, 触发识别) */ + key_init(GPIO_NUM_8); + /* 初始化通讯协议 (I2C 从机 GPIO2/3 + UART0 GPIO11/12) */ edc_proto_init(NULL); /* 启动协议接收任务 */ xTaskCreate(edc_proto_task, "proto", 4096, NULL, 2, NULL); + /* 启动按钮识别任务 */ + xTaskCreate(button_recognize_task, "btn", 2048, NULL, 1, NULL); + /* 上电延时 2 秒, 等待电机闭环初始化完毕 */ ESP_LOGI(TAG, "等待电机初始化..."); vTaskDelay(pdMS_TO_TICKS(2000));