312 lines
9.8 KiB
C
312 lines
9.8 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 "esp_timer.h"
|
|
|
|
static const char *TAG = "EMM_V5";
|
|
|
|
/* 运行时配置 (由 Emm_V5_Init 设置) */
|
|
static uart_port_t s_uart_port;
|
|
|
|
/* ============================================================
|
|
* 初始化
|
|
* ============================================================ */
|
|
|
|
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,
|
|
};
|
|
|
|
ESP_ERROR_CHECK(uart_driver_install(s_uart_port,
|
|
EMM_V5_UART_BUF_SIZE * 2,
|
|
EMM_V5_UART_BUF_SIZE * 2,
|
|
0, NULL, 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));
|
|
|
|
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);
|
|
}
|
|
|
|
/* ============================================================
|
|
* 接收数据
|
|
* ============================================================ */
|
|
|
|
uint8_t Emm_V5_Receive_Data(uint8_t *rxCmd, uint8_t maxLen)
|
|
{
|
|
uint8_t count = 0;
|
|
int64_t last_time, now;
|
|
|
|
last_time = esp_timer_get_time();
|
|
|
|
while (1) {
|
|
int len = uart_read_bytes(s_uart_port, &rxCmd[count], 1,
|
|
pdMS_TO_TICKS(10));
|
|
if (len > 0) {
|
|
count++;
|
|
if (count >= maxLen) break;
|
|
last_time = esp_timer_get_time();
|
|
} else {
|
|
now = esp_timer_get_time();
|
|
if ((now - last_time) > (EMM_V5_RX_TIMEOUT_MS * 1000)) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
return count;
|
|
}
|
|
|
|
/* ============================================================
|
|
* 控制命令 (协议与 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);
|
|
}
|
|
|
|
void Emm_V5_Read_Sys_Params(uint8_t addr, EmmSysParams_t s)
|
|
{
|
|
uint8_t cmd[8] = {0};
|
|
uint8_t i = 0;
|
|
|
|
cmd[i++] = addr;
|
|
switch (s) {
|
|
case S_VER: cmd[i++] = 0x1F; break;
|
|
case S_RL: cmd[i++] = 0x20; break;
|
|
case S_PID: cmd[i++] = 0x21; break;
|
|
case S_VBUS: cmd[i++] = 0x24; break;
|
|
case S_CPHA: cmd[i++] = 0x27; break;
|
|
case S_ENCL: cmd[i++] = 0x31; break;
|
|
case S_TPOS: cmd[i++] = 0x33; break;
|
|
case S_VEL: cmd[i++] = 0x35; break;
|
|
case S_CPOS: cmd[i++] = 0x36; break;
|
|
case S_PERR: cmd[i++] = 0x37; break;
|
|
case S_FLAG: cmd[i++] = 0x3A; break;
|
|
case S_ORG: cmd[i++] = 0x3B; break;
|
|
case S_Conf: cmd[i++] = 0x42; cmd[i++] = 0x6C; break;
|
|
case S_State: cmd[i++] = 0x43; cmd[i++] = 0x7A; break;
|
|
default: break;
|
|
}
|
|
cmd[i++] = 0x6B;
|
|
Emm_V5_Send(cmd, i);
|
|
}
|
|
|
|
/* ============================================================
|
|
* 读取电机状态标志 (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;
|
|
|
|
/* 清掉历史残留 (运动命令的 ACK/到位帧等), 防止误判 */
|
|
uart_flush_input(s_uart_port);
|
|
|
|
Emm_V5_Send(cmd, 3);
|
|
|
|
uint8_t n = 0;
|
|
int64_t last = esp_timer_get_time();
|
|
int64_t deadline = last + (int64_t)read_to * 1000;
|
|
while (esp_timer_get_time() < deadline) {
|
|
int len = uart_read_bytes(s_uart_port, &rx[n], 1, pdMS_TO_TICKS(5));
|
|
if (len > 0) {
|
|
if (n < sizeof(rx) - 1) n++;
|
|
last = esp_timer_get_time();
|
|
/* 帧收完整 (地址 + 3A + 状态 + 6B) 立即返回, 不再等空闲超时,
|
|
把单次轮询从 ~17ms 压到 ~3ms, 减小到位检测抖动 */
|
|
for (int i = 0; i + 3 < (int)n; i++) {
|
|
if (rx[i] == addr && rx[i + 1] == 0x3A && rx[i + 3] == 0x6B)
|
|
return rx[i + 2];
|
|
}
|
|
} else if ((esp_timer_get_time() - last) > 5000) {
|
|
break; /* 5ms 无新字节: 认为帧已收完/无响应 */
|
|
}
|
|
}
|
|
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};
|
|
uint8_t read_to = EMM_V5_RX_TIMEOUT_MS;
|
|
|
|
/* 清掉历史残留 (ACK/到位帧等), 防止误判 */
|
|
uart_flush_input(s_uart_port);
|
|
|
|
Emm_V5_Send(cmd, 3);
|
|
|
|
uint8_t n = 0;
|
|
int64_t last = esp_timer_get_time();
|
|
int64_t deadline = last + (int64_t)read_to * 1000;
|
|
while (esp_timer_get_time() < deadline) {
|
|
int len = uart_read_bytes(s_uart_port, &rx[n], 1, pdMS_TO_TICKS(5));
|
|
if (len > 0) {
|
|
if (n < sizeof(rx) - 1) n++;
|
|
last = esp_timer_get_time();
|
|
/* 帧收完整 (地址+功能码+符号+位置4B+6B) 立即返回 */
|
|
for (int i = 0; i + 7 < (int)n; i++) {
|
|
if (rx[i] == addr && rx[i + 1] == func &&
|
|
(rx[i + 2] == 0 || rx[i + 2] == 1) && rx[i + 7] == 0x6B)
|
|
return (int32_t)(((uint32_t)rx[i + 3] << 24) |
|
|
((uint32_t)rx[i + 4] << 16) |
|
|
((uint32_t)rx[i + 5] << 8) |
|
|
(uint32_t)rx[i + 6]);
|
|
}
|
|
} else if ((esp_timer_get_time() - last) > 5000) {
|
|
break; /* 5ms 无新字节: 认为帧已收完/无响应 */
|
|
}
|
|
}
|
|
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);
|
|
}
|