diff --git a/components/actuator/actuator.c b/components/actuator/actuator.c index dd143a9..d152d9b 100644 --- a/components/actuator/actuator.c +++ b/components/actuator/actuator.c @@ -24,6 +24,9 @@ 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 轴当前命令位置 (度, 绝对), 初始与驱动零位一致; 用于行程内选近路 */ +static float s_cur_angle_deg = 0.0f; + /* Z 旋转轴心 → 工具末端 偏移补偿 (mm) */ static float s_offset_x_mm = ACTUATOR_OFFSET_X_MM; static float s_offset_y_mm = ACTUATOR_OFFSET_Y_MM; @@ -92,6 +95,8 @@ void actuator_init(void) } s_initialized = true; + /* Z 轴命令位置跟踪与驱动零位对齐 (重新初始化时复位) */ + s_cur_angle_deg = 0.0f; ESP_LOGI(TAG, "初始化完成 (Angle=addr%d X=addr%d Y=addr%d/%d, servo=GPIO%d)", ACTUATOR_ADDR_ANGLE, ACTUATOR_ADDR_X, @@ -186,6 +191,68 @@ static void _actuator_move_abs(uint8_t addr, uint8_t dir, uint32_t pulses, snF); /* 多机同步标志 */ } +/* ================================================================ + * 内部: Z 旋转轴角度运动 (绝对角, ±行程内选最近可达代表角) + * ================================================================ */ + +/** 在 ±limit 行程内选择与当前位置最近的等价角 (mod 360) */ +static float _closest_reachable_angle(float target_deg, float cur_deg, float limit) +{ + target_deg = fmodf(target_deg, 360.0f); + if (target_deg < 0.0f) + target_deg += 360.0f; + + float best = target_deg; + float best_dist = 1e30f; + for (int k = -1; k <= 1; k++) { + float rep = target_deg + 360.0f * (float)k; + if (rep < -limit || rep > limit) + continue; + float dist = fabsf(rep - cur_deg); + if (dist < best_dist - 1e-4f || + (fabsf(dist - best_dist) <= 1e-4f && rep > best)) { + best_dist = dist; + best = rep; + } + } + return best; +} + +/** Z 轴绝对运动到目标朝向 (用于 source 归位); 负角自动翻转方向, 并更新当前位置跟踪 */ +static void _actuator_move_angle_abs(float angle_deg, uint16_t vel, uint8_t acc) +{ + float target = _closest_reachable_angle(angle_deg, s_cur_angle_deg, + ACTUATOR_ANGLE_LIMIT_DEG); + uint8_t dir = ACTUATOR_DIR_ANGLE; + float abs_deg = fabsf(target); + uint32_t pulses = (uint32_t)(abs_deg * 10.0f * s_scale_angle + 0.5f); + if (target < 0.0f) + dir = (uint8_t)!dir; + + _actuator_move_abs(ACTUATOR_ADDR_ANGLE, dir, pulses, vel, acc, false); + s_cur_angle_deg = target; + ESP_LOGI(TAG, "Z 角度: 目标 %.1f° -> 最近可达 %.1f° (%lu pulses, %s)", + (double)angle_deg, (double)target, (unsigned long)pulses, + dir ? "CCW" : "CW"); +} + +/** Z 轴相对运动 delta (带符号度): 正负自动选方向, 语义明确, 用于片间旋转 */ +static void _actuator_move_angle_rel(float delta_deg, uint16_t vel, uint8_t acc) +{ + uint8_t dir = ACTUATOR_DIR_ANGLE; + float abs_deg = fabsf(delta_deg); + uint32_t pulses = (uint32_t)(abs_deg * 10.0f * s_scale_angle + 0.5f); + if (delta_deg < 0.0f) + dir = (uint8_t)!dir; + + Emm_V5_Pos_Control(ACTUATOR_ADDR_ANGLE, dir, vel, acc, pulses, + false, /* raF=false: 相对运动 */ + false); + s_cur_angle_deg += delta_deg; + ESP_LOGI(TAG, "Z 角度: 相对 %.1f° (%lu pulses, %s)", + (double)delta_deg, (unsigned long)pulses, dir ? "CCW" : "CW"); +} + /* ================================================================ * 内部: Y 轴双电机同步运动 * ================================================================ */ @@ -226,31 +293,34 @@ static void _actuator_move_fragment(const edc_fragment_t *frag) uint16_t tgt_y = frag->target.y; uint16_t tgt_angle = frag->target.angle_x10; - /* Z 旋转轴心偏移补偿 (仅对 target 做) */ + /* 协议角度: source=0 (保持当前), target=相对转角 [0,360); + 执行时自动选行程内与当前位置最近的等价角 */ + float src_angle_deg = (float)src_angle * 0.1f; float tgt_angle_deg = (float)tgt_angle * 0.1f; + + /* Z 旋转轴心偏移补偿 (仅对 target 做, 与旧协议行为保持一致) */ actuator_apply_offset_compensation(&tgt_x, &tgt_y, tgt_angle_deg); /* source 坐标 → 脉冲数 */ 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)(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 comp x=%u y=%u)", - src_pulses_x, src_pulses_y, src_pulses_angle, - dst_pulses_x, dst_pulses_y, dst_pulses_angle, + ESP_LOGI(TAG, "Fragment: SRC(X=%lu Y=%lu A=%.1f) -> DST(X=%lu Y=%lu A=%.1f comp x=%u y=%u)", + (unsigned long)src_pulses_x, (unsigned long)src_pulses_y, + (double)src_angle_deg, + (unsigned long)dst_pulses_x, (unsigned long)dst_pulses_y, + (double)tgt_angle_deg, tgt_x, tgt_y); /* ---- 1. 磁铁掉电 ---- */ magnet_set(false); /* ---- 2. 运动到 source 位置 ---- */ - _actuator_move_abs(ACTUATOR_ADDR_ANGLE, ACTUATOR_DIR_ANGLE, - src_pulses_angle, vel_angle, acc_angle, false); + _actuator_move_angle_abs(src_angle_deg, vel_angle, acc_angle); _frame_gap(); _actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X, src_pulses_x, vel_xy, acc_xy, false); @@ -273,8 +343,11 @@ static void _actuator_move_fragment(const edc_fragment_t *frag) _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); + /* 目标角度取行程内与当前位置最近的等价角, 再用相对运动执行: + 终点朝向与旧协议 (绝对转到 rotate mod 360) 完全一致, 且不会绕远路 */ + float dst_rep = _closest_reachable_angle(tgt_angle_deg, s_cur_angle_deg, + ACTUATOR_ANGLE_LIMIT_DEG); + _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, dst_pulses_x, vel_xy, acc_xy, false); @@ -480,6 +553,9 @@ void actuator_motor_angle(uint8_t addr, float angle_deg, uint16_t vel, addr, peer, (double)angle_deg, raF ? "绝对" : "相对", vel); } else { + /* 跟踪 Z 轴命令位置 (供 fragment 行程内近路选择) */ + if (addr == ACTUATOR_ADDR_ANGLE) + s_cur_angle_deg = raF ? angle_deg : (s_cur_angle_deg + angle_deg); Emm_V5_Pos_Control_Angle(addr, dir, vel, acc, angle_deg, raF, false); ESP_LOGI(TAG, "角度: 电机%d %.1f° %s vel=%dRPM", addr, (double)angle_deg, diff --git a/components/actuator/actuator.h b/components/actuator/actuator.h index 223afb3..1244ea6 100644 --- a/components/actuator/actuator.h +++ b/components/actuator/actuator.h @@ -101,6 +101,11 @@ extern "C" { #define ACTUATOR_LIMIT_Y_MIN 0.0f #define ACTUATOR_LIMIT_Y_MAX 335.0f +/** Z 旋转轴软限位 (±°, 机械行程) */ +#ifndef ACTUATOR_ANGLE_LIMIT_DEG +#define ACTUATOR_ANGLE_LIMIT_DEG 270.0f +#endif + /** Fragment 间等待 (ms), 暂用固定延时替代到位检测 */ #ifndef ACTUATOR_FRAG_DELAY_MS #define ACTUATOR_FRAG_DELAY_MS 500 @@ -132,10 +137,10 @@ extern "C" { * 待机械测量后填入。 * ================================================================ */ #ifndef ACTUATOR_OFFSET_X_MM -#define ACTUATOR_OFFSET_X_MM 8.0f /* angle=0° 时工具末端偏右 ~8mm */ +#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 */ +#define ACTUATOR_OFFSET_Y_MM -30.0f /* angle=0° 时工具末端偏上 ~30mm (实测) */ #endif /** diff --git a/components/protocol/PROTOCOL.md b/components/protocol/PROTOCOL.md index 9c835d3..b0193d6 100644 --- a/components/protocol/PROTOCOL.md +++ b/components/protocol/PROTOCOL.md @@ -23,6 +23,10 @@ Cycle = { cycle_id: uint16, frag_count: uint8 (2~4), fragments: Fragment[] ``` - **Point**: 单个坐标点, `angle_x10 = 角度 × 10` (如 `450` = 45.0°) +- **角度语义**: `source.angle = 0` 表示保持当前朝向; `target.angle` 为搬运 + 过程中的**相对转角**, 归一化到 `[0, 360.0)` 度 (如 -20° 编码为 3400)。 + MCU 执行 fragment 时在 ±270° 行程内为 target 选择与当前位置最近的等价角 + (mod 360°), 就近旋转, 避免把 -20° 当成 +340° 绕远路 - **Fragment**: 一次运动片段, 从 `source` 移动到 `target` - **Cycle**: 一个执行周期, 包含 2~4 个 Fragment @@ -98,10 +102,10 @@ XX XX ← CRC16 |------|-----|------| | cycle_id | `0x0001` = 1 | 第 1 个 Cycle | | frag_count | `0x02` = 2 | 2 个 Fragment | -| F0 src | (100, 200), 45.0° | | -| F0 dst | (300, 400), 60.0° | | -| F1 src | (150, 300), 90.0° | | -| F1 dst | (500, 700), 120.0° | | +| F0 src | (100, 200), 45.0° | 示例: 抓取点 (角度字段保持当前) | +| F0 dst | (300, 400), 60.0° | 示例: 放置点 + 相对转角 60.0° | +| F1 src | (150, 300), 90.0° | 示例: 抓取点 (角度字段保持当前) | +| F1 dst | (500, 700), 120.0° | 示例: 放置点 + 相对转角 120.0° | ### 4.2 CYCLE_DONE (MCU → MPU) diff --git a/main/main.c b/main/main.c index 575c2c1..f23cae6 100644 --- a/main/main.c +++ b/main/main.c @@ -38,6 +38,9 @@ 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 */ static void button_recognize_task(void *pvParam) { @@ -48,12 +51,12 @@ static void button_recognize_task(void *pvParam) /* ---- 归零: 舵机抬起 + Z 回 0° ---- */ actuator_servo_up(); actuator_motor_angle(ACTUATOR_ADDR_ANGLE, 0.0f, 50, 100, true); - vTaskDelay(pdMS_TO_TICKS(500)); + vTaskDelay(pdMS_TO_TICKS(HOME_Z_SETTLE_MS)); /* ---- 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)); + vTaskDelay(pdMS_TO_TICKS(HOME_XY_SETTLE_MS)); ESP_LOGI(TAG, "归零完成, 发送 RECOGNIZE ..."); @@ -136,4 +139,3 @@ void app_main(void) vTaskDelay(pdMS_TO_TICKS(500)); } } -