EDC/edc-mcu
3
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

releasev1.2

Browse Source
This commit is contained in:
kirl committed 2026-08-03 04:09:26 +08:00
1 parent 74010ef611
commit c296caa328
10 files changed
+693 -267
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

components/actuator/actuator.c
+289 -130
View File
@@ -28,6 +28,11 @@ static float s_scale_angle = ACTUATOR_SCALE_ANGLE;
/* Z 轴当前命令位置 (度, 绝对), 初始与驱动零位一致; 用于行程内选近路 */
static float s_cur_angle_deg = 0.0f;
/* X/Y 当前绝对位置跟踪 (脉冲, 原点=0): 用于按“当前→目标”真实行程
计算到位掩码与超时基准, 而不是按绝对坐标 (原点) 估算 */
static uint32_t s_cur_x_pulses = 0;
static uint32_t s_cur_y_pulses = 0;
/* Z 旋转轴心 → 工具末端 偏移补偿 (mm) */
static float s_offset_x_mm = ACTUATOR_OFFSET_X_MM;
static float s_offset_y_mm = ACTUATOR_OFFSET_Y_MM;
@@ -70,23 +75,22 @@ static void _wait_flash(uint32_t ms, uint8_t r, uint8_t g, uint8_t b, uint32_t p
rgb_off();
}
/* 到位等待 (闭环反馈)
/* 到位等待 (事件驱动 + 预估时间兜底)
*
* 实测发现这台 X42S 的 S_FLAG Prf_TF 到位标志只有 X 轴会置位 (0x03),
* Z/Y1/Y2 始终为 0x01 (仅使能位), 轮询 Prf_TF 无法判定到位。
* 因此改为直接比对电机实时位置 (S_CPOS) 与目标位置 (S_TPOS):
* 偏差 < ACTUATOR_ARRIVAL_TOL_RAW 即认为到位 —— 不依赖 Prf_TF。
* 每轮重新读 S_TPOS (电机处理命令后目标才更新, 首轮可能是旧目标)。
* 需连续 2 次在容差内才确认: 确保电机已稳定停下 (减速途中/振荡时
* 位置可能短暂进入容差, 单独一次会"没到位就进行下一步")。
* 电机驱动 Response 菜单已设为 Reached: 位置模式 (0xFD) 命令运动到位后
* 会主动回传到位帧 (地址 + FD + 9F + 6B)。UART RX 由 emm_v5 内部任务
* 中断读取并解析, 置位对应到位位; 这里事件等待全部到位, 不再轮询
* S_TPOS/S_CPOS, 从根本上避免"前一个动作没到位、下一个动作就执行"。
*
* ⚠️ 防坑: 轮询第一帧前必须留帧间隔 —— 运动命令最后一条是 Y 同步广播
* 触发 (00 FF 66 6B), 若紧跟发轮询帧会"零间隔背靠背", 驱动板丢字节
* 吞掉触发帧, 电机不启动。
* 兜底: 同时按曲线加减速估算纯运动时间 (base_wait_ms), 超时 = 估算
* + ACTUATOR_SETTLE_MARGIN_MS; 若超过预估过多 (到位帧丢失/电机异常)
* 仍未到位, 打印告警并返回 false, 上层继续执行后续动作, 不卡死。
*
* 等待期间红灯快闪反馈 "运动中/未到位"。
*
* @param base_wait_ms 纯运动时间估算 (兜底超时基准)
* @param moved_mask 实际有行程的电机位掩码 (bit0=Z, bit1=X, bit2/3=Y1/Y2)
* @return true=全部到位, false=超时
* @return true=全部到位, false=超时 (调用方应继续后续动作)
*/
bool actuator_wait_arrival(uint32_t base_wait_ms, uint32_t moved_mask)
{
@@ -94,85 +98,65 @@ bool actuator_wait_arrival(uint32_t base_wait_ms, uint32_t moved_mask)
ACTUATOR_ADDR_ANGLE, ACTUATOR_ADDR_X,
ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2
};
bool arrived[4] = {false, false, false, false};
uint8_t set_cnt[4] = {0, 0, 0, 0}; /* 连续读到在容差内的次数 (≥2 确认已停稳) */
int32_t last_cpos[4] = {-1, -1, -1, -1}; /* 诊断 */
int32_t last_tpos[4] = {-1, -1, -1, -1}; /* 诊断 */
uint32_t polls = 0; /* 诊断: 轮询轮数 */
uint32_t timeout_ms = base_wait_ms + ACTUATOR_SETTLE_MARGIN_MS;
int64_t deadline = esp_timer_get_time() + (int64_t)timeout_ms * 1000;
uint8_t wait_addrs[4];
uint8_t wait_count = 0;
for (int i = 0; i < 4; i++) {
if (!(moved_mask & (1u << i)))
arrived[i] = true; /* 无行程: 视为已到位 */
if (moved_mask & (1u << i))
wait_addrs[wait_count++] = addrs[i];
}
if (wait_count == 0)
return true; /* 无行程: 视为已到位 */
/* 与上一条运动命令 (尤其 Y 同步触发) 之间留帧间隔, 防丢字节 */
vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS));
/* 兜底超时 = 预估纯运动时间 + 裕量 */
uint32_t timeout_ms = base_wait_ms + ACTUATOR_SETTLE_MARGIN_MS;
int64_t start = esp_timer_get_time();
int64_t deadline = start + (int64_t)timeout_ms * 1000;
while (esp_timer_get_time() < deadline) {
bool all = true;
for (int i = 0; i < 4; i++) {
if (arrived[i])
continue;
ESP_LOGI(TAG, "等待到位: mask=0x%02X (%u 轴: %u/%u/%u/%u), 超时=%u ms (预估 %u + 裕量 %u)",
moved_mask, wait_count,
wait_addrs[0], wait_count > 1 ? wait_addrs[1] : 0,
wait_count > 2 ? wait_addrs[2] : 0, wait_count > 3 ? wait_addrs[3] : 0,
timeout_ms, base_wait_ms, ACTUATOR_SETTLE_MARGIN_MS);
bool at_target = false;
int32_t tpos = Emm_V5_Read_TPOS(addrs[i]);
last_tpos[i] = tpos;
if (tpos >= 0) {
int32_t cpos = Emm_V5_Read_CPOS(addrs[i]);
last_cpos[i] = cpos;
if (cpos >= 0) {
int32_t diff = (int32_t)((uint16_t)cpos - (uint16_t)tpos);
if (diff < 0) diff = -diff;
if (diff > 32768) diff = 65536 - diff; /* 环形距离 */
at_target = (diff <= (int32_t)ACTUATOR_ARRIVAL_TOL_RAW);
}
} else {
/* 目标位置读失败: 退回 Prf_TF 到位标志 (仅 X 可靠) */
uint8_t flags = Emm_V5_Read_Status_Flag(addrs[i], 50);
at_target = (flags != 0xFF && (flags & 0x02));
}
if (at_target) {
/* 连续 2 次在容差内才确认: 电机已稳定停下,
防止减速途中/振荡时误判"到位"提前进入下一步 */
if (++set_cnt[i] >= 2) {
arrived[i] = true;
} else {
all = false;
}
} else {
set_cnt[i] = 0;
all = false;
}
vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS)); /* 帧间隔防粘包 */
bool ok = false;
while (1) {
int64_t left_us = deadline - esp_timer_get_time();
if (left_us <= 0)
break; /* 超过预估过多: 停止等待 */
uint32_t left_ms = (uint32_t)(left_us / 1000);
/* ⚠️ left_us 单位是微秒, 必须先转毫秒再用 (曾把 µs 当 ms → 每次等 40 秒) */
uint32_t chunk = (left_ms > 40000) ? 40000 : left_ms;
if (chunk == 0) chunk = 1;
if (Emm_V5_Wait_Arrival(wait_addrs, wait_count, chunk)) {
ok = true;
break;
}
polls++;
if (all)
return true;
/* 诊断: 前 3 轮打印各电机位置 (调试用, 定位后删除) */
if (polls <= 3) {
ESP_LOGI(TAG, "到位轮询#%u: cpos Z=%ld X=%ld Y1=%ld Y2=%ld "
"tpos Z=%ld X=%ld Y1=%ld Y2=%ld",
polls,
(long)last_cpos[0], (long)last_cpos[1], (long)last_cpos[2], (long)last_cpos[3],
(long)last_tpos[0], (long)last_tpos[1], (long)last_tpos[2], (long)last_tpos[3]);
}
/* 红灯快闪反馈 (与旧 _wait_flash 一致) */
ESP_LOGD(TAG, "到位等待中: 剩余 %u ms, 已到位位=0x%02X",
left_ms, Emm_V5_Arrival_Get_Bits());
/* 红灯快闪: 等待中 */
rgb_set(128, 0, 0);
vTaskDelay(pdMS_TO_TICKS(10));
rgb_off();
vTaskDelay(pdMS_TO_TICKS(10));
}
ESP_LOGW(TAG, "到位超时 (%u ms, %u 轮): cpos Z=%ld X=%ld Y1=%ld Y2=%ld "
"tpos Z=%ld X=%ld Y1=%ld Y2=%ld",
timeout_ms, polls,
(long)last_cpos[0], (long)last_cpos[1], (long)last_cpos[2], (long)last_cpos[3],
(long)last_tpos[0], (long)last_tpos[1], (long)last_tpos[2], (long)last_tpos[3]);
/* 最后再查一次 (可能刚好到位), 并消费本段 motion 的到位位 */
if (!ok)
ok = Emm_V5_Wait_Arrival(wait_addrs, wait_count, 0);
int64_t elapsed_ms = (esp_timer_get_time() - start) / 1000;
uint32_t got_bits = Emm_V5_Arrival_Get_Bits();
Emm_V5_Arrival_Consume(wait_addrs, wait_count);
if (ok) {
ESP_LOGI(TAG, "到位 (mask=0x%02X, %u 轴, 实际等待 %lld ms)",
moved_mask, wait_count, elapsed_ms);
return true;
}
ESP_LOGW(TAG, "到位超时 (%u ms = 预估 %u + 裕量 %u), mask=0x%02X, 实际已等 %lld ms, 已到位位=0x%02X, 继续后续动作",
timeout_ms, base_wait_ms, ACTUATOR_SETTLE_MARGIN_MS, moved_mask,
elapsed_ms, got_bits);
return false;
}
@@ -253,8 +237,8 @@ void actuator_init(void)
/* 初始化舵机 PWM */
servo_init(ACTUATOR_SERVO_GPIO);
/* 配置舵机限位: 抬起=80°, 放下=160° */
servo_set_limits(0.0f, 95.0f);
/* 配置舵机限位: 抬起=0°, 放下=103° */
servo_set_limits(0.0f, 102.0f);
/* 上电默认归位到抬起状态 */
servo_set_up();
@@ -273,8 +257,10 @@ void actuator_init(void)
}
s_initialized = true;
/* Z 轴命令位置跟踪与驱动零位对齐 (重新初始化时复位) */
/* 位置跟踪与驱动零位对齐 (重新初始化时复位) */
s_cur_angle_deg = 0.0f;
s_cur_x_pulses = 0;
s_cur_y_pulses = 0;
ESP_LOGI(TAG, "初始化完成 (Angle=addr%d X=addr%d Y=addr%d/%d, servo=GPIO%d)",
ACTUATOR_ADDR_ANGLE,
ACTUATOR_ADDR_X,
@@ -301,12 +287,18 @@ void actuator_set_scale(float x_scale, float y_scale, float angle_scale)
* 终端执行机构与 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θ)
* ⚠️ 实际语义 (2026-08-03): MPU 下发的 source/target 来自 calibrated_H,
* 本身是"旋转中心"坐标 —— θ=0 时中心到坐标点, 工具末端即落在
* D = 坐标点 + (dx,dy) (即视觉目标物点; 放置修复后实测验证)。
* 因此要把工具末端放到 D, 中心需去:
* center = D - R(θ)·(dx,dy) = 坐标 + (dx,dy) - R(θ)·(dx,dy)
* 即:
* X_motor = X + dx - (dx·cosθ - dy·sinθ)
* Y_motor = Y + dy - (dx·sinθ + dy·cosθ)
* source 与 target 都必须补偿 (source 不补偿时抓取点会随 Z 角偏移,
* 见 _actuator_move_fragment 调用处注释)。
* 旧公式 center = 坐标 - R(θ)·(dx,dy) 少了 "+ (dx,dy)" 项, 会把工具
* 末端整体放偏 (dx,dy) = (6.6,-35.75)mm (y 偏大 35.75mm)。
* ================================================================ */
void actuator_set_offset(float offset_x_mm, float offset_y_mm)
@@ -363,6 +355,31 @@ void actuator_apply_offset_compensation(uint16_t *target_x, uint16_t *target_y,
*target_x, *target_y);
}
/* ================================================================
* 内部: 位置跟踪 (到位掩码 / 超时按真实行程计算)
* ================================================================ */
/* X/Y 轴: 绝对运动直接置为 pulses, 相对运动按 dir 增减 (钳位 ≥0) */
static void _track_xy_axis(bool is_y, uint8_t dir, uint32_t pulses, bool raF)
{
uint32_t *cur = is_y ? &s_cur_y_pulses : &s_cur_x_pulses;
uint8_t pos_dir = is_y ? ACTUATOR_DIR_Y : ACTUATOR_DIR_X;
if (raF) {
*cur = pulses;
} else {
*cur = (dir == pos_dir) ? (*cur + pulses)
: (*cur > pulses ? *cur - pulses : 0);
}
}
/* Z 旋转轴: 脉冲数 → 角度跟踪 (绝对置位 / 相对按 dir 增减) */
static void _track_angle_pulses(uint8_t dir, uint32_t pulses, bool raF)
{
float deg = (float)pulses / (10.0f * s_scale_angle);
s_cur_angle_deg = raF ? deg
: (s_cur_angle_deg + (dir == ACTUATOR_DIR_ANGLE ? deg : -deg));
}
/* ================================================================
* 内部: 单轴绝对位置运动
* ================================================================ */
@@ -373,7 +390,13 @@ static void _actuator_move_abs(uint8_t addr, uint8_t dir, uint32_t pulses,
Emm_V5_Pos_Control(addr, dir, vel, acc,
pulses,
true, /* raF=true: 绝对运动 */
snF); /* 多机同步标志 */
snF);
/* 更新 XY 位置跟踪 (Z 角度由 _actuator_move_angle_rel 单独跟踪) */
if (addr == ACTUATOR_ADDR_X)
_track_xy_axis(false, dir, pulses, true);
else if (addr == ACTUATOR_ADDR_Y1 || addr == ACTUATOR_ADDR_Y2)
_track_xy_axis(true, dir, pulses, true);
}
/* ================================================================
@@ -403,25 +426,8 @@ static float _closest_reachable_angle(float target_deg, float cur_deg, float lim
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 (带符号度): 正负自动选方向, 语义明确, 用于片间旋转 */
/** Z 轴相对运动 delta (带符号度): 正负自动选方向, 语义明确, 用于片间旋转;
⚠️ 一律用相对运动 —— 实测电机1 对“绝对 clk=0”命令不回传到位帧 (会超时) */
static void _actuator_move_angle_rel(float delta_deg, uint16_t vel, uint8_t acc)
{
uint8_t dir = ACTUATOR_DIR_ANGLE;
@@ -438,6 +444,34 @@ static void _actuator_move_angle_rel(float delta_deg, uint16_t vel, uint8_t acc)
(double)delta_deg, (unsigned long)pulses, dir ? "CCW" : "CW");
}
/** Z 轴回 0° 到位确认: 电机1 (Z 轴) 对“终点靠近 0°”的移动不回传到位帧,
改用 Emm_V5_Read_CPOS 轮询实际位置确认 (0~65535 对应 0~360°, 0° 可能读
0 或 65535 负向回绕)。Z 回零移动小, 通常已被 X/Y 事件等待覆盖, 首次即返回。
@return true=已到 0° 附近 (容差 tol_deg) */
static bool _wait_z_at_zero(uint32_t timeout_ms, float tol_deg)
{
uint32_t tol_pulses = (uint32_t)(tol_deg / 360.0f * 65535.0f);
int64_t deadline = esp_timer_get_time() + (int64_t)timeout_ms * 1000;
int32_t last = -1;
while (1) {
int32_t cpos = Emm_V5_Read_CPOS(ACTUATOR_ADDR_ANGLE);
if (cpos >= 0) {
last = cpos;
uint32_t p = (uint32_t)cpos;
if (p <= tol_pulses || p >= 65535u - tol_pulses) {
ESP_LOGI(TAG, "Z 回 0 确认 (CPOS=%ld)", (long)cpos);
return true;
}
}
if (esp_timer_get_time() >= deadline)
break;
vTaskDelay(pdMS_TO_TICKS(20));
}
ESP_LOGW(TAG, "Z 回 0 轮询超时 (%u ms), 最后 CPOS=%ld, 继续",
timeout_ms, (long)last);
return false;
}
/* ================================================================
* 内部: Y 轴双电机同步运动
* ================================================================ */
@@ -484,7 +518,14 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
float src_angle_deg = (float)src_angle * 0.1f;
float tgt_angle_deg = (float)tgt_angle * 0.1f;
/* Z 旋转轴心偏移补偿 (仅对 target 做, 与旧协议行为保持一致) */
/* Z 旋转轴心偏移补偿 —— source 与 target 都做 (2026-08-03):
MPU 下发 source/target 均为"旋转中心"坐标 (θ=0 时吸盘在坐标+(dx,dy))。
若 source 不补偿, 只要抓取瞬间 Z 角 ≠ 0° (前一片放置转角/归零不到位/
超行程卡位等), 吸盘落点就会偏移 (R(θ)-I)·(dx,dy), 其中 y 分量
(dy=35.75mm) 主导 → 表现为"设置的抓取点 vs 实际抓取点偏 y"。
补偿后吸盘恒 = source+(dx,dy) = 视觉目标点, 与抓取时 Z 角无关;
θ=0° 时补偿为恒等, 对原有行为无影响。 */
actuator_apply_offset_compensation(&src_x, &src_y, src_angle_deg);
actuator_apply_offset_compensation(&tgt_x, &tgt_y, tgt_angle_deg);
/* source 坐标 → 脉冲数 */
@@ -502,27 +543,43 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
(double)tgt_angle_deg,
tgt_x, tgt_y);
/* 到位轮询掩码: 只等待实际有行程的电机 (脉冲>0), 无行程直接视为已到位 */
/* 到位轮询掩码与超时基准: 一律按“当前实际位置 → 目标”的真实行程计算,
而不是按绝对坐标 (原点) 估算。绝对坐标会把已在目标位/离原点很远的轴
算成大行程 → 无行程也等满超时 (初始==目标时整段空等), 且超时被放大 */
float src_target = _closest_reachable_angle(src_angle_deg, s_cur_angle_deg,
ACTUATOR_ANGLE_LIMIT_DEG);
uint32_t src_pulses_z = (uint32_t)(fabsf(src_target) * 10.0f * s_scale_angle + 0.5f);
uint32_t src_mask = 0;
if (src_pulses_z > 0) src_mask |= (1u << 0); /* Z 旋转 */
if (src_pulses_x > 0) src_mask |= (1u << 1); /* X */
if (src_pulses_y > 0) src_mask |= (1u << 2) | (1u << 3); /* Y 双电机 */
/* 各轴真实行程 (脉冲): Z 终点=0° 时电机1 不回传到位帧,
不纳入事件等待, 改为之后用 CPOS 轮询确认 */
float src_z_tgt = src_target;
uint32_t src_dz = (uint32_t)(fabsf(src_z_tgt - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f);
uint32_t src_dx = (src_pulses_x > s_cur_x_pulses) ? (src_pulses_x - s_cur_x_pulses)
: (s_cur_x_pulses - src_pulses_x);
uint32_t src_dy = (src_pulses_y > s_cur_y_pulses) ? (src_pulses_y - s_cur_y_pulses)
: (s_cur_y_pulses - src_pulses_y);
/* 纯运动时间 (无裕量), 仅作到位等待的兜底超时基准 */
uint32_t src_wait_ms = actuator_motion_time_ms(src_pulses_z, vel_angle, acc_angle);
uint32_t t_wait = actuator_motion_time_ms(src_pulses_x, vel_xy, acc_xy);
uint32_t src_mask = 0;
if (src_dz > 0 && src_z_tgt != 0.0f) src_mask |= (1u << 0); /* Z 旋转 */
if (src_dx > 0) src_mask |= (1u << 1); /* X */
if (src_dy > 0) src_mask |= (1u << 2) | (1u << 3); /* Y 双电机 */
/* 纯运动时间 (按真实行程, 无裕量), 仅作到位等待的兜底超时基准 */
uint32_t src_wait_ms = actuator_motion_time_ms(src_dz, vel_angle, acc_angle);
uint32_t t_wait = actuator_motion_time_ms(src_dx, vel_xy, acc_xy);
if (t_wait > src_wait_ms) src_wait_ms = t_wait;
t_wait = actuator_motion_time_ms(src_pulses_y, vel_xy, acc_xy);
t_wait = actuator_motion_time_ms(src_dy, vel_xy, acc_xy);
if (t_wait > src_wait_ms) src_wait_ms = t_wait;
ESP_LOGI(TAG, "source 段行程: dZ=%lu dX=%lu dY=%lu 脉冲, mask=0x%02X, 等待超时=%u ms",
(unsigned long)src_dz, (unsigned long)src_dx, (unsigned long)src_dy,
src_mask, src_wait_ms + ACTUATOR_SETTLE_MARGIN_MS);
/* ---- 1. 磁铁掉电 ---- */
magnet_set(false);
/* ---- 2. 运动到 source 位置 ---- */
_actuator_move_angle_abs(src_angle_deg, vel_angle, acc_angle);
Emm_V5_Arrival_Reset(); /* 清掉上一段迟到/多余的到位帧, 防污染本段等待 */
/* source Z 用相对运动转到目标朝向 (0° 基准位; 到位用 CPOS 轮询确认) */
_actuator_move_angle_rel(src_z_tgt - s_cur_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);
@@ -532,6 +589,10 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
/* ---- 3. 等待运动到位 (轮询闭环到位标志, 红灯快闪) ---- */
actuator_wait_arrival(src_wait_ms, src_mask);
/* Z 终点=0°: 不回传到位帧 → 已排除出事件等待, 用 CPOS 轮询确认到位 */
if (src_dz > 0 && src_z_tgt == 0.0f)
_wait_z_at_zero(ACTUATOR_Z_CONFIRM_MS, ACTUATOR_Z_CONFIRM_TOL_DEG);
/* ---- 4. 舵机放下 ---- */
servo_set_down();
_wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);
@@ -545,23 +606,36 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
_wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);
/* ---- 7. 运动到 target 位置 ---- */
Emm_V5_Arrival_Reset(); /* 清掉上一段迟到/多余的到位帧, 防污染本段等待 */
/* 目标角度取行程内与当前位置最近的等价角, 再用相对运动执行:
终点朝向与旧协议 (绝对转到 rotate mod 360) 完全一致, 且不会绕远路 */
float dst_rep = _closest_reachable_angle(tgt_angle_deg, s_cur_angle_deg,
ACTUATOR_ANGLE_LIMIT_DEG);
uint32_t dst_pulses_z = (uint32_t)(fabsf(dst_rep - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f);
/* 真实行程: 当前 (source 终点) → target; 无行程的轴不等待
Z 终点=0° (相对转角恰为 0) 时不纳入事件等待, 用 CPOS 轮询确认 */
float dst_z_tgt = dst_rep;
uint32_t dst_dz = (uint32_t)(fabsf(dst_z_tgt - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f);
uint32_t dst_dx = (dst_pulses_x > s_cur_x_pulses) ? (dst_pulses_x - s_cur_x_pulses)
: (s_cur_x_pulses - dst_pulses_x);
uint32_t dst_dy = (dst_pulses_y > s_cur_y_pulses) ? (dst_pulses_y - s_cur_y_pulses)
: (s_cur_y_pulses - dst_pulses_y);
uint32_t dst_mask = 0;
if (dst_pulses_z > 0) dst_mask |= (1u << 0);
if (dst_pulses_x > 0) dst_mask |= (1u << 1);
if (dst_pulses_y > 0) dst_mask |= (1u << 2) | (1u << 3);
if (dst_dz > 0 && dst_z_tgt != 0.0f) dst_mask |= (1u << 0);
if (dst_dx > 0) dst_mask |= (1u << 1);
if (dst_dy > 0) dst_mask |= (1u << 2) | (1u << 3);
uint32_t dst_wait_ms = actuator_motion_time_ms(dst_pulses_z, vel_angle, acc_angle);
t_wait = actuator_motion_time_ms(dst_pulses_x, vel_xy, acc_xy);
uint32_t dst_wait_ms = actuator_motion_time_ms(dst_dz, vel_angle, acc_angle);
t_wait = actuator_motion_time_ms(dst_dx, vel_xy, acc_xy);
if (t_wait > dst_wait_ms) dst_wait_ms = t_wait;
t_wait = actuator_motion_time_ms(dst_pulses_y, vel_xy, acc_xy);
t_wait = actuator_motion_time_ms(dst_dy, 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);
ESP_LOGI(TAG, "target 段行程: dZ=%lu dX=%lu dY=%lu 脉冲, mask=0x%02X, 等待超时=%u ms",
(unsigned long)dst_dz, (unsigned long)dst_dx, (unsigned long)dst_dy,
dst_mask, dst_wait_ms + ACTUATOR_SETTLE_MARGIN_MS);
_actuator_move_angle_rel(dst_z_tgt - 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);
@@ -571,6 +645,10 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
/* ---- 8. 等待运动到位 (轮询闭环到位标志, 红灯快闪) ---- */
actuator_wait_arrival(dst_wait_ms, dst_mask);
/* Z 终点=0°: 用 CPOS 轮询确认到位 */
if (dst_dz > 0 && dst_z_tgt == 0.0f)
_wait_z_at_zero(ACTUATOR_Z_CONFIRM_MS, ACTUATOR_Z_CONFIRM_TOL_DEG);
/* ---- 9. 舵机放下 ---- */
servo_set_down();
_wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);
@@ -646,7 +724,10 @@ void edc_proto_on_cycle(const edc_cycle_t *cycle, edc_channel_t ch)
ESP_LOGI(TAG, "开始执行 Cycle #%u ...", cycle->cycle_id);
actuator_execute_cycle(cycle);
/* 全部 Fragment 执行完成后回复 DONE */
/* 完成后执行机构置零 (归位): 舵机抬起 + Z/X/Y 三轴回零点 */
actuator_home_all();
/* 全部执行完毕 (含置零) 后回复 DONE */
edc_proto_send_done(cycle->cycle_id);
ESP_LOGI(TAG, "Cycle #%u 执行完毕, DONE 已发送", cycle->cycle_id);
@@ -740,11 +821,16 @@ void actuator_motor_pos(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc,
Emm_V5_Pos_Control(peer, dir, vel, acc, clk, raF, true);
_frame_gap();
Emm_V5_Synchronous_motion(0);
_track_xy_axis(true, dir, clk, raF); /* 更新位置跟踪 */
ESP_LOGI(TAG, "位置(Y同步): 电机%d+%d %s %dRPM %lu脉冲 %s",
addr, peer, dir ? "CCW" : "CW", vel, clk,
raF ? "绝对" : "相对");
} else {
Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, false);
if (addr == ACTUATOR_ADDR_X)
_track_xy_axis(false, dir, clk, raF);
else if (addr == ACTUATOR_ADDR_ANGLE)
_track_angle_pulses(dir, clk, raF);
ESP_LOGI(TAG, "位置: 电机%d %s %dRPM %lu脉冲 %s",
addr, dir ? "CCW" : "CW", vel, clk,
raF ? "绝对" : "相对");
@@ -770,9 +856,14 @@ 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)
/* 跟踪命令位置 (供 fragment 真实行程计算) */
if (addr == ACTUATOR_ADDR_ANGLE) {
s_cur_angle_deg = raF ? angle_deg : (s_cur_angle_deg + angle_deg);
} else if (addr == ACTUATOR_ADDR_X) {
/* X 轴按角度手动控制: 换算脉冲后更新跟踪 (负角内部翻转方向) */
uint8_t eff_dir = (angle_deg < 0.0f) ? (uint8_t)!dir : dir;
_track_xy_axis(false, eff_dir, EMM_V5_DEG_TO_CLK(fabsf(angle_deg)), raF);
}
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,
@@ -805,11 +896,14 @@ void actuator_motor_mm(uint8_t addr, float distance_mm, uint16_t vel,
Emm_V5_Pos_Control(peer, dir, vel, acc, clk, raF, true);
_frame_gap();
Emm_V5_Synchronous_motion(0);
_track_xy_axis(true, dir, clk, raF); /* 更新位置跟踪 */
ESP_LOGI(TAG, "直线(Y同步 %s): 电机%d+%d %.2fmm (%lu脉冲) %s vel=%dRPM",
dir ? "CCW" : "CW", addr, peer, (double)distance_mm,
(unsigned long)clk, raF ? "绝对" : "相对", vel);
} else {
Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, false);
if (addr == ACTUATOR_ADDR_X)
_track_xy_axis(false, dir, clk, raF);
ESP_LOGI(TAG, "直线(%s): 电机%d %.2fmm (%lu脉冲) %s vel=%dRPM",
dir ? "CCW" : "CW", addr, (double)distance_mm,
(unsigned long)clk, raF ? "绝对" : "相对", vel);
@@ -832,6 +926,71 @@ void actuator_motor_stop(uint8_t addr)
}
}
/* ================================================================
* 执行机构置零 (归位)
*
* 舵机抬起 + Z/X/Y 三轴并行回零点。Z 旋转与 X/Y 平移互不干扰
* (Fragment 里也是三轴并行), 无需等 Z 到位再动 X/Y —— 三轴同时
* 下发、一次到位等待。Cycle 执行完成后与按钮识别前都会调用。
* ================================================================ */
void actuator_home_all(void)
{
/* 舵机抬起 */
servo_set_up();
/* 各轴回零真实行程 (当前 → 零点): 已在零位的轴不等待、不算超时 */
uint32_t x_home_pulses = ACTUATOR_MM_TO_PULSES(ACTUATOR_HOME_X_MM);
uint32_t y_home_pulses = ACTUATOR_MM_TO_PULSES(ACTUATOR_HOME_Y_MM);
/* 置零 Z 终点=0°: 电机1 不回传到位帧 → 不纳入事件等待, 用 CPOS 轮询确认 */
float home_z_tgt = ACTUATOR_HOME_ANGLE_DEG;
uint32_t home_dz = (uint32_t)(fabsf(s_cur_angle_deg - home_z_tgt)
* 10.0f * s_scale_angle + 0.5f);
uint32_t home_dx = (s_cur_x_pulses > x_home_pulses) ? (s_cur_x_pulses - x_home_pulses)
: (x_home_pulses - s_cur_x_pulses);
uint32_t home_dy = (s_cur_y_pulses > y_home_pulses) ? (s_cur_y_pulses - y_home_pulses)
: (y_home_pulses - s_cur_y_pulses);
uint32_t home_mask = 0;
if (home_dz > 0 && home_z_tgt != 0.0f) home_mask |= (1u << 0);
if (home_dx > 0) home_mask |= (1u << 1); /* X */
if (home_dy > 0) home_mask |= (1u << 2) | (1u << 3); /* Y 双电机 */
/* 到位等待兜底超时基准 = 各轴真实回零行程的纯运动时间 */
uint32_t home_motion = actuator_motion_time_ms(home_dz, ACTUATOR_EXEC_Z_VEL,
ACTUATOR_EXEC_Z_ACC);
uint32_t t = actuator_motion_time_ms(home_dx, ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC);
if (t > home_motion) home_motion = t;
t = actuator_motion_time_ms(home_dy, ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC);
if (t > home_motion) home_motion = t;
ESP_LOGI(TAG, "置零行程: dZ=%lu dX=%lu dY=%lu 脉冲, mask=0x%02X, 等待超时=%u ms",
(unsigned long)home_dz, (unsigned long)home_dx, (unsigned long)home_dy,
home_mask, home_motion + ACTUATOR_SETTLE_MARGIN_MS);
/* 清掉可能残留的到位帧, 防止污染本次归零等待 (超时后迟到帧边界) */
Emm_V5_Arrival_Reset();
/* Z/X/Y 三轴并行回零 (Z 到位用 CPOS 轮询确认) */
actuator_motor_angle(ACTUATOR_ADDR_ANGLE, home_z_tgt - s_cur_angle_deg,
ACTUATOR_EXEC_Z_VEL, ACTUATOR_EXEC_Z_ACC, false);
actuator_motor_mm(ACTUATOR_ADDR_X, ACTUATOR_HOME_X_MM,
ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC, true);
actuator_motor_mm(ACTUATOR_ADDR_Y1, ACTUATOR_HOME_Y_MM,
ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC, true);
actuator_wait_arrival(home_motion, home_mask);
/* Z 终点=0°: 用 CPOS 轮询确认到位 */
if (home_dz > 0 && home_z_tgt == 0.0f)
_wait_z_at_zero(ACTUATOR_Z_CONFIRM_MS, ACTUATOR_Z_CONFIRM_TOL_DEG);
ESP_LOGI(TAG, "置零完成 (Z=%.1f° X=%.0fmm Y=%.0fmm)",
(double)home_z_tgt,
(double)ACTUATOR_HOME_X_MM,
(double)ACTUATOR_HOME_Y_MM);
}
/* ================================================================
* 最近 Cycle 快照查询
* ================================================================ */
components/actuator/actuator.h
+43 -10
View File
@@ -106,6 +106,27 @@ extern "C" {
#define ACTUATOR_ANGLE_LIMIT_DEG 270.0f
#endif
/** 置零 (归零) 目标位置 (Cycle 完成后 / 按钮识别前回位) */
#ifndef ACTUATOR_HOME_X_MM
#define ACTUATOR_HOME_X_MM 0.0f /* X 轴回零位置 (mm) */
#endif
#ifndef ACTUATOR_HOME_Y_MM
#define ACTUATOR_HOME_Y_MM 330.0f /* Y 轴回零位置 (mm, 接近行程上限, 与原按钮归零一致) */
#endif
#ifndef ACTUATOR_HOME_ANGLE_DEG
#define ACTUATOR_HOME_ANGLE_DEG 0.0f /* Z 旋转轴回零角度 (°) */
#endif
/** Z 轴回 0° 到位确认参数: 电机1 (Z 轴) 对“终点靠近 0°”的移动不回传到位帧
(实测 0.1°/0.5° 垫高都不触发) → 用 Emm_V5_Read_CPOS 轮询确认到位:
timeout_ms = 轮询超时 (Z 回零移动小, 通常已被 X/Y 等待覆盖), tol = 容差 (°) */
#ifndef ACTUATOR_Z_CONFIRM_MS
#define ACTUATOR_Z_CONFIRM_MS 1000
#endif
#ifndef ACTUATOR_Z_CONFIRM_TOL_DEG
#define ACTUATOR_Z_CONFIRM_TOL_DEG 2.0f
#endif
/** 执行 Cycle / 归零时 XY 轴的速度与加速度档位
2026-08-01 提速: 3000RPM → 3500RPM (加速度保持 250 档, 实测稳定)
注: 手册 FD 位置模式速度标称上限 0-3000RPM, 3500 超出文档范围需实测 */
@@ -126,9 +147,11 @@ extern "C" {
#endif
/** 到位等待超时兜底裕量 (ms): 闭环到位轮询的硬超时上限 (纯运动时间 + 此裕量);
正常到位会提前返回, 此裕量仅作保底防卡死 */
正常到位会提前返回, 此裕量仅作保底防卡死。
2026-08-03: 400→1000 —— 大行程/高速运动后驱动闭环在 0.1° 窗口内稳定需要
额外时间, 400ms 裕量实测多次在到位帧到达前就超时 (电机3/4 迟到 ~0.5s) */
#ifndef ACTUATOR_SETTLE_MARGIN_MS
#define ACTUATOR_SETTLE_MARGIN_MS 400
#define ACTUATOR_SETTLE_MARGIN_MS 1000
#endif
/** 到位判定容差 (原始单位, Emm 固件 0~65535 = 360°; 1.5° ≈ 273)
@@ -137,9 +160,9 @@ extern "C" {
#define ACTUATOR_ARRIVAL_TOL_RAW 273
#endif
/** 舵机动作 / 磁铁吸合后的等待时间 (ms) (实测优化: 300 → 150) */
/** 舵机动作 / 磁铁吸合后的等待时间 (ms) (150 偏短不稳, 300 略保守 → 200) */
#ifndef ACTUATOR_GRIP_SETTLE_MS
#define ACTUATOR_GRIP_SETTLE_MS 150
#define ACTUATOR_GRIP_SETTLE_MS 200
#endif
/**
@@ -162,12 +185,14 @@ extern "C" {
*
* ⚠️ 语义 (2026-08-03 修正): MPU 下发的坐标 (source/target) 来自 calibrated_H,
* 本身是"旋转中心"坐标 —— θ=0 时中心到坐标点, 工具末端即落在目标物点
* (source 抓取不补偿且工作正常可佐证)。因此 target 的补偿公式为:
* X_motor = X_tgt + dx - (dx * cosθ - dy * sinθ)
* Y_motor = Y_tgt + dy - (dx * sinθ + dy * cosθ)
* (即先把中心坐标换算回目标物点 D = target + (dx,dy),
* D = 坐标 + (dx,dy)。source 与 target 都须按此补偿, 公式为:
* X_motor = X + dx - (dx * cosθ - dy * sinθ)
* Y_motor = Y + dy - (dx * sinθ + dy * cosθ)
* (即先把中心坐标换算回目标物点 D = 坐标 + (dx,dy),
* 再按转角 θ 反算旋转中心)。旧公式少了 "+ (dx,dy)" 项,
* 会把工具末端整体放偏 (dx,dy) = (6.6, -35.75)mm (y 偏大 35.75mm)。
* ⚠️ source 不补偿时, 抓取瞬间 Z 角 ≠ 0° 则吸盘偏移 (R(θ)-I)·(dx,dy),
* y 分量 (35.75mm) 主导 → 抓取点偏 y (2026-08-03 已修: source 也补偿)。
*
* dx, dy 为工具末端相对于 Z 旋转中心的 XY 偏移 (mm)。
* ================================================================ */
@@ -256,11 +281,12 @@ uint32_t actuator_mm_motion_time_ms(float distance_mm, uint16_t vel, uint8_t acc
uint32_t actuator_angle_motion_time_ms(float angle_deg, uint16_t vel, uint8_t acc);
/**
* @brief 等待指定电机到位 (轮询闭环 S_FLAG 到位标志)
* @brief 等待指定电机到位 (事件驱动: 收到驱动主动回传的到位帧)
* @param base_wait_ms 纯运动时间估算 (兜底超时基准, 超时=纯运动时间+裕量)
* @param moved_mask 实际有行程的电机位掩码:
* bit0=Z旋转, bit1=X, bit2=Y1, bit3=Y2
* @return true=全部到位, false=超时 (已按最坏情况等待)
* @return true=全部到位, false=超时
* @note 依赖驱动 Response=Reached/Both/Other 回传到位帧 (地址+FD+9F+6B)
*/
bool actuator_wait_arrival(uint32_t base_wait_ms, uint32_t moved_mask);
@@ -297,6 +323,13 @@ void actuator_apply_offset_compensation(uint16_t *target_x, uint16_t *target_y,
*/
int actuator_execute_cycle(const edc_cycle_t *cycle);
/**
* @brief 执行机构置零 (归位): 舵机抬起 + Z/X/Y 三轴并行回零点
* @note Cycle 执行完成 / 按钮识别前调用; 内部含到位等待;
* 目标位置由 ACTUATOR_HOME_X_MM / Y_MM / ANGLE_DEG 配置
*/
void actuator_home_all(void);
/**
* @brief 急停全部 4 路电机
*/
components/emm_v5/CMakeLists.txt
+1 -1
View File
@@ -1,3 +1,3 @@
idf_component_register(SRCS "emm_v5.c"
INCLUDE_DIRS "."
REQUIRES esp_driver_uart esp_driver_gpio driver esp_timer)
REQUIRES esp_driver_uart esp_driver_gpio driver esp_timer freertos)
components/emm_v5/emm_v5.c
+276 -90
View File
@@ -8,6 +8,9 @@
#include <math.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "freertos/event_groups.h"
#include "esp_timer.h"
static const char *TAG = "EMM_V5";
@@ -15,6 +18,40 @@ static const char *TAG = "EMM_V5";
/* 运行时配置 (由 Emm_V5_Init 设置) */
static uart_port_t s_uart_port;
/* ============================================================
* RX 中断/事件驱动状态
*
* 驱动 Response 菜单设为 Reached/Both/Other 后, 位置模式 (0xFD) 命令
* 运动到位会主动回传到位帧: 地址 + FD + 9F + 6B。
* UART RX 由内部任务独占读取, 解析出到位帧后置位事件组位
* (bit = 电机地址 - 1); 调用方用 Emm_V5_Wait_Arrival 事件等待,
* 全部到位即返回 —— 不再轮询 S_TPOS/S_CPOS, 避免"没到位就下一步"。
* ============================================================ */
#define EMM_V5_ARRIVAL_MAX_MOTORS 8
static EventGroupHandle_t s_arrival_evt = NULL;
static QueueHandle_t s_uart_evt_queue = NULL;
static TaskHandle_t s_rx_task = NULL;
/* 请求-响应 (网页手动读参数等) */
static SemaphoreHandle_t s_resp_mutex = NULL;
static struct {
bool active;
uint8_t addr;
uint8_t func;
uint8_t *out;
uint8_t max;
uint8_t len;
SemaphoreHandle_t done;
} s_resp;
/* RX 帧累积缓冲 (0x6B 为帧尾) */
static uint8_t s_rx_frame[64];
static uint8_t s_rx_frame_len;
static void emm_v5_rx_task(void *arg); /* 前置声明 (Emm_V5_Init 中创建) */
/* ============================================================
* 初始化
* ============================================================ */
@@ -32,10 +69,13 @@ void Emm_V5_Init(uart_port_t port, int tx_pin, int rx_pin)
.source_clk = UART_SCLK_DEFAULT,
};
/* 事件队列: 中断驱动 RX (到位帧解析); 64 深度防突发丢事件 */
s_uart_evt_queue = xQueueCreate(64, sizeof(uart_event_t));
ESP_ERROR_CHECK(uart_driver_install(s_uart_port,
EMM_V5_UART_BUF_SIZE * 2,
EMM_V5_UART_BUF_SIZE * 2,
0, NULL, 0));
16, &s_uart_evt_queue, 0));
ESP_ERROR_CHECK(uart_param_config(s_uart_port, &uart_cfg));
ESP_ERROR_CHECK(uart_set_pin(s_uart_port,
tx_pin,
@@ -43,7 +83,15 @@ void Emm_V5_Init(uart_port_t port, int tx_pin, int rx_pin)
UART_PIN_NO_CHANGE,
UART_PIN_NO_CHANGE));
ESP_LOGI(TAG, "初始化完成 (UART%d, TX=%d, RX=%d)",
/* 到位事件组 / 请求-响应信号量 / RX 任务 */
if (s_arrival_evt == NULL) s_arrival_evt = xEventGroupCreate();
if (s_resp_mutex == NULL) s_resp_mutex = xSemaphoreCreateMutex();
if (s_resp.done == NULL) s_resp.done = xSemaphoreCreateBinary();
if (s_rx_task == NULL) {
xTaskCreate(emm_v5_rx_task, "emm_rx", 3072, NULL, 5, &s_rx_task);
}
ESP_LOGI(TAG, "初始化完成 (UART%d, TX=%d, RX=%d, 到位帧中断已启用)",
s_uart_port, tx_pin, rx_pin);
}
@@ -57,31 +105,178 @@ static inline void Emm_V5_Send(const uint8_t *cmd, uint8_t len)
}
/* ============================================================
* 接收数据
* RX 中断/事件驱动
*
* 帧格式: 地址 + 功能码 + 数据... + 0x6B (校验字节)
* 到位帧: 地址 + FD + 9F + 6B (位置模式命令运动到位后主动回传)
* ============================================================ */
/* 处理一帧完整数据 (以 0x6B 结尾) */
static void emm_v5_handle_frame(const uint8_t *f, uint8_t len)
{
if (len < 4) return;
/* 到位帧: 地址 + FD + 9F + 6B → 置位事件组 (bit = 地址 - 1) */
if (f[1] == 0xFD && f[2] == 0x9F) {
uint8_t addr = f[0];
if (addr >= 1 && addr <= EMM_V5_ARRIVAL_MAX_MOTORS && s_arrival_evt) {
xEventGroupSetBits(s_arrival_evt, (EventBits_t)(1u << (addr - 1)));
ESP_LOGI(TAG, "到位帧: 电机%d", addr);
}
}
/* 请求-响应匹配 (网页手动读参数等) */
if (s_resp_mutex) {
xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
if (s_resp.active && f[0] == s_resp.addr && f[1] == s_resp.func) {
uint8_t n = (len < s_resp.max) ? len : s_resp.max;
if (s_resp.out && n) memcpy(s_resp.out, f, n);
s_resp.len = n;
s_resp.active = false;
xSemaphoreGive(s_resp.done);
}
xSemaphoreGive(s_resp_mutex);
}
}
/* UART RX 独占任务: 事件队列驱动, 解析帧并分派 */
static void emm_v5_rx_task(void *arg)
{
uart_event_t evt;
uint8_t buf[64];
while (1) {
if (xQueueReceive(s_uart_evt_queue, &evt, portMAX_DELAY) != pdTRUE)
continue;
if (evt.type == UART_DATA || evt.type == UART_FIFO_OVF ||
evt.type == UART_BUFFER_FULL) {
int len;
while ((len = uart_read_bytes(s_uart_port, buf, sizeof(buf), 0)) > 0) {
for (int i = 0; i < len; i++) {
s_rx_frame[s_rx_frame_len++] = buf[i];
if (s_rx_frame_len >= sizeof(s_rx_frame)) {
s_rx_frame_len = 0; /* 缓冲溢出: 重新同步 */
}
if (buf[i] == 0x6B) { /* 帧尾 */
emm_v5_handle_frame(s_rx_frame, s_rx_frame_len);
s_rx_frame_len = 0;
}
}
}
if (evt.type == UART_FIFO_OVF || evt.type == UART_BUFFER_FULL) {
/* 溢出只说明有部分字节被丢弃: 重置帧累积重新同步即可。
不能 uart_flush_input —— 那会把缓冲里仍有效的到位帧一并冲掉,
是偶发丢电机2/3/4 到位帧的根因之一 */
s_rx_frame_len = 0;
}
}
}
}
/* 内部: 发送命令并等待匹配的响应帧 (地址+功能码), 供读函数复用 */
static int emm_v5_request_response(const uint8_t *cmd, uint8_t cmd_len,
uint8_t addr, uint8_t func,
uint8_t *out, uint8_t max,
uint32_t timeout_ms)
{
if (!s_resp_mutex || !s_resp.done) return -1;
xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
if (s_resp.active) {
xSemaphoreGive(s_resp_mutex);
ESP_LOGW(TAG, "请求-响应忙, 放弃 (addr=%u func=0x%02X)", addr, func);
return -1;
}
s_resp.active = true;
s_resp.addr = addr;
s_resp.func = func;
s_resp.out = out;
s_resp.max = max;
s_resp.len = 0;
xSemaphoreGive(s_resp_mutex);
xSemaphoreTake(s_resp.done, 0); /* 清残留计数 */
Emm_V5_Send(cmd, cmd_len);
if (xSemaphoreTake(s_resp.done, pdMS_TO_TICKS(timeout_ms)) != pdTRUE) {
xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
s_resp.active = false; /* 放弃等待 */
xSemaphoreGive(s_resp_mutex);
ESP_LOGW(TAG, "响应超时 (addr=%u func=0x%02X)", addr, func);
return -1;
}
xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
int n = s_resp.len;
xSemaphoreGive(s_resp_mutex);
return n;
}
/* ============================================================
* 到位等待 (事件驱动)
*
* 注意: Emm_V5_Wait_Arrival 不消费到位位 (便于调用方分段等待时闪烁 LED);
* 一段 motion 的等待结束后必须调用 Emm_V5_Arrival_Consume 清除到位位,
* 防止残留位污染下一段 motion。
* ============================================================ */
bool Emm_V5_Wait_Arrival(const uint8_t *addrs, uint8_t count, uint32_t timeout_ms)
{
if (!s_arrival_evt) return false;
EventBits_t expected = 0;
for (uint8_t i = 0; i < count; i++) {
uint8_t a = addrs[i];
if (a >= 1 && a <= EMM_V5_ARRIVAL_MAX_MOTORS)
expected |= (EventBits_t)(1u << (a - 1));
}
if (expected == 0) return true;
EventBits_t got = xEventGroupWaitBits(s_arrival_evt, expected,
pdFALSE, pdTRUE,
pdMS_TO_TICKS(timeout_ms));
return ((got & expected) == expected);
}
void Emm_V5_Arrival_Consume(const uint8_t *addrs, uint8_t count)
{
if (!s_arrival_evt) return;
EventBits_t expected = 0;
for (uint8_t i = 0; i < count; i++) {
uint8_t a = addrs[i];
if (a >= 1 && a <= EMM_V5_ARRIVAL_MAX_MOTORS)
expected |= (EventBits_t)(1u << (a - 1));
}
if (expected)
xEventGroupClearBits(s_arrival_evt, expected);
}
void Emm_V5_Arrival_Reset(void)
{
if (!s_arrival_evt) return;
xEventGroupClearBits(s_arrival_evt, 0xFF); /* 清 addr 1~8 对应位 (bit=地址-1) */
}
/* 查询当前已置位的到位位 (调试用): 返回 addr1~8 对应位掩码 */
uint32_t Emm_V5_Arrival_Get_Bits(void)
{
if (!s_arrival_evt) return 0;
return xEventGroupGetBits(s_arrival_evt) & 0xFF;
}
/* ============================================================
* 接收数据 (已废弃)
* ============================================================ */
uint8_t Emm_V5_Receive_Data(uint8_t *rxCmd, uint8_t maxLen)
{
uint8_t count = 0;
int64_t last_time, now;
last_time = esp_timer_get_time();
while (1) {
int len = uart_read_bytes(s_uart_port, &rxCmd[count], 1,
pdMS_TO_TICKS(10));
if (len > 0) {
count++;
if (count >= maxLen) break;
last_time = esp_timer_get_time();
} else {
now = esp_timer_get_time();
if ((now - last_time) > (EMM_V5_RX_TIMEOUT_MS * 1000)) {
break;
}
}
}
return count;
/* 已废弃: UART RX 由内部 RX 任务独占 (中断驱动), 此函数不再直接读 UART;
网页手动读参数请改用 Emm_V5_Read_Sys_Params_Resp */
(void)rxCmd;
(void)maxLen;
return 0;
}
/* ============================================================
@@ -100,33 +295,62 @@ void Emm_V5_Reset_Clog_Pro(uint8_t addr)
Emm_V5_Send(cmd, 4);
}
/* 系统参数 → 功能码 (读取响应帧按 地址+功能码 匹配) */
static uint8_t emm_v5_sysparam_func(EmmSysParams_t s)
{
switch (s) {
case S_VER: return 0x1F;
case S_RL: return 0x20;
case S_PID: return 0x21;
case S_VBUS: return 0x24;
case S_CPHA: return 0x27;
case S_ENCL: return 0x31;
case S_TPOS: return 0x33;
case S_VEL: return 0x35;
case S_CPOS: return 0x36;
case S_PERR: return 0x37;
case S_FLAG: return 0x3A;
case S_ORG: return 0x3B;
case S_Conf: return 0x42;
case S_State: return 0x43;
default: return 0x00;
}
}
void Emm_V5_Read_Sys_Params(uint8_t addr, EmmSysParams_t s)
{
uint8_t cmd[8] = {0};
uint8_t i = 0;
uint8_t func = emm_v5_sysparam_func(s);
cmd[i++] = addr;
switch (s) {
case S_VER: cmd[i++] = 0x1F; break;
case S_RL: cmd[i++] = 0x20; break;
case S_PID: cmd[i++] = 0x21; break;
case S_VBUS: cmd[i++] = 0x24; break;
case S_CPHA: cmd[i++] = 0x27; break;
case S_ENCL: cmd[i++] = 0x31; break;
case S_TPOS: cmd[i++] = 0x33; break;
case S_VEL: cmd[i++] = 0x35; break;
case S_CPOS: cmd[i++] = 0x36; break;
case S_PERR: cmd[i++] = 0x37; break;
case S_FLAG: cmd[i++] = 0x3A; break;
case S_ORG: cmd[i++] = 0x3B; break;
case S_Conf: cmd[i++] = 0x42; cmd[i++] = 0x6C; break;
case S_State: cmd[i++] = 0x43; cmd[i++] = 0x7A; break;
default: break;
}
if (func) cmd[i++] = func;
if (s == S_Conf) cmd[i++] = 0x6C;
if (s == S_State) cmd[i++] = 0x7A;
cmd[i++] = 0x6B;
Emm_V5_Send(cmd, i);
}
uint8_t Emm_V5_Read_Sys_Params_Resp(uint8_t addr, EmmSysParams_t s,
uint8_t *rxOut, uint8_t maxLen,
uint32_t timeout_ms)
{
uint8_t cmd[8] = {0};
uint8_t i = 0;
uint8_t func = emm_v5_sysparam_func(s);
cmd[i++] = addr;
if (func) cmd[i++] = func;
if (s == S_Conf) cmd[i++] = 0x6C;
if (s == S_State) cmd[i++] = 0x7A;
cmd[i++] = 0x6B;
int n = emm_v5_request_response(cmd, i, addr, func, rxOut, maxLen,
(timeout_ms > 0) ? timeout_ms
: EMM_V5_RX_TIMEOUT_MS);
return (n > 0) ? (uint8_t)n : 0;
}
/* ============================================================
* 读取电机状态标志 (S_FLAG 0x3A) 并解析返回
* ============================================================ */
@@ -137,29 +361,10 @@ uint8_t Emm_V5_Read_Status_Flag(uint8_t addr, uint32_t timeout_ms)
uint8_t cmd[3] = {addr, 0x3A, 0x6B};
uint8_t read_to = (timeout_ms > 0) ? timeout_ms : EMM_V5_RX_TIMEOUT_MS;
/* 清掉历史残留 (运动命令的 ACK/到位帧等), 防止误判 */
uart_flush_input(s_uart_port);
Emm_V5_Send(cmd, 3);
uint8_t n = 0;
int64_t last = esp_timer_get_time();
int64_t deadline = last + (int64_t)read_to * 1000;
while (esp_timer_get_time() < deadline) {
int len = uart_read_bytes(s_uart_port, &rx[n], 1, pdMS_TO_TICKS(5));
if (len > 0) {
if (n < sizeof(rx) - 1) n++;
last = esp_timer_get_time();
/* 帧收完整 (地址 + 3A + 状态 + 6B) 立即返回, 不再等空闲超时,
把单次轮询从 ~17ms 压到 ~3ms, 减小到位检测抖动 */
for (int i = 0; i + 3 < (int)n; i++) {
if (rx[i] == addr && rx[i + 1] == 0x3A && rx[i + 3] == 0x6B)
return rx[i + 2];
}
} else if ((esp_timer_get_time() - last) > 5000) {
break; /* 5ms 无新字节: 认为帧已收完/无响应 */
}
}
int n = emm_v5_request_response(cmd, sizeof(cmd), addr, 0x3A,
rx, sizeof(rx), read_to);
if (n >= 4 && rx[0] == addr && rx[1] == 0x3A && rx[3] == 0x6B)
return rx[2];
return 0xFF;
}
@@ -174,34 +379,15 @@ static int32_t Emm_V5_Read_Pos(uint8_t addr, uint8_t func)
{
uint8_t rx[16] = {0};
uint8_t cmd[3] = {addr, func, 0x6B};
uint8_t read_to = EMM_V5_RX_TIMEOUT_MS;
/* 清掉历史残留 (ACK/到位帧等), 防止误判 */
uart_flush_input(s_uart_port);
Emm_V5_Send(cmd, 3);
uint8_t n = 0;
int64_t last = esp_timer_get_time();
int64_t deadline = last + (int64_t)read_to * 1000;
while (esp_timer_get_time() < deadline) {
int len = uart_read_bytes(s_uart_port, &rx[n], 1, pdMS_TO_TICKS(5));
if (len > 0) {
if (n < sizeof(rx) - 1) n++;
last = esp_timer_get_time();
/* 帧收完整 (地址+功能码+符号+位置4B+6B) 立即返回 */
for (int i = 0; i + 7 < (int)n; i++) {
if (rx[i] == addr && rx[i + 1] == func &&
(rx[i + 2] == 0 || rx[i + 2] == 1) && rx[i + 7] == 0x6B)
return (int32_t)(((uint32_t)rx[i + 3] << 24) |
((uint32_t)rx[i + 4] << 16) |
((uint32_t)rx[i + 5] << 8) |
(uint32_t)rx[i + 6]);
}
} else if ((esp_timer_get_time() - last) > 5000) {
break; /* 5ms 无新字节: 认为帧已收完/无响应 */
}
}
int n = emm_v5_request_response(cmd, sizeof(cmd), addr, func,
rx, sizeof(rx), EMM_V5_RX_TIMEOUT_MS);
if (n >= 8 && rx[0] == addr && rx[1] == func &&
(rx[2] == 0 || rx[2] == 1) && rx[7] == 0x6B)
return (int32_t)(((uint32_t)rx[3] << 24) |
((uint32_t)rx[4] << 16) |
((uint32_t)rx[5] << 8) |
(uint32_t)rx[6]);
return -1;
}
components/emm_v5/emm_v5.h
+58 -5
View File
@@ -25,7 +25,7 @@ extern "C" {
* 可配置常量
* ============================================================ */
#define EMM_V5_UART_BAUDRATE 115200
#define EMM_V5_UART_BUF_SIZE 256
#define EMM_V5_UART_BUF_SIZE 512
/* 接收超时时间 (毫秒) */
#define EMM_V5_RX_TIMEOUT_MS 100
@@ -220,10 +220,63 @@ void Emm_V5_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF);
void Emm_V5_Origin_Interrupt(uint8_t addr);
/**
* @brief 接收电机返回数据 (100ms 超时判定帧结束)
* @param rxCmd 接收缓冲区
* @param maxLen 缓冲区最大长度
* @return 实际接收到的字节数
* @brief 等待指定电机到位 (事件驱动, 不消费到位位)
*
* 电机驱动 Response 菜单设为 Reached/Both/Other 后, 位置模式 (0xFD) 命令
* 运动到位会主动回传到位帧 (地址 + FD + 9F + 6B)。UART RX 由本组件内部
* 任务中断读取并解析, 此处事件等待全部到位, 不再轮询 S_TPOS/S_CPOS。
*
* @note 本函数不清除到位位, 便于调用方分段等待 (如等待期间闪烁 LED);
* 一段 motion 等待结束后必须调用 Emm_V5_Arrival_Consume 清除,
* 防止残留位污染下一段 motion。
* @param addrs 电机地址数组, 如 {ACTUATOR_ADDR_X}
* @param count 数组长度
* @param timeout_ms 本次等待超时 (ms); 0 = 立即返回当前状态
* @return true=全部到位, false=超时/未到齐
*/
bool Emm_V5_Wait_Arrival(const uint8_t *addrs, uint8_t count, uint32_t timeout_ms);
/**
* @brief 消费 (清除) 指定电机的到位位
* @param addrs 电机地址数组
* @param count 数组长度
* @note 一段 motion 的到位等待结束后调用 (成功/超时都要调)
*/
void Emm_V5_Arrival_Consume(const uint8_t *addrs, uint8_t count);
/**
* @brief 清除全部电机到位位 (开始新一段 motion 前调用)
* @note 防止上一段 motion 超时后迟到的到位帧 (或 0 脉冲命令仍回传的帧)
* 污染新一段 motion 的到位等待
*/
void Emm_V5_Arrival_Reset(void);
/**
* @brief 查询当前已置位的到位位 (调试用)
* @return 位掩码: bit = (电机地址 - 1), 仅低 8 位有效
*/
uint32_t Emm_V5_Arrival_Get_Bits(void);
/**
* @brief 发送读取系统参数命令并等待响应帧返回 (请求-响应式)
* @param addr 电机地址
* @param s 系统参数类型 (@see EmmSysParams_t)
* @param rxOut 接收缓冲区
* @param maxLen 缓冲区最大长度
* @param timeout_ms 等待响应超时 (ms)
* @return 实际收到的响应字节数 (含 地址/功能码/.../0x6B), 失败返回 0
*/
uint8_t Emm_V5_Read_Sys_Params_Resp(uint8_t addr, EmmSysParams_t s,
uint8_t *rxOut, uint8_t maxLen,
uint32_t timeout_ms);
/**
* @brief 接收电机返回数据
* @deprecated UART RX 已由内部 RX 任务独占 (中断驱动), 此函数不再读取 UART;
* 网页手动读参数请改用 Emm_V5_Read_Sys_Params_Resp
* @param rxCmd 接收缓冲区 (保留, 未使用)
* @param maxLen 缓冲区最大长度 (保留, 未使用)
* @return 恒为 0
*/
uint8_t Emm_V5_Receive_Data(uint8_t *rxCmd, uint8_t maxLen);
components/protocol/PROTOCOL.md
+3 -1
View File
@@ -55,8 +55,10 @@ Cycle = { cycle_id: uint16, frag_count: uint8 (2~4), fragments: Fragment[]
| 命令 | 值 | 方向 | 说明 |
|------|-----|------|------|
| `CYCLE_DATA` | `0x01` | MPU → MCU | 下发 Cycle 数据 |
| `CYCLE_DONE` | `0x02` | MCU → MPU | Cycle 执行完成 |
| `CYCLE_DONE` | `0x02` | MCU → MPU | Cycle 执行完成 (MPU 同时终止任务计时) |
| `CYCLE_REQUEST` | `0x03` | MCU → MPU | 请求下一个 Cycle |
| `RECOGNIZE` | `0x04` | MCU → MPU | 触发视觉识别 |
| `TASK_START` | `0x05` | MCU → MPU | 任务开始 (按钮按下即发, 与 RECOGNIZE 区分; MPU 据此开始计时) |
| `ACK` | `0x80` | 双向 | 通用应答 (预留) |
| `NACK` | `0xFF` | 双向 | 错误 (预留) |
components/protocol/protocol.c
+8
View File
@@ -460,6 +460,14 @@ void edc_proto_send_recognize(void)
ESP_LOGI(TAG, "RECOGNIZE sent");
}
void edc_proto_send_task_start(void)
{
if (!s_uart_enabled) return;
uart_send_frame(EDC_CMD_TASK_START, NULL, 0);
s_stat.request_sent++;
ESP_LOGI(TAG, "TASK_START sent");
}
/* ================================================================
* 统计
* ================================================================ */
components/protocol/protocol.h
+7
View File
@@ -77,6 +77,7 @@ typedef enum {
EDC_CMD_CYCLE_DONE = 0x02, /* MCU → MPU: Cycle 执行完成 */
EDC_CMD_CYCLE_REQUEST = 0x03, /* MCU → MPU: 请求下一个 Cycle */
EDC_CMD_RECOGNIZE = 0x04, /* MCU → MPU: 触发视觉识别 */
EDC_CMD_TASK_START = 0x05, /* MCU → MPU: 任务开始 (按钮按下即发, 与 RECOGNIZE 区分) */
EDC_CMD_ACK = 0x80, /* 通用应答 */
EDC_CMD_NACK = 0xFF, /* 错误 */
} edc_cmd_t;
@@ -215,6 +216,12 @@ void edc_proto_send_request(void);
*/
void edc_proto_send_recognize(void);
/**
* @brief 发送 TASK_START 通知 MPU 任务开始计时 (通过 UART, 按钮按下即发,
* 与 RECOGNIZE 区分, 不触发识别)
*/
void edc_proto_send_task_start(void);
/**
* @brief 获取统计信息
*/
components/web_ctrl/web_ctrl.c
+2 -3
View File
@@ -685,10 +685,9 @@ static esp_err_t api_read_param_handler(httpd_req_t *req)
int addr = get_query_int(req, "addr", 1);
int param = get_query_int(req, "param", 0);
Emm_V5_Read_Sys_Params((uint8_t)addr, (EmmSysParams_t)param);
uint8_t rx[128];
uint8_t len = Emm_V5_Receive_Data(rx, sizeof(rx));
uint8_t len = Emm_V5_Read_Sys_Params_Resp((uint8_t)addr, (EmmSysParams_t)param,
rx, sizeof(rx), 200);
uint32_t val32 = 0;
uint16_t val16 = 0;
main/main.c
+6 -27
View File
@@ -22,7 +22,6 @@
* WiFi: 连接 ICE (密码 12345678), 获取 IP 后用浏览器打开控制页面
*/
#include "emm_v5.h"
#include "magnet.h"
#include "servo.h"
#include "web_ctrl.h"
@@ -47,35 +46,15 @@ static void button_recognize_task(void *pvParam)
{
while (1) {
if (key_is_pressed()) {
ESP_LOGI(TAG, "按钮按下, 归零后再发送 RECOGNIZE ...");
ESP_LOGI(TAG, "按钮按下, 先发 TASK_START (MPU 开始计时) ...");
buzzer_beep(2000, 100); /* 短音: 识别开始 */
/* 按下按钮即通知 MPU 任务开始计时 (与 RECOGNIZE 区分, 不触发识别) */
edc_proto_send_task_start();
/* ---- 归零: 舵机抬起 + Z/X/Y 三轴并行回零 ----
Z 旋转与 X/Y 平移互不干扰 (Fragment 里也是三轴并行),
无需等 Z 到位再动 X/Y —— 三轴同时下发、一次到位等待 */
actuator_servo_up();
uint32_t z_motion = actuator_angle_motion_time_ms(ACTUATOR_ANGLE_LIMIT_DEG,
ACTUATOR_EXEC_Z_VEL,
ACTUATOR_EXEC_Z_ACC);
uint32_t xy_motion = actuator_mm_motion_time_ms(ACTUATOR_LIMIT_X_MAX,
ACTUATOR_EXEC_VEL,
ACTUATOR_EXEC_ACC);
uint32_t y_motion = actuator_mm_motion_time_ms(ACTUATOR_LIMIT_Y_MAX,
ACTUATOR_EXEC_VEL,
ACTUATOR_EXEC_ACC);
uint32_t home_motion = z_motion;
if (xy_motion > home_motion) home_motion = xy_motion;
if (y_motion > home_motion) home_motion = y_motion;
actuator_motor_angle(ACTUATOR_ADDR_ANGLE, 0.0f,
ACTUATOR_EXEC_Z_VEL, ACTUATOR_EXEC_Z_ACC, true);
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);
actuator_wait_arrival(home_motion,
(1u << 0) | (1u << 1) | (1u << 2) | (1u << 3));
Cycle 执行完成后也会调用 actuator_home_all() 置零 */
actuator_home_all();
ESP_LOGI(TAG, "归零完成, 发送 RECOGNIZE ...");