You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
512 lines
18 KiB
512 lines
18 KiB
/******************************************************************************
|
|
|
|
版权所有 (C), 2018-2099, Radkil
|
|
|
|
******************************************************************************
|
|
文 件 名 : motor_manager.c
|
|
版 本 号 : 初稿
|
|
作 者 : radkil
|
|
生成日期 : 2026年7月13日
|
|
最近修改 :
|
|
功能描述 : 电机管理模块（多实例管理，协议无关）
|
|
|
|
修改历史 :
|
|
1.日 期 : 2026年7月13日
|
|
作 者 : radkil
|
|
修改内容 : 创建文件
|
|
|
|
******************************************************************************/
|
|
|
|
#include "motor_manager.h"
|
|
|
|
/*----------------------------------------------*
|
|
* 外部变量说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 外部函数原型说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 内部函数原型说明 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 全局变量 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 模块级变量 *
|
|
*----------------------------------------------*/
|
|
static rd_slist_t g_stMotorList; // 电机链表头
|
|
|
|
/*----------------------------------------------*
|
|
* 常量定义 *
|
|
*----------------------------------------------*/
|
|
|
|
/*----------------------------------------------*
|
|
* 宏定义 *
|
|
*----------------------------------------------*/
|
|
|
|
// 宏：检查协议函数是否存在并调用
|
|
#define MOTOR_PROTOCOL_CALL(pstMotor, func, ...) \
|
|
((pstMotor)->m_pstProtocol && (pstMotor)->m_pstProtocol->func) ? \
|
|
(pstMotor)->m_pstProtocol->func(pstMotor, ##__VA_ARGS__) : RD_NULL
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_Init
|
|
功能描述 : 初始化电机管理模块
|
|
输入参数 : 无
|
|
输出参数 : 无
|
|
返 回 值 : void
|
|
调用函数 :
|
|
被调函数 :
|
|
|
|
修改历史 :
|
|
1.日 期 : 2026年7月13日
|
|
作 者 : radkil
|
|
修改内容 : 新生成函数
|
|
|
|
*****************************************************************************/
|
|
void MotorMgr_Init(void)
|
|
{
|
|
rd_slist_init(&g_stMotorList);
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_Create
|
|
功能描述 : 创建电机实例
|
|
输入参数 : const Motor_Config_t *pstConfig 电机配置
|
|
TComCtrl *ptCAN 绑定的CAN控制器
|
|
const Motor_Protocol_t *pstProtocol 协议操作函数表
|
|
void *pstPrivData 协议私有数据
|
|
输出参数 : 无
|
|
返 回 值 : Motor_Instance_t* 电机实例指针
|
|
调用函数 :
|
|
被调函数 :
|
|
|
|
修改历史 :
|
|
1.日 期 : 2026年7月13日
|
|
作 者 : radkil
|
|
修改内容 : 新生成函数
|
|
|
|
*****************************************************************************/
|
|
Motor_Instance_t* MotorMgr_Create(const Motor_Config_t *pstConfig,
|
|
TComCtrl *ptCAN,
|
|
const Motor_Protocol_t *pstProtocol,
|
|
void *pstPrivData)
|
|
{
|
|
if (NULL == pstConfig || NULL == ptCAN || NULL == pstProtocol)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
// 检查是否已存在相同ID的电机
|
|
rd_slist_t *pos;
|
|
rd_slist_t *n;
|
|
rd_slist_for_each(pos, n, &g_stMotorList)
|
|
{
|
|
Motor_Instance_t *pstCheck = rd_slist_entry(pos, Motor_Instance_t, node);
|
|
if (pstCheck->m_stConfig.m_ucMotorID == pstConfig->m_ucMotorID)
|
|
{
|
|
return NULL; // ID重复
|
|
}
|
|
}
|
|
|
|
// 分配内存
|
|
Motor_Instance_t *pstMotor = (Motor_Instance_t *)RD_CALLOC(1, sizeof(Motor_Instance_t));
|
|
if (NULL == pstMotor)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
// 初始化链表节点
|
|
pstMotor->node.next = NULL;
|
|
|
|
// 复制配置
|
|
pstMotor->m_stConfig.m_ucMotorID = pstConfig->m_ucMotorID;
|
|
pstMotor->m_stConfig.m_uiPulsePerRound = pstConfig->m_uiPulsePerRound > 0 ? pstConfig->m_uiPulsePerRound : 10000;
|
|
pstMotor->m_stConfig.m_uiReductionRatio = pstConfig->m_uiReductionRatio > 0 ? pstConfig->m_uiReductionRatio : 70;
|
|
pstMotor->m_stConfig.m_fWheelDiameter = pstConfig->m_fWheelDiameter > 0 ? pstConfig->m_fWheelDiameter : 0.26f;
|
|
|
|
// 初始化运行数据
|
|
pstMotor->m_stData.m_iRealPosition = 0;
|
|
pstMotor->m_stData.m_iRealVelocity = 0;
|
|
pstMotor->m_stData.m_iRealTorque = 0;
|
|
pstMotor->m_stData.m_uiFaultCode = 0;
|
|
pstMotor->m_stData.m_dRealDistance = 0.0;
|
|
pstMotor->m_stData.m_iStartMeasurePos = 0;
|
|
pstMotor->m_stData.m_iLastPosition = 0;
|
|
pstMotor->m_stData.m_iRoundCount = 0;
|
|
pstMotor->m_stData.m_ucStartMeasureFlag = 1;
|
|
|
|
// 绑定CAN控制器和协议
|
|
pstMotor->m_ptCAN = ptCAN;
|
|
pstMotor->m_eState = MOTOR_STATE_IDLE;
|
|
pstMotor->m_uiLastHeartbeat = 0;
|
|
pstMotor->m_pstProtocol = pstProtocol;
|
|
pstMotor->m_pstPrivData = pstPrivData;
|
|
|
|
// 设置实例名称
|
|
RD_SNPRINTF(pstMotor->m_szName, MOTOR_MAX_NAME_LEN, "Motor_%d", pstConfig->m_ucMotorID);
|
|
|
|
// 添加到链表尾部
|
|
rd_slist_append(&g_stMotorList, &pstMotor->node);
|
|
|
|
return pstMotor;
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_Destroy
|
|
功能描述 : 销毁电机实例
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
调用函数 :
|
|
被调函数 :
|
|
|
|
修改历史 :
|
|
1.日 期 : 2026年7月13日
|
|
作 者 : radkil
|
|
修改内容 : 新生成函数
|
|
|
|
*****************************************************************************/
|
|
int MotorMgr_Destroy(Motor_Instance_t *pstMotor)
|
|
{
|
|
if (NULL == pstMotor)
|
|
{
|
|
return RD_NULL;
|
|
}
|
|
|
|
// 从链表中移除
|
|
rd_slist_remove(&g_stMotorList, &pstMotor->node);
|
|
RD_FREE(pstMotor);
|
|
|
|
return RD_SUCCESS;
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_FindByName
|
|
功能描述 : 通过名称查找电机实例
|
|
输入参数 : const char *pszName 实例名称
|
|
输出参数 : 无
|
|
返 回 值 : Motor_Instance_t* 电机实例指针
|
|
调用函数 :
|
|
被调函数 :
|
|
|
|
修改历史 :
|
|
1.日 期 : 2026年7月13日
|
|
作 者 : radkil
|
|
修改内容 : 新生成函数
|
|
|
|
*****************************************************************************/
|
|
Motor_Instance_t* MotorMgr_FindByName(const char *pszName)
|
|
{
|
|
if (NULL == pszName)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
rd_slist_t *pos;
|
|
rd_slist_t *n;
|
|
rd_slist_for_each(pos, n, &g_stMotorList)
|
|
{
|
|
Motor_Instance_t *pstMotor = rd_slist_entry(pos, Motor_Instance_t, node);
|
|
if (RD_STRCMP(pstMotor->m_szName, pszName) == 0)
|
|
{
|
|
return pstMotor;
|
|
}
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_FindByID
|
|
功能描述 : 通过ID查找电机实例
|
|
输入参数 : uint8_t ucMotorID 电机节点ID
|
|
输出参数 : 无
|
|
返 回 值 : Motor_Instance_t* 电机实例指针
|
|
调用函数 :
|
|
被调函数 :
|
|
|
|
修改历史 :
|
|
1.日 期 : 2026年7月13日
|
|
作 者 : radkil
|
|
修改内容 : 新生成函数
|
|
|
|
*****************************************************************************/
|
|
Motor_Instance_t* MotorMgr_FindByID(uint8_t ucMotorID)
|
|
{
|
|
rd_slist_t *pos;
|
|
rd_slist_t *n;
|
|
rd_slist_for_each(pos, n, &g_stMotorList)
|
|
{
|
|
Motor_Instance_t *pstMotor = rd_slist_entry(pos, Motor_Instance_t, node);
|
|
if (pstMotor->m_stConfig.m_ucMotorID == ucMotorID)
|
|
{
|
|
return pstMotor;
|
|
}
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_ResetAll
|
|
功能描述 : 复位所有节点
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
*****************************************************************************/
|
|
int MotorMgr_ResetAll(Motor_Instance_t *pstMotor)
|
|
{
|
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfResetAll);
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_ActivateAll
|
|
功能描述 : 激活所有节点
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
*****************************************************************************/
|
|
int MotorMgr_ActivateAll(Motor_Instance_t *pstMotor)
|
|
{
|
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfActivateAll);
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_SetHome
|
|
功能描述 : 设置当前位置为零点
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
*****************************************************************************/
|
|
int MotorMgr_SetHome(Motor_Instance_t *pstMotor)
|
|
{
|
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfSetHome);
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_SetWatchdog
|
|
功能描述 : 设置看门狗时间
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
uint32_t uiTimeMs 毫秒
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
*****************************************************************************/
|
|
int MotorMgr_SetWatchdog(Motor_Instance_t *pstMotor, uint32_t uiTimeMs)
|
|
{
|
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfSetWatchdog, uiTimeMs);
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_RequestPosition
|
|
功能描述 : 读取电机位置
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
*****************************************************************************/
|
|
int MotorMgr_RequestPosition(Motor_Instance_t *pstMotor)
|
|
{
|
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfRequestPosition);
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_RequestVelocity
|
|
功能描述 : 读取电机速度
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
*****************************************************************************/
|
|
int MotorMgr_RequestVelocity(Motor_Instance_t *pstMotor)
|
|
{
|
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfRequestVelocity);
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_RequestTorque
|
|
功能描述 : 读取电机电流/转矩
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
*****************************************************************************/
|
|
int MotorMgr_RequestTorque(Motor_Instance_t *pstMotor)
|
|
{
|
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfRequestTorque);
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_RequestFault
|
|
功能描述 : 读取故障码
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
*****************************************************************************/
|
|
int MotorMgr_RequestFault(Motor_Instance_t *pstMotor)
|
|
{
|
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfRequestFault);
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_SpeedModeInit
|
|
功能描述 : 速度模式初始化
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
*****************************************************************************/
|
|
int MotorMgr_SpeedModeInit(Motor_Instance_t *pstMotor)
|
|
{
|
|
int iRet = MOTOR_PROTOCOL_CALL(pstMotor, pfSpeedModeInit);
|
|
if (iRet == RD_SUCCESS)
|
|
{
|
|
pstMotor->m_eState = MOTOR_STATE_READY;
|
|
}
|
|
return iRet;
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_SetTargetSpeed
|
|
功能描述 : 设置目标速度
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
int32_t iTargetSpeed 目标速度(脉冲/秒)
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
*****************************************************************************/
|
|
int MotorMgr_SetTargetSpeed(Motor_Instance_t *pstMotor, int32_t iTargetSpeed)
|
|
{
|
|
int iRet = MOTOR_PROTOCOL_CALL(pstMotor, pfSetTargetSpeed, iTargetSpeed);
|
|
if (iRet == RD_SUCCESS)
|
|
{
|
|
pstMotor->m_eState = MOTOR_STATE_RUNNING;
|
|
}
|
|
return iRet;
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_PositionModeInit
|
|
功能描述 : 位置模式初始化
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
int32_t iAccel 加速度(脉冲/秒²)
|
|
int32_t iDecel 减速度(脉冲/秒²)
|
|
int32_t iTargetSpeed 目标速度(脉冲/秒)
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
*****************************************************************************/
|
|
int MotorMgr_PositionModeInit(Motor_Instance_t *pstMotor, int32_t iAccel, int32_t iDecel, int32_t iTargetSpeed)
|
|
{
|
|
int iRet = MOTOR_PROTOCOL_CALL(pstMotor, pfPositionModeInit, iAccel, iDecel, iTargetSpeed);
|
|
if (iRet == RD_SUCCESS)
|
|
{
|
|
pstMotor->m_eState = MOTOR_STATE_READY;
|
|
}
|
|
return iRet;
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_SetTargetPosition
|
|
功能描述 : 设置目标位置（相对运动）
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
int32_t iTargetPos 目标位置(脉冲)
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
*****************************************************************************/
|
|
int MotorMgr_SetTargetPosition(Motor_Instance_t *pstMotor, int32_t iTargetPos)
|
|
{
|
|
int iRet = MOTOR_PROTOCOL_CALL(pstMotor, pfSetTargetPosition, iTargetPos);
|
|
if (iRet == RD_SUCCESS)
|
|
{
|
|
pstMotor->m_eState = MOTOR_STATE_RUNNING;
|
|
}
|
|
return iRet;
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_SetDO
|
|
功能描述 : 设置数字输出
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
uint8_t ucDoIndex 输出索引(1或2)
|
|
uint8_t ucState 状态(0或1)
|
|
输出参数 : 无
|
|
返 回 值 : int 0成功，非0失败
|
|
*****************************************************************************/
|
|
int MotorMgr_SetDO(Motor_Instance_t *pstMotor, uint8_t ucDoIndex, uint8_t ucState)
|
|
{
|
|
return MOTOR_PROTOCOL_CALL(pstMotor, pfSetDO, ucDoIndex, ucState);
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_ParseResponse
|
|
功能描述 : 解析CAN响应数据（遍历所有电机实例尝试解析）
|
|
输入参数 : uint8_t ucMotorID 电机节点ID
|
|
const uint8_t *pucData 数据指针
|
|
uint32_t uiLen 数据长度
|
|
输出参数 : 无
|
|
返 回 值 : void
|
|
*****************************************************************************/
|
|
void MotorMgr_ParseResponse(uint8_t ucMotorID, const uint8_t *pucData, uint32_t uiLen)
|
|
{
|
|
if (NULL == pucData || uiLen < 8)
|
|
{
|
|
return;
|
|
}
|
|
|
|
Motor_Instance_t *pstMotor = MotorMgr_FindByID(ucMotorID);
|
|
if (NULL == pstMotor)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (pstMotor->m_pstProtocol && pstMotor->m_pstProtocol->pfParseResponse)
|
|
{
|
|
pstMotor->m_pstProtocol->pfParseResponse(pstMotor, pucData, uiLen);
|
|
}
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_SpeedConvert
|
|
功能描述 : 速度单位转换
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
int32_t iSpeedUserUnit 用户速度单位
|
|
输出参数 : 无
|
|
返 回 值 : int32_t 脉冲/秒
|
|
*****************************************************************************/
|
|
int32_t MotorMgr_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedUserUnit)
|
|
{
|
|
if (pstMotor->m_pstProtocol && pstMotor->m_pstProtocol->pfSpeedConvert)
|
|
{
|
|
return pstMotor->m_pstProtocol->pfSpeedConvert(pstMotor, iSpeedUserUnit);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_GetData
|
|
功能描述 : 获取电机数据
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
输出参数 : 无
|
|
返 回 值 : const Motor_Data_t*
|
|
*****************************************************************************/
|
|
const Motor_Data_t* MotorMgr_GetData(Motor_Instance_t *pstMotor)
|
|
{
|
|
if (NULL == pstMotor)
|
|
{
|
|
return NULL;
|
|
}
|
|
return &pstMotor->m_stData;
|
|
}
|
|
|
|
/*****************************************************************************
|
|
函 数 名 : MotorMgr_GetState
|
|
功能描述 : 获取电机状态
|
|
输入参数 : Motor_Instance_t *pstMotor 电机实例
|
|
输出参数 : 无
|
|
返 回 值 : Motor_State_e
|
|
*****************************************************************************/
|
|
Motor_State_e MotorMgr_GetState(Motor_Instance_t *pstMotor)
|
|
{
|
|
if (NULL == pstMotor)
|
|
{
|
|
return MOTOR_STATE_IDLE;
|
|
}
|
|
return pstMotor->m_eState;
|
|
}
|
|
|