执行加速（部分动作连接处过快）
This commit is contained in:
kirl committed
1 parent
351e22e123
commit
c3028a9433
5 files changed
+321
-44
No files matched your search
+146
-20
@@ -11,6 +11,7 @@
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_timer.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
@@ -69,6 +70,112 @@ 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 次在容差内才确认: 确保电机已稳定停下 (减速途中/振荡时
|
||||
* 位置可能短暂进入容差, 单独一次会"没到位就进行下一步")。
|
||||
*
|
||||
* ⚠️ 防坑: 轮询第一帧前必须留帧间隔 —— 运动命令最后一条是 Y 同步广播
|
||||
* 触发 (00 FF 66 6B), 若紧跟发轮询帧会"零间隔背靠背", 驱动板丢字节
|
||||
* 吞掉触发帧, 电机不启动。
|
||||
*
|
||||
* @param base_wait_ms 纯运动时间估算 (兜底超时基准)
|
||||
* @param moved_mask 实际有行程的电机位掩码 (bit0=Z, bit1=X, bit2/3=Y1/Y2)
|
||||
* @return true=全部到位, false=超时
|
||||
*/
|
||||
bool actuator_wait_arrival(uint32_t base_wait_ms, uint32_t moved_mask)
|
||||
{
|
||||
const uint8_t addrs[4] = {
|
||||
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;
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
if (!(moved_mask & (1u << i)))
|
||||
arrived[i] = true; /* 无行程: 视为已到位 */
|
||||
}
|
||||
|
||||
/* 与上一条运动命令 (尤其 Y 同步触发) 之间留帧间隔, 防丢字节 */
|
||||
vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS));
|
||||
|
||||
while (esp_timer_get_time() < deadline) {
|
||||
bool all = true;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
if (arrived[i])
|
||||
continue;
|
||||
|
||||
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)); /* 帧间隔防粘包 */
|
||||
}
|
||||
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 一致) */
|
||||
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]);
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
* 运动到位时间估算
|
||||
*
|
||||
@@ -110,18 +217,28 @@ 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)
|
||||
uint32_t actuator_mm_motion_time_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);
|
||||
return actuator_motion_time_ms(pulses, vel, acc);
|
||||
}
|
||||
|
||||
uint32_t actuator_angle_motion_time_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_motion_time_ms(pulses, vel, acc);
|
||||
}
|
||||
|
||||
uint32_t actuator_mm_settle_ms(float distance_mm, uint16_t vel, uint8_t acc)
|
||||
{
|
||||
return actuator_mm_motion_time_ms(distance_mm, vel, acc) + ACTUATOR_SETTLE_MARGIN_MS;
|
||||
}
|
||||
|
||||
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);
|
||||
return actuator_angle_motion_time_ms(angle_deg, vel, acc) + ACTUATOR_SETTLE_MARGIN_MS;
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
@@ -378,15 +495,20 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
|
||||
(double)tgt_angle_deg,
|
||||
tgt_x, tgt_y);
|
||||
|
||||
/* 按实际行程/速度/加速度计算到位等待 (Z/X/Y 并发, 取最长者, 含安全裕量) */
|
||||
/* 到位轮询掩码: 只等待实际有行程的电机 (脉冲>0), 无行程直接视为已到位 */
|
||||
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);
|
||||
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 双电机 */
|
||||
|
||||
/* 纯运动时间 (无裕量), 仅作到位等待的兜底超时基准 */
|
||||
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);
|
||||
if (t_wait > src_wait_ms) src_wait_ms = t_wait;
|
||||
t_wait = actuator_settle_ms(src_pulses_y, vel_xy, acc_xy);
|
||||
t_wait = actuator_motion_time_ms(src_pulses_y, vel_xy, acc_xy);
|
||||
if (t_wait > src_wait_ms) src_wait_ms = t_wait;
|
||||
|
||||
/* ---- 1. 磁铁掉电 ---- */
|
||||
@@ -400,8 +522,8 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
|
||||
_frame_gap();
|
||||
_actuator_move_y_sync(src_pulses_y, vel_xy, acc_xy);
|
||||
|
||||
/* ---- 3. 等待运动到位 (红灯快闪, 按行程实时计算) ---- */
|
||||
_wait_flash(src_wait_ms, 128, 0, 0, 200);
|
||||
/* ---- 3. 等待运动到位 (轮询闭环到位标志, 红灯快闪) ---- */
|
||||
actuator_wait_arrival(src_wait_ms, src_mask);
|
||||
|
||||
/* ---- 4. 舵机放下 ---- */
|
||||
servo_set_down();
|
||||
@@ -420,12 +542,16 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
|
||||
终点朝向与旧协议 (绝对转到 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);
|
||||
uint32_t dst_pulses_z = (uint32_t)(fabsf(dst_rep - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f);
|
||||
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);
|
||||
|
||||
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);
|
||||
if (t_wait > dst_wait_ms) dst_wait_ms = t_wait;
|
||||
t_wait = actuator_settle_ms(dst_pulses_y, vel_xy, acc_xy);
|
||||
t_wait = actuator_motion_time_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);
|
||||
@@ -435,8 +561,8 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
|
||||
_frame_gap();
|
||||
_actuator_move_y_sync(dst_pulses_y, vel_xy, acc_xy);
|
||||
|
||||
/* ---- 8. 等待运动到位 (红灯快闪, 按行程实时计算) ---- */
|
||||
_wait_flash(dst_wait_ms, 128, 0, 0, 200);
|
||||
/* ---- 8. 等待运动到位 (轮询闭环到位标志, 红灯快闪) ---- */
|
||||
actuator_wait_arrival(dst_wait_ms, dst_mask);
|
||||
|
||||
/* ---- 9. 舵机放下 ---- */
|
||||
servo_set_down();
|
||||
|
||||
Reference in new issue
Block a user