release v1.0
This commit is contained in:
kirl committed
1 parent
4ce4a33603
commit
fef37b36e9
3 files changed
+185
-38
No files matched your search
+104
-23
@@ -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);
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
|
||||
Reference in new issue
Block a user