365 lines
13 KiB
C
365 lines
13 KiB
C
/**
|
|
* @file actuator.c
|
|
* @brief 执行器应用层封装实现
|
|
*/
|
|
|
|
#include "actuator.h"
|
|
#include "emm_v5.h"
|
|
#include "servo.h"
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "esp_log.h"
|
|
#include <math.h>
|
|
|
|
static const char *TAG = "ACTUATOR";
|
|
|
|
/* ================================================================
|
|
* 运行时状态
|
|
* ================================================================ */
|
|
|
|
static float s_scale_x = ACTUATOR_SCALE_X;
|
|
static float s_scale_y = ACTUATOR_SCALE_Y;
|
|
static float s_scale_angle = ACTUATOR_SCALE_ANGLE;
|
|
|
|
static bool s_initialized = false;
|
|
|
|
/* ================================================================
|
|
* 内部: 帧间间隔
|
|
*
|
|
* Emm_V5 驱动板无法解析 UART 上零间隔的背靠背连续帧 (会丢字节,
|
|
* 导致本帧与后续帧全部失效), 凡连发多帧必须插入该延时。
|
|
* 曾导致故障: Y 轴 3/4 号电机使能 (单帧) 正常, 速度/位置 (连发多帧) 无反应
|
|
* ================================================================ */
|
|
|
|
static inline void _frame_gap(void)
|
|
{
|
|
vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS));
|
|
}
|
|
|
|
/* ================================================================
|
|
* 初始化
|
|
* ================================================================ */
|
|
|
|
void actuator_init(void)
|
|
{
|
|
/* 初始化电机驱动 UART */
|
|
Emm_V5_Init(ACTUATOR_UART_PORT, ACTUATOR_UART_TX, ACTUATOR_UART_RX);
|
|
|
|
/* 初始化舵机 PWM */
|
|
servo_init(ACTUATOR_SERVO_GPIO);
|
|
|
|
/* 配置 4 路电机: 闭环位置模式 + 使能 */
|
|
const uint8_t addrs[] = {
|
|
ACTUATOR_ADDR_ANGLE, ACTUATOR_ADDR_X,
|
|
ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2
|
|
};
|
|
for (int i = 0; i < 4; i++) {
|
|
uint8_t addr = addrs[i];
|
|
Emm_V5_Modify_Ctrl_Mode(addr, false, ACTUATOR_CTRL_MODE);
|
|
vTaskDelay(pdMS_TO_TICKS(20));
|
|
Emm_V5_En_Control(addr, true, false);
|
|
vTaskDelay(pdMS_TO_TICKS(20));
|
|
}
|
|
|
|
s_initialized = true;
|
|
ESP_LOGI(TAG, "初始化完成 (Angle=addr%d X=addr%d Y=addr%d/%d, servo=GPIO%d)",
|
|
ACTUATOR_ADDR_ANGLE,
|
|
ACTUATOR_ADDR_X,
|
|
ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2,
|
|
ACTUATOR_SERVO_GPIO);
|
|
}
|
|
|
|
/* ================================================================
|
|
* 脉冲换算
|
|
* ================================================================ */
|
|
|
|
void actuator_set_scale(float x_scale, float y_scale, float angle_scale)
|
|
{
|
|
s_scale_x = x_scale;
|
|
s_scale_y = y_scale;
|
|
s_scale_angle = angle_scale;
|
|
ESP_LOGI(TAG, "脉冲比例: X=%.3f Y=%.3f Angle=%.3f",
|
|
s_scale_x, s_scale_y, s_scale_angle);
|
|
}
|
|
|
|
/* ================================================================
|
|
* 内部: 单轴绝对位置运动
|
|
* ================================================================ */
|
|
|
|
static void _actuator_move_abs(uint8_t addr, uint8_t dir, uint32_t pulses,
|
|
uint16_t vel, uint8_t acc, bool snF)
|
|
{
|
|
Emm_V5_Pos_Control(addr, dir, vel, acc,
|
|
pulses,
|
|
true, /* raF=true: 绝对运动 */
|
|
snF); /* 多机同步标志 */
|
|
}
|
|
|
|
/* ================================================================
|
|
* 内部: Y 轴双电机同步运动
|
|
* ================================================================ */
|
|
|
|
static void _actuator_move_y_sync(uint32_t pulses_y)
|
|
{
|
|
uint16_t vel = ACTUATOR_DEFAULT_VEL;
|
|
uint8_t acc = ACTUATOR_DEFAULT_ACC;
|
|
|
|
/* Y 轴正向 = CCW (dir=1), 双电机同步 */
|
|
_actuator_move_abs(ACTUATOR_ADDR_Y1, ACTUATOR_DIR_Y, pulses_y, vel, acc, true);
|
|
_frame_gap();
|
|
_actuator_move_abs(ACTUATOR_ADDR_Y2, ACTUATOR_DIR_Y, pulses_y, vel, acc, true);
|
|
_frame_gap();
|
|
|
|
/* 广播地址触发一次, 两台电机同时开始运动 */
|
|
Emm_V5_Synchronous_motion(0);
|
|
|
|
ESP_LOGD(TAG, "Y 同步(CCW): addr%d+%d -> %lu pulses (%.2fmm)",
|
|
ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2, pulses_y,
|
|
(double)pulses_y / (double)ACTUATOR_SCALE_Y);
|
|
}
|
|
|
|
/* ================================================================
|
|
* 内部: 执行单个 Fragment
|
|
* ================================================================ */
|
|
|
|
static void _actuator_move_fragment(const edc_fragment_t *frag)
|
|
{
|
|
uint16_t vel = ACTUATOR_DEFAULT_VEL;
|
|
uint8_t acc = ACTUATOR_DEFAULT_ACC;
|
|
|
|
/* target 坐标 → 脉冲数 */
|
|
uint32_t pulses_x = (uint32_t)(frag->target.x * s_scale_x);
|
|
uint32_t pulses_y = (uint32_t)(frag->target.y * s_scale_y);
|
|
uint32_t pulses_angle = (uint32_t)(frag->target.angle_x10 * s_scale_angle);
|
|
|
|
ESP_LOGI(TAG, "Fragment: X=%lu Y=%lu Angle=%lu (raw x=%u y=%u a=%u)",
|
|
pulses_x, pulses_y, pulses_angle,
|
|
frag->target.x, frag->target.y, frag->target.angle_x10);
|
|
|
|
/* Angle / Z 旋转轴 (独立, CW) */
|
|
_actuator_move_abs(ACTUATOR_ADDR_ANGLE, ACTUATOR_DIR_ANGLE, pulses_angle, vel, acc, false);
|
|
_frame_gap();
|
|
|
|
/* X 轴 (独立, CW) */
|
|
_actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X, pulses_x, vel, acc, false);
|
|
_frame_gap();
|
|
|
|
/* Y 轴 (双电机同步) */
|
|
_actuator_move_y_sync(pulses_y);
|
|
}
|
|
|
|
/* ================================================================
|
|
* 执行完整 Cycle
|
|
* ================================================================ */
|
|
|
|
int actuator_execute_cycle(const edc_cycle_t *cycle)
|
|
{
|
|
if (!s_initialized) {
|
|
ESP_LOGE(TAG, "未初始化, 无法执行 Cycle");
|
|
return -1;
|
|
}
|
|
if (!cycle) {
|
|
ESP_LOGE(TAG, "Cycle 指针为空");
|
|
return -1;
|
|
}
|
|
if (cycle->frag_count < EDC_FRAG_MIN || cycle->frag_count > EDC_FRAG_MAX) {
|
|
ESP_LOGE(TAG, "Fragment 数量无效: %u (需 %d~%d)",
|
|
cycle->frag_count, EDC_FRAG_MIN, EDC_FRAG_MAX);
|
|
return -1;
|
|
}
|
|
|
|
ESP_LOGI(TAG, "=== Cycle #%u 开始 (%u fragments) ===",
|
|
cycle->cycle_id, cycle->frag_count);
|
|
|
|
for (int i = 0; i < cycle->frag_count; i++) {
|
|
ESP_LOGI(TAG, "-- Fragment %d/%d --", i + 1, cycle->frag_count);
|
|
_actuator_move_fragment(&cycle->fragments[i]);
|
|
|
|
/* 等待当前片段运动完成 (TODO: 改为轮询到位标志) */
|
|
if (i < cycle->frag_count - 1) {
|
|
vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAG_DELAY_MS));
|
|
}
|
|
}
|
|
|
|
/* 最后一个 fragment 也等待 */
|
|
vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAG_DELAY_MS));
|
|
|
|
ESP_LOGI(TAG, "=== Cycle #%u 完成 ===", cycle->cycle_id);
|
|
return 0;
|
|
}
|
|
|
|
/* ================================================================
|
|
* 急停
|
|
* ================================================================ */
|
|
|
|
void actuator_stop_all(void)
|
|
{
|
|
const uint8_t addrs[] = {
|
|
ACTUATOR_ADDR_ANGLE, ACTUATOR_ADDR_X,
|
|
ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2
|
|
};
|
|
_frame_gap(); /* 防止与上一帧 UART 冲突 */
|
|
for (int i = 0; i < 4; i++) {
|
|
Emm_V5_Stop_Now(addrs[i], false);
|
|
_frame_gap();
|
|
}
|
|
ESP_LOGW(TAG, "全部电机急停");
|
|
}
|
|
|
|
/* ================================================================
|
|
* 舵机控制
|
|
* ================================================================ */
|
|
|
|
void actuator_servo_up(void)
|
|
{
|
|
servo_set_up();
|
|
ESP_LOGD(TAG, "舵机: 抬起");
|
|
}
|
|
|
|
void actuator_servo_down(void)
|
|
{
|
|
servo_set_down();
|
|
ESP_LOGD(TAG, "舵机: 放下");
|
|
}
|
|
|
|
void actuator_servo_set_angle(float angle)
|
|
{
|
|
servo_set_angle(angle);
|
|
ESP_LOGD(TAG, "舵机: %.1f°", (double)angle);
|
|
}
|
|
|
|
/* ================================================================
|
|
* 单电机手动控制 (Y 轴自动同步)
|
|
* ================================================================ */
|
|
|
|
static inline bool _is_y_motor(uint8_t addr)
|
|
{
|
|
return (addr == ACTUATOR_ADDR_Y1 || addr == ACTUATOR_ADDR_Y2);
|
|
}
|
|
|
|
void actuator_motor_enable(uint8_t addr, bool state)
|
|
{
|
|
_frame_gap(); /* 防止与上一帧 UART 冲突 */
|
|
Emm_V5_En_Control(addr, state, false);
|
|
ESP_LOGI(TAG, "电机 %d %s", addr, state ? "使能" : "关闭");
|
|
}
|
|
|
|
void actuator_motor_vel(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc)
|
|
{
|
|
_frame_gap(); /* 防止与上一帧 UART 冲突 */
|
|
Emm_V5_Vel_Control(addr, dir, vel, acc, false);
|
|
ESP_LOGI(TAG, "速度: 电机%d %s %dRPM acc=%d",
|
|
addr, dir ? "CCW" : "CW", vel, acc);
|
|
|
|
/* Y 轴同步: 对端电机同样命令 (帧间留间隔) */
|
|
if (_is_y_motor(addr)) {
|
|
uint8_t peer = (addr == ACTUATOR_ADDR_Y1)
|
|
? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1;
|
|
_frame_gap();
|
|
Emm_V5_Vel_Control(peer, dir, vel, acc, false);
|
|
ESP_LOGD(TAG, "Y 同步: 电机%d 同速", peer);
|
|
}
|
|
}
|
|
|
|
void actuator_motor_pos(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc,
|
|
uint32_t clk, bool raF)
|
|
{
|
|
_frame_gap(); /* 防止与上一帧 UART 冲突 */
|
|
if (_is_y_motor(addr)) {
|
|
/* Y 轴双电机同步位置 (帧间留间隔, 广播地址触发一次) */
|
|
uint8_t peer = (addr == ACTUATOR_ADDR_Y1)
|
|
? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1;
|
|
Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, true);
|
|
_frame_gap();
|
|
Emm_V5_Pos_Control(peer, dir, vel, acc, clk, raF, true);
|
|
_frame_gap();
|
|
Emm_V5_Synchronous_motion(0);
|
|
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);
|
|
ESP_LOGI(TAG, "位置: 电机%d %s %dRPM %lu脉冲 %s",
|
|
addr, dir ? "CCW" : "CW", vel, clk,
|
|
raF ? "绝对" : "相对");
|
|
}
|
|
}
|
|
|
|
void actuator_motor_angle(uint8_t addr, float angle_deg, uint16_t vel,
|
|
uint8_t acc, bool raF)
|
|
{
|
|
_frame_gap(); /* 防止与上一帧 UART 冲突 */
|
|
/* 方向由 Emm_V5_Pos_Control_Angle 根据角度正负自动翻转 */
|
|
uint8_t dir = ACTUATOR_DIR_ANGLE;
|
|
|
|
if (_is_y_motor(addr)) {
|
|
uint8_t peer = (addr == ACTUATOR_ADDR_Y1)
|
|
? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1;
|
|
Emm_V5_Pos_Control_Angle(addr, dir, vel, acc, angle_deg, raF, true);
|
|
_frame_gap();
|
|
Emm_V5_Pos_Control_Angle(peer, dir, vel, acc, angle_deg, raF, true);
|
|
_frame_gap();
|
|
Emm_V5_Synchronous_motion(0);
|
|
ESP_LOGI(TAG, "角度(Y同步): 电机%d+%d %.1f° %s vel=%dRPM",
|
|
addr, peer, (double)angle_deg,
|
|
raF ? "绝对" : "相对", vel);
|
|
} else {
|
|
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,
|
|
raF ? "绝对" : "相对", vel);
|
|
}
|
|
}
|
|
|
|
void actuator_motor_mm(uint8_t addr, float distance_mm, uint16_t vel,
|
|
uint8_t acc, bool raF)
|
|
{
|
|
/* 软限位钳位 */
|
|
if (_is_y_motor(addr)) {
|
|
if (distance_mm < ACTUATOR_LIMIT_Y_MIN) distance_mm = ACTUATOR_LIMIT_Y_MIN;
|
|
if (distance_mm > ACTUATOR_LIMIT_Y_MAX) distance_mm = ACTUATOR_LIMIT_Y_MAX;
|
|
} else {
|
|
if (distance_mm < ACTUATOR_LIMIT_X_MIN) distance_mm = ACTUATOR_LIMIT_X_MIN;
|
|
if (distance_mm > ACTUATOR_LIMIT_X_MAX) distance_mm = ACTUATOR_LIMIT_X_MAX;
|
|
}
|
|
|
|
_frame_gap(); /* 防止与上一帧 UART 冲突 */
|
|
uint32_t clk = ACTUATOR_MM_TO_PULSES(distance_mm);
|
|
/* Y 轴正向 = CCW, X 轴正向 = CW */
|
|
uint8_t dir = _is_y_motor(addr) ? ACTUATOR_DIR_Y : ACTUATOR_DIR_X;
|
|
|
|
if (_is_y_motor(addr)) {
|
|
uint8_t peer = (addr == ACTUATOR_ADDR_Y1)
|
|
? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1;
|
|
Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, true);
|
|
_frame_gap();
|
|
Emm_V5_Pos_Control(peer, dir, vel, acc, clk, raF, true);
|
|
_frame_gap();
|
|
Emm_V5_Synchronous_motion(0);
|
|
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);
|
|
ESP_LOGI(TAG, "直线(%s): 电机%d %.2fmm (%lu脉冲) %s vel=%dRPM",
|
|
dir ? "CCW" : "CW", addr, (double)distance_mm,
|
|
(unsigned long)clk, raF ? "绝对" : "相对", vel);
|
|
}
|
|
}
|
|
|
|
void actuator_motor_stop(uint8_t addr)
|
|
{
|
|
_frame_gap(); /* 防止与上一帧 UART 冲突 */
|
|
Emm_V5_Stop_Now(addr, false);
|
|
ESP_LOGI(TAG, "停止: 电机%d", addr);
|
|
|
|
/* Y 轴同步停止 (帧间留间隔) */
|
|
if (_is_y_motor(addr)) {
|
|
uint8_t peer = (addr == ACTUATOR_ADDR_Y1)
|
|
? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1;
|
|
_frame_gap();
|
|
Emm_V5_Stop_Now(peer, false);
|
|
ESP_LOGD(TAG, "Y 同步: 电机%d 同停", peer);
|
|
}
|
|
}
|