498 lines
16 KiB
C
498 lines
16 KiB
C
/**
|
|
* @file emm_v5.c
|
|
* @brief Emm_V5.0 步进闭环电机驱动器 - ESP-IDF 实现
|
|
*/
|
|
|
|
#include "emm_v5.h"
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "freertos/queue.h"
|
|
#include "freertos/semphr.h"
|
|
#include "freertos/event_groups.h"
|
|
#include "esp_timer.h"
|
|
|
|
static const char *TAG = "EMM_V5";
|
|
|
|
/* 运行时配置 (由 Emm_V5_Init 设置) */
|
|
static uart_port_t s_uart_port;
|
|
|
|
/* ============================================================
|
|
* RX 中断/事件驱动状态
|
|
*
|
|
* 驱动 Response 菜单设为 Reached/Both/Other 后, 位置模式 (0xFD) 命令
|
|
* 运动到位会主动回传到位帧: 地址 + FD + 9F + 6B。
|
|
* UART RX 由内部任务独占读取, 解析出到位帧后置位事件组位
|
|
* (bit = 电机地址 - 1); 调用方用 Emm_V5_Wait_Arrival 事件等待,
|
|
* 全部到位即返回 —— 不再轮询 S_TPOS/S_CPOS, 避免"没到位就下一步"。
|
|
* ============================================================ */
|
|
|
|
#define EMM_V5_ARRIVAL_MAX_MOTORS 8
|
|
|
|
static EventGroupHandle_t s_arrival_evt = NULL;
|
|
static QueueHandle_t s_uart_evt_queue = NULL;
|
|
static TaskHandle_t s_rx_task = NULL;
|
|
|
|
/* 请求-响应 (网页手动读参数等) */
|
|
static SemaphoreHandle_t s_resp_mutex = NULL;
|
|
static struct {
|
|
bool active;
|
|
uint8_t addr;
|
|
uint8_t func;
|
|
uint8_t *out;
|
|
uint8_t max;
|
|
uint8_t len;
|
|
SemaphoreHandle_t done;
|
|
} s_resp;
|
|
|
|
/* RX 帧累积缓冲 (0x6B 为帧尾) */
|
|
static uint8_t s_rx_frame[64];
|
|
static uint8_t s_rx_frame_len;
|
|
|
|
static void emm_v5_rx_task(void *arg); /* 前置声明 (Emm_V5_Init 中创建) */
|
|
|
|
/* ============================================================
|
|
* 初始化
|
|
* ============================================================ */
|
|
|
|
void Emm_V5_Init(uart_port_t port, int tx_pin, int rx_pin)
|
|
{
|
|
s_uart_port = port;
|
|
|
|
uart_config_t uart_cfg = {
|
|
.baud_rate = EMM_V5_UART_BAUDRATE,
|
|
.data_bits = UART_DATA_8_BITS,
|
|
.parity = UART_PARITY_DISABLE,
|
|
.stop_bits = UART_STOP_BITS_1,
|
|
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
|
.source_clk = UART_SCLK_DEFAULT,
|
|
};
|
|
|
|
/* 事件队列: 中断驱动 RX (到位帧解析); 64 深度防突发丢事件 */
|
|
s_uart_evt_queue = xQueueCreate(64, sizeof(uart_event_t));
|
|
|
|
ESP_ERROR_CHECK(uart_driver_install(s_uart_port,
|
|
EMM_V5_UART_BUF_SIZE * 2,
|
|
EMM_V5_UART_BUF_SIZE * 2,
|
|
16, &s_uart_evt_queue, 0));
|
|
ESP_ERROR_CHECK(uart_param_config(s_uart_port, &uart_cfg));
|
|
ESP_ERROR_CHECK(uart_set_pin(s_uart_port,
|
|
tx_pin,
|
|
rx_pin,
|
|
UART_PIN_NO_CHANGE,
|
|
UART_PIN_NO_CHANGE));
|
|
|
|
/* 到位事件组 / 请求-响应信号量 / RX 任务 */
|
|
if (s_arrival_evt == NULL) s_arrival_evt = xEventGroupCreate();
|
|
if (s_resp_mutex == NULL) s_resp_mutex = xSemaphoreCreateMutex();
|
|
if (s_resp.done == NULL) s_resp.done = xSemaphoreCreateBinary();
|
|
if (s_rx_task == NULL) {
|
|
xTaskCreate(emm_v5_rx_task, "emm_rx", 3072, NULL, 5, &s_rx_task);
|
|
}
|
|
|
|
ESP_LOGI(TAG, "初始化完成 (UART%d, TX=%d, RX=%d, 到位帧中断已启用)",
|
|
s_uart_port, tx_pin, rx_pin);
|
|
}
|
|
|
|
/* ============================================================
|
|
* 内部辅助
|
|
* ============================================================ */
|
|
|
|
static inline void Emm_V5_Send(const uint8_t *cmd, uint8_t len)
|
|
{
|
|
uart_write_bytes(s_uart_port, (const char *)cmd, len);
|
|
}
|
|
|
|
/* ============================================================
|
|
* RX 中断/事件驱动
|
|
*
|
|
* 帧格式: 地址 + 功能码 + 数据... + 0x6B (校验字节)
|
|
* 到位帧: 地址 + FD + 9F + 6B (位置模式命令运动到位后主动回传)
|
|
* ============================================================ */
|
|
|
|
/* 处理一帧完整数据 (以 0x6B 结尾) */
|
|
static void emm_v5_handle_frame(const uint8_t *f, uint8_t len)
|
|
{
|
|
if (len < 4) return;
|
|
|
|
/* 到位帧: 地址 + FD + 9F + 6B → 置位事件组 (bit = 地址 - 1) */
|
|
if (f[1] == 0xFD && f[2] == 0x9F) {
|
|
uint8_t addr = f[0];
|
|
if (addr >= 1 && addr <= EMM_V5_ARRIVAL_MAX_MOTORS && s_arrival_evt) {
|
|
xEventGroupSetBits(s_arrival_evt, (EventBits_t)(1u << (addr - 1)));
|
|
ESP_LOGI(TAG, "到位帧: 电机%d", addr);
|
|
}
|
|
}
|
|
|
|
/* 请求-响应匹配 (网页手动读参数等) */
|
|
if (s_resp_mutex) {
|
|
xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
|
|
if (s_resp.active && f[0] == s_resp.addr && f[1] == s_resp.func) {
|
|
uint8_t n = (len < s_resp.max) ? len : s_resp.max;
|
|
if (s_resp.out && n) memcpy(s_resp.out, f, n);
|
|
s_resp.len = n;
|
|
s_resp.active = false;
|
|
xSemaphoreGive(s_resp.done);
|
|
}
|
|
xSemaphoreGive(s_resp_mutex);
|
|
}
|
|
}
|
|
|
|
/* UART RX 独占任务: 事件队列驱动, 解析帧并分派 */
|
|
static void emm_v5_rx_task(void *arg)
|
|
{
|
|
uart_event_t evt;
|
|
uint8_t buf[64];
|
|
|
|
while (1) {
|
|
if (xQueueReceive(s_uart_evt_queue, &evt, portMAX_DELAY) != pdTRUE)
|
|
continue;
|
|
|
|
if (evt.type == UART_DATA || evt.type == UART_FIFO_OVF ||
|
|
evt.type == UART_BUFFER_FULL) {
|
|
int len;
|
|
while ((len = uart_read_bytes(s_uart_port, buf, sizeof(buf), 0)) > 0) {
|
|
for (int i = 0; i < len; i++) {
|
|
s_rx_frame[s_rx_frame_len++] = buf[i];
|
|
if (s_rx_frame_len >= sizeof(s_rx_frame)) {
|
|
s_rx_frame_len = 0; /* 缓冲溢出: 重新同步 */
|
|
}
|
|
if (buf[i] == 0x6B) { /* 帧尾 */
|
|
emm_v5_handle_frame(s_rx_frame, s_rx_frame_len);
|
|
s_rx_frame_len = 0;
|
|
}
|
|
}
|
|
}
|
|
if (evt.type == UART_FIFO_OVF || evt.type == UART_BUFFER_FULL) {
|
|
/* 溢出只说明有部分字节被丢弃: 重置帧累积重新同步即可。
|
|
不能 uart_flush_input —— 那会把缓冲里仍有效的到位帧一并冲掉,
|
|
是偶发丢电机2/3/4 到位帧的根因之一 */
|
|
s_rx_frame_len = 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/* 内部: 发送命令并等待匹配的响应帧 (地址+功能码), 供读函数复用 */
|
|
static int emm_v5_request_response(const uint8_t *cmd, uint8_t cmd_len,
|
|
uint8_t addr, uint8_t func,
|
|
uint8_t *out, uint8_t max,
|
|
uint32_t timeout_ms)
|
|
{
|
|
if (!s_resp_mutex || !s_resp.done) return -1;
|
|
|
|
xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
|
|
if (s_resp.active) {
|
|
xSemaphoreGive(s_resp_mutex);
|
|
ESP_LOGW(TAG, "请求-响应忙, 放弃 (addr=%u func=0x%02X)", addr, func);
|
|
return -1;
|
|
}
|
|
s_resp.active = true;
|
|
s_resp.addr = addr;
|
|
s_resp.func = func;
|
|
s_resp.out = out;
|
|
s_resp.max = max;
|
|
s_resp.len = 0;
|
|
xSemaphoreGive(s_resp_mutex);
|
|
|
|
xSemaphoreTake(s_resp.done, 0); /* 清残留计数 */
|
|
Emm_V5_Send(cmd, cmd_len);
|
|
|
|
if (xSemaphoreTake(s_resp.done, pdMS_TO_TICKS(timeout_ms)) != pdTRUE) {
|
|
xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
|
|
s_resp.active = false; /* 放弃等待 */
|
|
xSemaphoreGive(s_resp_mutex);
|
|
ESP_LOGW(TAG, "响应超时 (addr=%u func=0x%02X)", addr, func);
|
|
return -1;
|
|
}
|
|
|
|
xSemaphoreTake(s_resp_mutex, portMAX_DELAY);
|
|
int n = s_resp.len;
|
|
xSemaphoreGive(s_resp_mutex);
|
|
return n;
|
|
}
|
|
|
|
/* ============================================================
|
|
* 到位等待 (事件驱动)
|
|
*
|
|
* 注意: Emm_V5_Wait_Arrival 不消费到位位 (便于调用方分段等待时闪烁 LED);
|
|
* 一段 motion 的等待结束后必须调用 Emm_V5_Arrival_Consume 清除到位位,
|
|
* 防止残留位污染下一段 motion。
|
|
* ============================================================ */
|
|
|
|
bool Emm_V5_Wait_Arrival(const uint8_t *addrs, uint8_t count, uint32_t timeout_ms)
|
|
{
|
|
if (!s_arrival_evt) return false;
|
|
|
|
EventBits_t expected = 0;
|
|
for (uint8_t i = 0; i < count; i++) {
|
|
uint8_t a = addrs[i];
|
|
if (a >= 1 && a <= EMM_V5_ARRIVAL_MAX_MOTORS)
|
|
expected |= (EventBits_t)(1u << (a - 1));
|
|
}
|
|
if (expected == 0) return true;
|
|
|
|
EventBits_t got = xEventGroupWaitBits(s_arrival_evt, expected,
|
|
pdFALSE, pdTRUE,
|
|
pdMS_TO_TICKS(timeout_ms));
|
|
return ((got & expected) == expected);
|
|
}
|
|
|
|
void Emm_V5_Arrival_Consume(const uint8_t *addrs, uint8_t count)
|
|
{
|
|
if (!s_arrival_evt) return;
|
|
|
|
EventBits_t expected = 0;
|
|
for (uint8_t i = 0; i < count; i++) {
|
|
uint8_t a = addrs[i];
|
|
if (a >= 1 && a <= EMM_V5_ARRIVAL_MAX_MOTORS)
|
|
expected |= (EventBits_t)(1u << (a - 1));
|
|
}
|
|
if (expected)
|
|
xEventGroupClearBits(s_arrival_evt, expected);
|
|
}
|
|
|
|
void Emm_V5_Arrival_Reset(void)
|
|
{
|
|
if (!s_arrival_evt) return;
|
|
xEventGroupClearBits(s_arrival_evt, 0xFF); /* 清 addr 1~8 对应位 (bit=地址-1) */
|
|
}
|
|
|
|
/* 查询当前已置位的到位位 (调试用): 返回 addr1~8 对应位掩码 */
|
|
uint32_t Emm_V5_Arrival_Get_Bits(void)
|
|
{
|
|
if (!s_arrival_evt) return 0;
|
|
return xEventGroupGetBits(s_arrival_evt) & 0xFF;
|
|
}
|
|
|
|
/* ============================================================
|
|
* 接收数据 (已废弃)
|
|
* ============================================================ */
|
|
|
|
uint8_t Emm_V5_Receive_Data(uint8_t *rxCmd, uint8_t maxLen)
|
|
{
|
|
/* 已废弃: UART RX 由内部 RX 任务独占 (中断驱动), 此函数不再直接读 UART;
|
|
网页手动读参数请改用 Emm_V5_Read_Sys_Params_Resp */
|
|
(void)rxCmd;
|
|
(void)maxLen;
|
|
return 0;
|
|
}
|
|
|
|
/* ============================================================
|
|
* 控制命令 (协议与 Arduino 版本完全一致)
|
|
* ============================================================ */
|
|
|
|
void Emm_V5_Reset_CurPos_To_Zero(uint8_t addr)
|
|
{
|
|
uint8_t cmd[4] = {addr, 0x0A, 0x6D, 0x6B};
|
|
Emm_V5_Send(cmd, 4);
|
|
}
|
|
|
|
void Emm_V5_Reset_Clog_Pro(uint8_t addr)
|
|
{
|
|
uint8_t cmd[4] = {addr, 0x0E, 0x52, 0x6B};
|
|
Emm_V5_Send(cmd, 4);
|
|
}
|
|
|
|
/* 系统参数 → 功能码 (读取响应帧按 地址+功能码 匹配) */
|
|
static uint8_t emm_v5_sysparam_func(EmmSysParams_t s)
|
|
{
|
|
switch (s) {
|
|
case S_VER: return 0x1F;
|
|
case S_RL: return 0x20;
|
|
case S_PID: return 0x21;
|
|
case S_VBUS: return 0x24;
|
|
case S_CPHA: return 0x27;
|
|
case S_ENCL: return 0x31;
|
|
case S_TPOS: return 0x33;
|
|
case S_VEL: return 0x35;
|
|
case S_CPOS: return 0x36;
|
|
case S_PERR: return 0x37;
|
|
case S_FLAG: return 0x3A;
|
|
case S_ORG: return 0x3B;
|
|
case S_Conf: return 0x42;
|
|
case S_State: return 0x43;
|
|
default: return 0x00;
|
|
}
|
|
}
|
|
|
|
void Emm_V5_Read_Sys_Params(uint8_t addr, EmmSysParams_t s)
|
|
{
|
|
uint8_t cmd[8] = {0};
|
|
uint8_t i = 0;
|
|
uint8_t func = emm_v5_sysparam_func(s);
|
|
|
|
cmd[i++] = addr;
|
|
if (func) cmd[i++] = func;
|
|
if (s == S_Conf) cmd[i++] = 0x6C;
|
|
if (s == S_State) cmd[i++] = 0x7A;
|
|
cmd[i++] = 0x6B;
|
|
Emm_V5_Send(cmd, i);
|
|
}
|
|
|
|
uint8_t Emm_V5_Read_Sys_Params_Resp(uint8_t addr, EmmSysParams_t s,
|
|
uint8_t *rxOut, uint8_t maxLen,
|
|
uint32_t timeout_ms)
|
|
{
|
|
uint8_t cmd[8] = {0};
|
|
uint8_t i = 0;
|
|
uint8_t func = emm_v5_sysparam_func(s);
|
|
|
|
cmd[i++] = addr;
|
|
if (func) cmd[i++] = func;
|
|
if (s == S_Conf) cmd[i++] = 0x6C;
|
|
if (s == S_State) cmd[i++] = 0x7A;
|
|
cmd[i++] = 0x6B;
|
|
|
|
int n = emm_v5_request_response(cmd, i, addr, func, rxOut, maxLen,
|
|
(timeout_ms > 0) ? timeout_ms
|
|
: EMM_V5_RX_TIMEOUT_MS);
|
|
return (n > 0) ? (uint8_t)n : 0;
|
|
}
|
|
|
|
/* ============================================================
|
|
* 读取电机状态标志 (S_FLAG 0x3A) 并解析返回
|
|
* ============================================================ */
|
|
|
|
uint8_t Emm_V5_Read_Status_Flag(uint8_t addr, uint32_t timeout_ms)
|
|
{
|
|
uint8_t rx[8] = {0};
|
|
uint8_t cmd[3] = {addr, 0x3A, 0x6B};
|
|
uint8_t read_to = (timeout_ms > 0) ? timeout_ms : EMM_V5_RX_TIMEOUT_MS;
|
|
|
|
int n = emm_v5_request_response(cmd, sizeof(cmd), addr, 0x3A,
|
|
rx, sizeof(rx), read_to);
|
|
if (n >= 4 && rx[0] == addr && rx[1] == 0x3A && rx[3] == 0x6B)
|
|
return rx[2];
|
|
return 0xFF;
|
|
}
|
|
|
|
/* ============================================================
|
|
* 读取电机位置 (S_TPOS 0x33 / S_CPOS 0x36) 并解析返回
|
|
*
|
|
* 返回帧: 地址 + 功能码 + 符号 + 位置(4B) + 6B (共 8 字节)
|
|
* Emm 固件: 0~65535 对应 0~360°
|
|
* ============================================================ */
|
|
|
|
static int32_t Emm_V5_Read_Pos(uint8_t addr, uint8_t func)
|
|
{
|
|
uint8_t rx[16] = {0};
|
|
uint8_t cmd[3] = {addr, func, 0x6B};
|
|
|
|
int n = emm_v5_request_response(cmd, sizeof(cmd), addr, func,
|
|
rx, sizeof(rx), EMM_V5_RX_TIMEOUT_MS);
|
|
if (n >= 8 && rx[0] == addr && rx[1] == func &&
|
|
(rx[2] == 0 || rx[2] == 1) && rx[7] == 0x6B)
|
|
return (int32_t)(((uint32_t)rx[3] << 24) |
|
|
((uint32_t)rx[4] << 16) |
|
|
((uint32_t)rx[5] << 8) |
|
|
(uint32_t)rx[6]);
|
|
return -1;
|
|
}
|
|
|
|
int32_t Emm_V5_Read_TPOS(uint8_t addr)
|
|
{
|
|
return Emm_V5_Read_Pos(addr, 0x33);
|
|
}
|
|
|
|
int32_t Emm_V5_Read_CPOS(uint8_t addr)
|
|
{
|
|
return Emm_V5_Read_Pos(addr, 0x36);
|
|
}
|
|
|
|
void Emm_V5_Modify_Ctrl_Mode(uint8_t addr, bool svF, uint8_t ctrl_mode)
|
|
{
|
|
uint8_t cmd[6] = {addr, 0x46, 0x69, (uint8_t)svF, ctrl_mode, 0x6B};
|
|
Emm_V5_Send(cmd, 6);
|
|
}
|
|
|
|
void Emm_V5_En_Control(uint8_t addr, bool state, bool snF)
|
|
{
|
|
uint8_t cmd[6] = {addr, 0xF3, 0xAB, (uint8_t)state, (uint8_t)snF, 0x6B};
|
|
Emm_V5_Send(cmd, 6);
|
|
}
|
|
|
|
void Emm_V5_Vel_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, bool snF)
|
|
{
|
|
uint8_t cmd[8] = {
|
|
addr, 0xF6, dir,
|
|
(uint8_t)(vel >> 8), (uint8_t)(vel >> 0),
|
|
acc, (uint8_t)snF, 0x6B
|
|
};
|
|
Emm_V5_Send(cmd, 8);
|
|
}
|
|
|
|
void Emm_V5_Pos_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc,
|
|
uint32_t clk, bool raF, bool snF)
|
|
{
|
|
uint8_t cmd[13] = {
|
|
addr, 0xFD, dir,
|
|
(uint8_t)(vel >> 8), (uint8_t)(vel >> 0), acc,
|
|
(uint8_t)(clk >> 24), (uint8_t)(clk >> 16),
|
|
(uint8_t)(clk >> 8), (uint8_t)(clk >> 0),
|
|
(uint8_t)raF, (uint8_t)snF, 0x6B
|
|
};
|
|
Emm_V5_Send(cmd, 13);
|
|
}
|
|
|
|
void Emm_V5_Pos_Control_Angle(uint8_t addr, uint8_t dir, uint16_t vel,
|
|
uint8_t acc, float angle_deg, bool raF, bool snF)
|
|
{
|
|
/* 负角度 → 取绝对值 + 翻转方向 */
|
|
float abs_deg = fabsf(angle_deg);
|
|
uint32_t clk = EMM_V5_DEG_TO_CLK(abs_deg);
|
|
if (angle_deg < 0.0f) dir = (uint8_t)(!dir);
|
|
ESP_LOGD("EMM_V5", "角度→脉冲: %.1f° (%s) → %lu",
|
|
(double)angle_deg, dir ? "CCW" : "CW", (unsigned long)clk);
|
|
Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, snF);
|
|
}
|
|
|
|
void Emm_V5_Stop_Now(uint8_t addr, bool snF)
|
|
{
|
|
uint8_t cmd[5] = {addr, 0xFE, 0x98, (uint8_t)snF, 0x6B};
|
|
Emm_V5_Send(cmd, 5);
|
|
}
|
|
|
|
void Emm_V5_Synchronous_motion(uint8_t addr)
|
|
{
|
|
uint8_t cmd[4] = {addr, 0xFF, 0x66, 0x6B};
|
|
Emm_V5_Send(cmd, 4);
|
|
}
|
|
|
|
void Emm_V5_Origin_Set_O(uint8_t addr, bool svF)
|
|
{
|
|
uint8_t cmd[5] = {addr, 0x93, 0x88, (uint8_t)svF, 0x6B};
|
|
Emm_V5_Send(cmd, 5);
|
|
}
|
|
|
|
void Emm_V5_Origin_Modify_Params(uint8_t addr, bool svF, uint8_t o_mode,
|
|
uint8_t o_dir, uint16_t o_vel, uint32_t o_tm,
|
|
uint16_t sl_vel, uint16_t sl_ma, uint16_t sl_ms, bool potF)
|
|
{
|
|
uint8_t cmd[20] = {
|
|
addr, 0x4C, 0xAE, (uint8_t)svF,
|
|
o_mode, o_dir,
|
|
(uint8_t)(o_vel >> 8), (uint8_t)(o_vel >> 0),
|
|
(uint8_t)(o_tm >> 24), (uint8_t)(o_tm >> 16),
|
|
(uint8_t)(o_tm >> 8), (uint8_t)(o_tm >> 0),
|
|
(uint8_t)(sl_vel >> 8), (uint8_t)(sl_vel >> 0),
|
|
(uint8_t)(sl_ma >> 8), (uint8_t)(sl_ma >> 0),
|
|
(uint8_t)(sl_ms >> 8), (uint8_t)(sl_ms >> 0),
|
|
(uint8_t)potF, 0x6B
|
|
};
|
|
Emm_V5_Send(cmd, 20);
|
|
}
|
|
|
|
void Emm_V5_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF)
|
|
{
|
|
uint8_t cmd[5] = {addr, 0x9A, o_mode, (uint8_t)snF, 0x6B};
|
|
Emm_V5_Send(cmd, 5);
|
|
}
|
|
|
|
void Emm_V5_Origin_Interrupt(uint8_t addr)
|
|
{
|
|
uint8_t cmd[4] = {addr, 0x9C, 0x48, 0x6B};
|
|
Emm_V5_Send(cmd, 4);
|
|
}
|