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

release v0.9

Browse Source
This commit is contained in:
kirl committed 2026-08-01 05:05:25 +08:00
1 parent 0045c0c390
commit 14f2ff94d0
11 files changed
+337 -61
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

components/actuator/actuator.c
+139 -60
View File
@@ -12,6 +12,7 @@
#include "freertos/task.h"
#include "esp_log.h"
#include <math.h>
#include <string.h>
static const char *TAG = "ACTUATOR";
@@ -23,8 +24,16 @@ 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 旋转轴心 → 工具末端 偏移补偿 (mm) */
static float s_offset_x_mm = ACTUATOR_OFFSET_X_MM;
static float s_offset_y_mm = ACTUATOR_OFFSET_Y_MM;
static bool s_initialized = false;
/* 最近一次执行的 Cycle 快照 */
static edc_cycle_t s_last_cycle;
static bool s_last_cycle_valid = false;
/* ================================================================
* 内部: 帧间间隔
*
@@ -103,6 +112,67 @@ void actuator_set_scale(float x_scale, float y_scale, float angle_scale)
s_scale_x, s_scale_y, s_scale_angle);
}
/* ================================================================
* Z 旋转轴心 ↔ 工具末端 偏移补偿
*
* 终端执行机构与 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θ)
* ================================================================ */
void actuator_set_offset(float offset_x_mm, float offset_y_mm)
{
s_offset_x_mm = offset_x_mm;
s_offset_y_mm = offset_y_mm;
ESP_LOGI(TAG, "偏移补偿: dx=%.2fmm dy=%.2fmm", (double)offset_x_mm, (double)offset_y_mm);
}
void actuator_get_offset(float *offset_x_mm, float *offset_y_mm)
{
if (offset_x_mm) *offset_x_mm = s_offset_x_mm;
if (offset_y_mm) *offset_y_mm = s_offset_y_mm;
}
void actuator_apply_offset_compensation(uint16_t *target_x, uint16_t *target_y,
float angle_deg)
{
if (!target_x || !target_y) return;
/* 偏移量为零时跳过, 避免浮点运算 */
if (s_offset_x_mm == 0.0f && s_offset_y_mm == 0.0f) return;
float theta_rad = angle_deg * (float)(M_PI / 180.0);
float cos_t = cosf(theta_rad);
float sin_t = sinf(theta_rad);
/* R(θ)·(dx, dy): 旋转后的偏移向量 */
float rot_dx = s_offset_x_mm * cos_t - s_offset_y_mm * sin_t;
float rot_dy = s_offset_x_mm * sin_t + s_offset_y_mm * cos_t;
/* center = target - rotated_offset, 四舍五入 */
float cx = (float)(*target_x) - rot_dx;
float cy = (float)(*target_y) - rot_dy;
/* 钳位到有效范围并写回 */
if (cx < 0.0f) cx = 0.0f;
if (cy < 0.0f) cy = 0.0f;
*target_x = (uint16_t)(cx + 0.5f);
*target_y = (uint16_t)(cy + 0.5f);
ESP_LOGD(TAG, "补偿: (%.1f°) offset(%.2f,%.2f)→rot(%.2f,%.2f) "
"tgt→ctr (%u,%u)",
(double)angle_deg,
(double)s_offset_x_mm, (double)s_offset_y_mm,
(double)rot_dx, (double)rot_dy,
*target_x, *target_y);
}
/* ================================================================
* 内部: 单轴绝对位置运动
* ================================================================ */
@@ -148,77 +218,83 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
uint16_t vel_angle = 50;
uint8_t acc_angle = 100;
/* ---- 提取 source / target 坐标 (可被补偿修改) ---- */
uint16_t src_x = frag->source.x;
uint16_t src_y = frag->source.y;
uint16_t src_angle = frag->source.angle_x10;
uint16_t tgt_x = frag->target.x;
uint16_t tgt_y = frag->target.y;
uint16_t tgt_angle = frag->target.angle_x10;
/* Z 旋转轴心偏移补偿 (仅对 target 做) */
float tgt_angle_deg = (float)tgt_angle * 0.1f;
actuator_apply_offset_compensation(&tgt_x, &tgt_y, tgt_angle_deg);
/* source 坐标 → 脉冲数 */
uint32_t src_pulses_x = (uint32_t)(frag->source.x * s_scale_x);
uint32_t src_pulses_y = (uint32_t)(frag->source.y * s_scale_y);
uint32_t src_pulses_angle = (uint32_t)(frag->source.angle_x10 * s_scale_angle);
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)(frag->target.x * s_scale_x);
uint32_t dst_pulses_y = (uint32_t)(frag->target.y * s_scale_y);
uint32_t dst_pulses_angle = (uint32_t)(frag->target.angle_x10 * s_scale_angle);
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)",
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);
dst_pulses_x, dst_pulses_y, dst_pulses_angle,
tgt_x, tgt_y);
/* ---- 1. 磁铁掉电 ---- */
magnet_set(false);
/* ---- 2. 运动到 source 位置 ---- */
/* Angle / Z 旋转轴 */
_actuator_move_abs(ACTUATOR_ADDR_ANGLE, ACTUATOR_DIR_ANGLE,
src_pulses_angle, vel_angle, acc_angle, false);
_frame_gap();
/* X 轴 */
_actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X,
src_pulses_x, vel_xy, acc_xy, false);
_frame_gap();
/* Y 轴 (双电机同步) */
_actuator_move_y_sync(src_pulses_y, vel_xy, acc_xy);
/* ---- 3. 等待 2s 运动到位 (红灯快闪) ---- */
_wait_flash(2000, 128, 0, 0, 200);
/* ---- 3. 等待 4s 运动到位 (红灯快闪) ---- */
_wait_flash(4000, 128, 0, 0, 200);
/* ---- 4. 舵机放下 ---- */
servo_set_down();
_wait_flash(500, 128, 0, 0, 200);
_wait_flash(1000, 128, 0, 0, 200);
/* ---- 5. 磁铁吸合 ---- */
magnet_set(true);
_wait_flash(500, 128, 0, 0, 200);
_wait_flash(1000, 128, 0, 0, 200);
/* ---- 6. 舵机抬起 ---- */
servo_set_up();
_wait_flash(500, 128, 0, 0, 200);
_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);
_frame_gap();
_actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X,
dst_pulses_x, vel_xy, acc_xy, false);
_frame_gap();
_actuator_move_y_sync(dst_pulses_y, vel_xy, acc_xy);
/* ---- 8. 等待 2s 运动到位 (红灯快闪) ---- */
_wait_flash(2000, 128, 0, 0, 200);
/* ---- 8. 等待 4s 运动到位 (红灯快闪) ---- */
_wait_flash(4000, 128, 0, 0, 200);
/* ---- 9. 舵机放下 ---- */
servo_set_down();
_wait_flash(500, 128, 0, 0, 200);
_wait_flash(1000, 128, 0, 0, 200);
/* ---- 10. 磁铁掉电 ---- */
magnet_set(false);
_wait_flash(500, 128, 0, 0, 200);
_wait_flash(1000, 128, 0, 0, 200);
/* ---- 11. 舵机抬起 ---- */
servo_set_up();
_wait_flash(500, 128, 0, 0, 200);
_wait_flash(1000, 128, 0, 0, 200);
}
/* ================================================================
@@ -241,6 +317,10 @@ int actuator_execute_cycle(const edc_cycle_t *cycle)
return -1;
}
/* 保存快照 (执行前先存, 即使执行中也可查询) */
memcpy(&s_last_cycle, cycle, sizeof(edc_cycle_t));
s_last_cycle_valid = true;
ESP_LOGI(TAG, "=== Cycle #%u 开始 (%u fragments) ===",
cycle->cycle_id, cycle->frag_count);
@@ -253,6 +333,37 @@ int actuator_execute_cycle(const edc_cycle_t *cycle)
return 0;
}
/* ================================================================
* 协议回调: 收到 Cycle → 执行 → DONE
* ================================================================ */
void edc_proto_on_cycle(const edc_cycle_t *cycle, edc_channel_t ch)
{
static uint16_t s_last_exec_id = 0xFFFF;
ESP_LOGI(TAG, "Cycle #%u via %s, %u frags",
cycle->cycle_id, ch == EDC_CH_I2C ? "I2C" : "UART",
cycle->frag_count);
/* I2C / UART 冗余下发, 同一 cycle_id 只执行一次 */
if (cycle->cycle_id == s_last_exec_id) {
ESP_LOGD(TAG, "Cycle #%u 重复, 跳过执行", cycle->cycle_id);
return;
}
s_last_exec_id = cycle->cycle_id;
/* 执行电机运动 (阻塞, 含拾放全流程) */
ESP_LOGI(TAG, "开始执行 Cycle #%u ...", cycle->cycle_id);
actuator_execute_cycle(cycle);
/* 全部 Fragment 执行完成后回复 DONE */
edc_proto_send_done(cycle->cycle_id);
ESP_LOGI(TAG, "Cycle #%u 执行完毕, DONE 已发送", cycle->cycle_id);
/* 设置 RGB 状态为 DONE, 主循环将持续绿灯慢闪 */
rgb_set_state(RGB_STATE_DONE);
}
/* ================================================================
* 急停
* ================================================================ */
@@ -429,13 +540,9 @@ void actuator_motor_stop(uint8_t addr)
}
/* ================================================================
* 协议回调: 收到 Cycle → 执行 → DONE
* 最近 Cycle 快照查询
* ================================================================ */
static edc_cycle_t s_last_cycle;
static bool s_last_cycle_valid;
static portMUX_TYPE s_cycle_lock = portMUX_INITIALIZER_UNLOCKED;
const edc_cycle_t *actuator_get_last_cycle(void)
{
return s_last_cycle_valid ? &s_last_cycle : NULL;
@@ -446,35 +553,7 @@ bool actuator_is_last_cycle_valid(void)
return s_last_cycle_valid;
}
void edc_proto_on_cycle(const edc_cycle_t *cycle, edc_channel_t ch)
void actuator_clear_last_cycle(void)
{
static uint16_t s_last_exec_id = 0xFFFF;
/* 保存快照 */
portENTER_CRITICAL(&s_cycle_lock);
memcpy(&s_last_cycle, cycle, sizeof(edc_cycle_t));
s_last_cycle_valid = true;
portEXIT_CRITICAL(&s_cycle_lock);
ESP_LOGI(TAG, "Cycle #%u via %s, %u frags",
cycle->cycle_id, ch == EDC_CH_I2C ? "I2C" : "UART",
cycle->frag_count);
/* I2C / UART 冗余下发, 同一 cycle_id 只执行一次 */
if (cycle->cycle_id == s_last_exec_id) {
ESP_LOGD(TAG, "Cycle #%u 重复, 跳过执行", cycle->cycle_id);
return;
}
s_last_exec_id = cycle->cycle_id;
/* 执行电机运动 (阻塞, 含拾放全流程) */
ESP_LOGI(TAG, "开始执行 Cycle #%u ...", cycle->cycle_id);
actuator_execute_cycle(cycle);
/* 全部 Fragment 执行完成后回复 DONE */
edc_proto_send_done(cycle->cycle_id);
ESP_LOGI(TAG, "Cycle #%u 执行完毕, DONE 已发送", cycle->cycle_id);
/* 设置 RGB 状态为 DONE, 主循环将持续绿灯慢闪 */
rgb_set_state(RGB_STATE_DONE);
s_last_cycle_valid = false;
}
components/actuator/actuator.h
+52
View File
@@ -118,6 +118,26 @@ extern "C" {
/** 控制模式: 2 = 闭环位置模式 */
#define ACTUATOR_CTRL_MODE 2
/* ================================================================
* Z 旋转轴心 ↔ 工具末端 偏移补偿
*
* 终端执行机构 (工具末端) 与 Z 旋转机构不在同一条纵轴上,
* 旋转角度 θ 时, 工具末端的 XY 位置会绕旋转中心偏移。
*
* 补偿公式 (将工具末端定位到 target):
* X_motor = X_tgt - (dx * cosθ - dy * sinθ)
* Y_motor = Y_tgt - (dx * sinθ + dy * cosθ)
*
* dx, dy 为工具末端相对于 Z 旋转中心的 XY 偏移 (mm),
* 待机械测量后填入。
* ================================================================ */
#ifndef ACTUATOR_OFFSET_X_MM
#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 */
#endif
/**
* XY 平面左上零点坐标系 运动方向:
* X 轴正向 (→): CW (dir=0) ── 电机2
@@ -146,6 +166,32 @@ void actuator_init(void);
*/
void actuator_set_scale(float x_scale, float y_scale, float angle_scale);
/**
* @brief 设置 Z 旋转轴心 ↔ 工具末端 XY 偏移量 (mm)
* @param offset_x_mm 工具末端相对于旋转中心的 X 偏移 (mm)
* @param offset_y_mm 工具末端相对于旋转中心的 Y 偏移 (mm)
* @note 补偿会在每次 fragment 执行时自动应用;
* 未调用时默认使用 ACTUATOR_OFFSET_X_MM / ACTUATOR_OFFSET_Y_MM
*/
void actuator_set_offset(float offset_x_mm, float offset_y_mm);
/**
* @brief 获取当前 XY 偏移量
* @param offset_x_mm [out] X 偏移 (mm), 可为 NULL
* @param offset_y_mm [out] Y 偏移 (mm), 可为 NULL
*/
void actuator_get_offset(float *offset_x_mm, float *offset_y_mm);
/**
* @brief 对目标坐标应用 Z 旋转偏移补偿
* @param target_x [in/out] 目标 X 坐标 (mm)
* @param target_y [in/out] 目标 Y 坐标 (mm)
* @param angle_deg 当前旋转角度 (度)
* @note 内部使用, 也可由上层在手动模式调用
*/
void actuator_apply_offset_compensation(uint16_t *target_x, uint16_t *target_y,
float angle_deg);
/**
* @brief 执行一个完整 Cycle (遍历所有 Fragment, 逐段运动)
* @param cycle 协议下发的 Cycle 数据
@@ -241,6 +287,12 @@ const edc_cycle_t *actuator_get_last_cycle(void);
* @brief 最近 Cycle 是否有效
*/
bool actuator_is_last_cycle_valid(void);
/**
* @brief 清除最近 Cycle 记录 (用于等待新 Cycle)
*/
void actuator_clear_last_cycle(void);
#ifdef __cplusplus
}
#endif