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);
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
|
||||
@@ -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)
|
||||
|
||||
+22
-10
@@ -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 ...");
|
||||
|
||||
|
||||
Reference in new issue
Block a user