From fef37b36e964bd01df38600bd40f8d083535c293 Mon Sep 17 00:00:00 2001 From: kirl <18827922006@163.com> Date: Sat, 1 Aug 2026 13:24:47 +0800 Subject: [PATCH] release v1.0 --- components/actuator/actuator.c | 127 +++++++++++++++++++++++++++------ components/actuator/actuator.h | 64 +++++++++++++++-- main/main.c | 32 ++++++--- 3 files changed, 185 insertions(+), 38 deletions(-) diff --git a/components/actuator/actuator.c b/components/actuator/actuator.c index d152d9b..0d9ec20 100644 --- a/components/actuator/actuator.c +++ b/components/actuator/actuator.c @@ -50,17 +50,78 @@ static inline void _frame_gap(void) vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS)); } -/* 内部: 带 RGB 闪烁的延时 (周期 ms, 亮灭各半) */ +/* 内部: 带 RGB 闪烁的延时 (周期 ms, 亮灭各半; 精确到系统 tick) */ static void _wait_flash(uint32_t ms, uint8_t r, uint8_t g, uint8_t b, uint32_t period_ms) { - uint32_t half = pdMS_TO_TICKS(period_ms / 2); - int cycles = (int)(ms / period_ms); - for (int i = 0; i < cycles; i++) { - rgb_set(r, g, b); - vTaskDelay(half); - rgb_off(); - vTaskDelay(half); + TickType_t total = pdMS_TO_TICKS(ms); + TickType_t half = pdMS_TO_TICKS(period_ms / 2); + if (half == 0) half = 1; + bool led_on = true; + + while (total > 0) { + TickType_t chunk = (total > half) ? half : total; + if (led_on) rgb_set(r, g, b); + else rgb_off(); + vTaskDelay(chunk); + total -= chunk; + led_on = !led_on; } + rgb_off(); +} + +/* ================================================================ + * 运动到位时间估算 + * + * Emm_V5 曲线加减速 (官方文档): + * t2 - t1 = (256 - acc) * 50(us), Vt2 = Vt1 + 1(RPM) + * 即每提升 1 RPM 耗时 (256-acc)*50us, 按梯形/三角形加减速估算。 + * ================================================================ */ + +uint32_t actuator_motion_time_ms(uint32_t pulses, uint16_t vel, uint8_t acc) +{ + if (pulses == 0 || vel == 0) return 0; + + float revs = (float)pulses / (float)EMM_V5_PULSES_PER_REV; + float v_rps = (float)vel / 60.0f; + float t_ms; + + if (acc == 0) { + /* acc=0: 无曲线加减速, 直接按设定速度恒速运行 */ + t_ms = revs / v_rps * 1000.0f; + } else { + /* 从 0 加速到目标转速的时间与加速段距离 (线性曲线近似) */ + float t_ramp_s = (float)vel * (float)(256 - acc) * 50e-6f; + float a_rps2 = v_rps / t_ramp_s; + float d_ramp_rev = 0.5f * a_rps2 * t_ramp_s * t_ramp_s; + + if (revs <= 2.0f * d_ramp_rev) { + /* 行程不足, 未到目标转速即减速 → 三角形 */ + t_ms = 2.0f * sqrtf(revs / a_rps2) * 1000.0f; + } else { + /* 加速 + 恒速 + 减速 */ + t_ms = (2.0f * t_ramp_s + (revs - 2.0f * d_ramp_rev) / v_rps) * 1000.0f; + } + } + return (uint32_t)(t_ms + 0.999f); +} + +uint32_t actuator_settle_ms(uint32_t pulses, uint16_t vel, uint8_t acc) +{ + return actuator_motion_time_ms(pulses, vel, acc) + ACTUATOR_SETTLE_MARGIN_MS; +} + +uint32_t actuator_mm_settle_ms(float distance_mm, uint16_t vel, uint8_t acc) +{ + if (distance_mm < 0.0f) distance_mm = -distance_mm; + uint32_t pulses = (uint32_t)(distance_mm * s_scale_x + 0.5f); + return actuator_settle_ms(pulses, vel, acc); +} + +uint32_t actuator_angle_settle_ms(float angle_deg, uint16_t vel, uint8_t acc) +{ + if (angle_deg < 0.0f) angle_deg = -angle_deg; + uint32_t pulses = (uint32_t)(angle_deg * 10.0f * s_scale_angle + 0.5f); + return actuator_settle_ms(pulses, vel, acc); } /* ================================================================ @@ -279,11 +340,12 @@ static void _actuator_move_y_sync(uint32_t pulses_y, uint16_t vel, uint8_t acc) static void _actuator_move_fragment(const edc_fragment_t *frag) { - /* 固定速度/加速度 */ - uint16_t vel_xy = 1500; - uint8_t acc_xy = 200; - uint16_t vel_angle = 50; - uint8_t acc_angle = 100; + /* XY 执行速度/加速度 (3000RPM + 250 档, 实测稳定); + Z 旋转轴保持低速 (见 ACTUATOR_EXEC_Z_VEL/Z_ACC) */ + uint16_t vel_xy = ACTUATOR_EXEC_VEL; + uint8_t acc_xy = ACTUATOR_EXEC_ACC; + uint16_t vel_angle = ACTUATOR_EXEC_Z_VEL; + uint8_t acc_angle = ACTUATOR_EXEC_Z_ACC; /* ---- 提取 source / target 坐标 (可被补偿修改) ---- */ uint16_t src_x = frag->source.x; @@ -316,6 +378,17 @@ static void _actuator_move_fragment(const edc_fragment_t *frag) (double)tgt_angle_deg, tgt_x, tgt_y); + /* 按实际行程/速度/加速度计算到位等待 (Z/X/Y 并发, 取最长者, 含安全裕量) */ + float src_target = _closest_reachable_angle(src_angle_deg, s_cur_angle_deg, + ACTUATOR_ANGLE_LIMIT_DEG); + uint32_t src_wait_ms = actuator_settle_ms( + (uint32_t)(fabsf(src_target) * 10.0f * s_scale_angle + 0.5f), + vel_angle, acc_angle); + uint32_t t_wait = actuator_settle_ms(src_pulses_x, vel_xy, acc_xy); + if (t_wait > src_wait_ms) src_wait_ms = t_wait; + t_wait = actuator_settle_ms(src_pulses_y, vel_xy, acc_xy); + if (t_wait > src_wait_ms) src_wait_ms = t_wait; + /* ---- 1. 磁铁掉电 ---- */ magnet_set(false); @@ -327,26 +400,34 @@ static void _actuator_move_fragment(const edc_fragment_t *frag) _frame_gap(); _actuator_move_y_sync(src_pulses_y, vel_xy, acc_xy); - /* ---- 3. 等待 4s 运动到位 (红灯快闪) ---- */ - _wait_flash(4000, 128, 0, 0, 200); + /* ---- 3. 等待运动到位 (红灯快闪, 按行程实时计算) ---- */ + _wait_flash(src_wait_ms, 128, 0, 0, 200); /* ---- 4. 舵机放下 ---- */ servo_set_down(); - _wait_flash(1000, 128, 0, 0, 200); + _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200); /* ---- 5. 磁铁吸合 ---- */ magnet_set(true); - _wait_flash(1000, 128, 0, 0, 200); + _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200); /* ---- 6. 舵机抬起 ---- */ servo_set_up(); - _wait_flash(1000, 128, 0, 0, 200); + _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200); /* ---- 7. 运动到 target 位置 ---- */ /* 目标角度取行程内与当前位置最近的等价角, 再用相对运动执行: 终点朝向与旧协议 (绝对转到 rotate mod 360) 完全一致, 且不会绕远路 */ float dst_rep = _closest_reachable_angle(tgt_angle_deg, s_cur_angle_deg, ACTUATOR_ANGLE_LIMIT_DEG); + uint32_t dst_wait_ms = actuator_settle_ms( + (uint32_t)(fabsf(dst_rep - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f), + vel_angle, acc_angle); + t_wait = actuator_settle_ms(dst_pulses_x, vel_xy, acc_xy); + if (t_wait > dst_wait_ms) dst_wait_ms = t_wait; + t_wait = actuator_settle_ms(dst_pulses_y, vel_xy, acc_xy); + if (t_wait > dst_wait_ms) dst_wait_ms = t_wait; + _actuator_move_angle_rel(dst_rep - s_cur_angle_deg, vel_angle, acc_angle); _frame_gap(); _actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X, @@ -354,20 +435,20 @@ static void _actuator_move_fragment(const edc_fragment_t *frag) _frame_gap(); _actuator_move_y_sync(dst_pulses_y, vel_xy, acc_xy); - /* ---- 8. 等待 4s 运动到位 (红灯快闪) ---- */ - _wait_flash(4000, 128, 0, 0, 200); + /* ---- 8. 等待运动到位 (红灯快闪, 按行程实时计算) ---- */ + _wait_flash(dst_wait_ms, 128, 0, 0, 200); /* ---- 9. 舵机放下 ---- */ servo_set_down(); - _wait_flash(1000, 128, 0, 0, 200); + _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200); /* ---- 10. 磁铁掉电 ---- */ magnet_set(false); - _wait_flash(1000, 128, 0, 0, 200); + _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200); /* ---- 11. 舵机抬起 ---- */ servo_set_up(); - _wait_flash(1000, 128, 0, 0, 200); + _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200); } /* ================================================================ diff --git a/components/actuator/actuator.h b/components/actuator/actuator.h index 1244ea6..42656df 100644 --- a/components/actuator/actuator.h +++ b/components/actuator/actuator.h @@ -106,9 +106,30 @@ extern "C" { #define ACTUATOR_ANGLE_LIMIT_DEG 270.0f #endif -/** Fragment 间等待 (ms), 暂用固定延时替代到位检测 */ -#ifndef ACTUATOR_FRAG_DELAY_MS -#define ACTUATOR_FRAG_DELAY_MS 500 +/** 执行 Cycle / 归零时 XY 轴的速度与加速度档位 (实测 3000RPM + 250 档稳定) */ +#ifndef ACTUATOR_EXEC_VEL +#define ACTUATOR_EXEC_VEL 3000 +#endif +#ifndef ACTUATOR_EXEC_ACC +#define ACTUATOR_EXEC_ACC 250 +#endif + +/** Z 旋转轴执行速度与加速度 (结构刚性有限, 必须保持低速; 300RPM 已偏快) */ +#ifndef ACTUATOR_EXEC_Z_VEL +#define ACTUATOR_EXEC_Z_VEL 50 +#endif +#ifndef ACTUATOR_EXEC_Z_ACC +#define ACTUATOR_EXEC_Z_ACC 100 +#endif + +/** 运动到位等待裕量 (ms): 无到位反馈, 在计算运动时间上叠加的安全余量 */ +#ifndef ACTUATOR_SETTLE_MARGIN_MS +#define ACTUATOR_SETTLE_MARGIN_MS 400 +#endif + +/** 舵机动作 / 磁铁吸合后的等待时间 (ms) */ +#ifndef ACTUATOR_GRIP_SETTLE_MS +#define ACTUATOR_GRIP_SETTLE_MS 300 #endif /** @@ -137,10 +158,10 @@ extern "C" { * 待机械测量后填入。 * ================================================================ */ #ifndef ACTUATOR_OFFSET_X_MM -#define ACTUATOR_OFFSET_X_MM 8.0f /* angle=0° 时工具末端偏右 ~8mm (实测) */ +#define ACTUATOR_OFFSET_X_MM 6.6f /* angle=0° 时工具末端偏右 ~8mm (实测) */ #endif #ifndef ACTUATOR_OFFSET_Y_MM -#define ACTUATOR_OFFSET_Y_MM -30.0f /* angle=0° 时工具末端偏上 ~30mm (实测) */ +#define ACTUATOR_OFFSET_Y_MM -35.75f /* angle=0° 时工具末端偏上 ~30mm (实测) */ #endif /** @@ -171,6 +192,39 @@ void actuator_init(void); */ void actuator_set_scale(float x_scale, float y_scale, float angle_scale); +/** + * @brief 按 Emm_V5 曲线加减速公式估算纯运动时间 (ms) + * @param pulses 脉冲数 (行程) + * @param vel 速度 RPM + * @param acc 加速度档位 0~255 (0=直接启动, 无曲线) + * @note 曲线公式: 每提升 1 RPM 耗时 (256-acc)*50us, 按梯形/三角形加减速估算 + */ +uint32_t actuator_motion_time_ms(uint32_t pulses, uint16_t vel, uint8_t acc); + +/** + * @brief 运动到位等待 = 估算运动时间 + 安全裕量 + * @param pulses 脉冲数 (行程) + * @param vel 速度 RPM + * @param acc 加速度档位 0~255 + */ +uint32_t actuator_settle_ms(uint32_t pulses, uint16_t vel, uint8_t acc); + +/** + * @brief 直线轴运动到位等待 (按实际 mm 距离换算脉冲) + * @param distance_mm 直线距离 (mm, 取绝对值) + * @param vel 速度 RPM + * @param acc 加速度档位 0~255 + */ +uint32_t actuator_mm_settle_ms(float distance_mm, uint16_t vel, uint8_t acc); + +/** + * @brief Z 旋转轴运动到位等待 (按实际角度换算脉冲) + * @param angle_deg 旋转角度 (°, 取绝对值) + * @param vel 速度 RPM + * @param acc 加速度档位 0~255 + */ +uint32_t actuator_angle_settle_ms(float angle_deg, uint16_t vel, uint8_t acc); + /** * @brief 设置 Z 旋转轴心 ↔ 工具末端 XY 偏移量 (mm) * @param offset_x_mm 工具末端相对于旋转中心的 X 偏移 (mm) diff --git a/main/main.c b/main/main.c index f23cae6..b7bdc50 100644 --- a/main/main.c +++ b/main/main.c @@ -38,9 +38,8 @@ static const char *TAG = "MAIN"; /* ---- 按钮识别任务 ---- */ #define RECOGNIZE_TIMEOUT_MS 20000 #define RECOGNIZE_POLL_MS 100 -/* 归零后等待电机运动到位的时间 (无到位反馈, 按最大行程 + 裕量给定时长) */ -#define HOME_Z_SETTLE_MS 1500 /* Z 回 0°: vel=50RPM, 最大 ~270° 约 0.9s */ -#define HOME_XY_SETTLE_MS 4000 /* XY 回零: vel=1500RPM, Y 最大 335mm 约 2s */ +/* 归零速度: XY 用 ACTUATOR_EXEC_VEL/ACC (3000/250), Z 用 ACTUATOR_EXEC_Z_VEL/Z_ACC (低速); + 等待时间按最大行程 + 速度/加速度实时计算 */ static void button_recognize_task(void *pvParam) { @@ -48,15 +47,28 @@ static void button_recognize_task(void *pvParam) if (key_is_pressed()) { ESP_LOGI(TAG, "按钮按下, 归零后再发送 RECOGNIZE ..."); - /* ---- 归零: 舵机抬起 + Z 回 0° ---- */ + /* ---- 归零: 舵机抬起 + Z 回 0° (低速, 最坏行程 270°) ---- */ actuator_servo_up(); - actuator_motor_angle(ACTUATOR_ADDR_ANGLE, 0.0f, 50, 100, true); - vTaskDelay(pdMS_TO_TICKS(HOME_Z_SETTLE_MS)); + actuator_motor_angle(ACTUATOR_ADDR_ANGLE, 0.0f, + ACTUATOR_EXEC_Z_VEL, ACTUATOR_EXEC_Z_ACC, true); + vTaskDelay(pdMS_TO_TICKS(actuator_angle_settle_ms( + ACTUATOR_ANGLE_LIMIT_DEG, + ACTUATOR_EXEC_Z_VEL, ACTUATOR_EXEC_Z_ACC))); - /* ---- 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(HOME_XY_SETTLE_MS)); + /* ---- XY 回零: X=0, Y=330 (按最坏行程实时计算等待) ---- */ + uint32_t xy_settle = actuator_mm_settle_ms(ACTUATOR_LIMIT_X_MAX, + ACTUATOR_EXEC_VEL, + ACTUATOR_EXEC_ACC); + uint32_t y_settle = actuator_mm_settle_ms(ACTUATOR_LIMIT_Y_MAX, + ACTUATOR_EXEC_VEL, + ACTUATOR_EXEC_ACC); + if (y_settle > xy_settle) xy_settle = y_settle; + + actuator_motor_mm(ACTUATOR_ADDR_X, 0.0f, + ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC, true); + actuator_motor_mm(ACTUATOR_ADDR_Y1, 330.0f, + ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC, true); + vTaskDelay(pdMS_TO_TICKS(xy_settle)); ESP_LOGI(TAG, "归零完成, 发送 RECOGNIZE ...");