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.
487 lines
19 KiB
487 lines
19 KiB
2 weeks ago
|
/******************************************************************************
|
||
|
|
|
||
|
|
版权所有 (C), 2018-2099, Radkil
|
||
|
|
|
||
|
|
******************************************************************************
|
||
|
|
文 件 名 : msp_motor_leisai.c
|
||
|
|
版 本 号 : 初稿
|
||
|
|
作 者 : radkil
|
||
|
|
生成日期 : 2026年7月13日
|
||
|
|
最近修改 :
|
||
|
|
功能描述 : 雷赛ISV2电机CAN协议实现
|
||
|
|
|
||
|
|
修改历史 :
|
||
|
|
1.日 期 : 2026年7月13日
|
||
|
|
作 者 : radkil
|
||
|
|
修改内容 : 创建文件
|
||
|
|
|
||
|
|
******************************************************************************/
|
||
|
|
|
||
|
|
#include "msp_motor_leisai.h"
|
||
|
|
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 外部变量说明 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 外部函数原型说明 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 内部函数原型说明 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
static int LeiSai_SendSDO(Motor_Instance_t *pstMotor, uint8_t ucCmd,
|
||
|
|
uint16_t usIndex, uint8_t ucSubIndex,
|
||
|
|
int32_t iValue);
|
||
|
|
static void LeiSai_UpdateDistance(Motor_Instance_t *pstMotor);
|
||
|
|
|
||
|
|
static int LeiSai_Init(Motor_Instance_t *pstMotor);
|
||
|
|
static int LeiSai_ResetAll(Motor_Instance_t *pstMotor);
|
||
|
|
static int LeiSai_ActivateAll(Motor_Instance_t *pstMotor);
|
||
|
|
static int LeiSai_SetHome(Motor_Instance_t *pstMotor);
|
||
|
|
static int LeiSai_SetWatchdog(Motor_Instance_t *pstMotor, uint32_t uiTimeMs);
|
||
|
|
static int LeiSai_RequestPosition(Motor_Instance_t *pstMotor);
|
||
|
|
static int LeiSai_RequestVelocity(Motor_Instance_t *pstMotor);
|
||
|
|
static int LeiSai_RequestTorque(Motor_Instance_t *pstMotor);
|
||
|
|
static int LeiSai_RequestFault(Motor_Instance_t *pstMotor);
|
||
|
|
static int LeiSai_SpeedModeInit(Motor_Instance_t *pstMotor);
|
||
|
|
static int LeiSai_SetTargetSpeed(Motor_Instance_t *pstMotor, int32_t iTargetSpeed);
|
||
|
|
static int LeiSai_PositionModeInit(Motor_Instance_t *pstMotor, int32_t iAccel, int32_t iDecel, int32_t iTargetSpeed);
|
||
|
|
static int LeiSai_SetTargetPosition(Motor_Instance_t *pstMotor, int32_t iTargetPos);
|
||
|
|
static int LeiSai_SetDO(Motor_Instance_t *pstMotor, uint8_t ucDoIndex, uint8_t ucState);
|
||
|
|
static int LeiSai_ParseResponse(Motor_Instance_t *pstMotor, const uint8_t *pucData, uint32_t uiLen);
|
||
|
|
static int32_t LeiSai_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedE01Mmin);
|
||
|
|
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 全局变量 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
|
||
|
|
// 雷赛协议操作函数表
|
||
|
|
const Motor_Protocol_t g_stLeiSaiProtocol = {
|
||
|
|
.m_szName = "LeiSai_ISV2",
|
||
|
|
.pfInit = LeiSai_Init,
|
||
|
|
.pfResetAll = LeiSai_ResetAll,
|
||
|
|
.pfActivateAll = LeiSai_ActivateAll,
|
||
|
|
.pfSetHome = LeiSai_SetHome,
|
||
|
|
.pfSetWatchdog = LeiSai_SetWatchdog,
|
||
|
|
.pfRequestPosition = LeiSai_RequestPosition,
|
||
|
|
.pfRequestVelocity = LeiSai_RequestVelocity,
|
||
|
|
.pfRequestTorque = LeiSai_RequestTorque,
|
||
|
|
.pfRequestFault = LeiSai_RequestFault,
|
||
|
|
.pfSpeedModeInit = LeiSai_SpeedModeInit,
|
||
|
|
.pfSetTargetSpeed = LeiSai_SetTargetSpeed,
|
||
|
|
.pfPositionModeInit = LeiSai_PositionModeInit,
|
||
|
|
.pfSetTargetPosition = LeiSai_SetTargetPosition,
|
||
|
|
.pfSetDO = LeiSai_SetDO,
|
||
|
|
.pfParseResponse = LeiSai_ParseResponse,
|
||
|
|
.pfSpeedConvert = LeiSai_SpeedConvert,
|
||
|
|
};
|
||
|
|
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 模块级变量 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 常量定义 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
|
||
|
|
/*----------------------------------------------*
|
||
|
|
* 宏定义 *
|
||
|
|
*----------------------------------------------*/
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_GetProtocol
|
||
|
|
功能描述 : 获取雷赛协议操作函数表
|
||
|
|
输入参数 : 无
|
||
|
|
输出参数 : 无
|
||
|
|
返 回 值 : const Motor_Protocol_t*
|
||
|
|
*****************************************************************************/
|
||
|
|
const Motor_Protocol_t* LeiSai_GetProtocol(void)
|
||
|
|
{
|
||
|
|
return &g_stLeiSaiProtocol;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_SendSDO
|
||
|
|
功能描述 : 发送SDO命令
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_SendSDO(Motor_Instance_t *pstMotor, uint8_t ucCmd,
|
||
|
|
uint16_t usIndex, uint8_t ucSubIndex,
|
||
|
|
int32_t iValue)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor || NULL == pstMotor->m_ptCAN)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint8_t aucData[8];
|
||
|
|
aucData[0] = ucCmd;
|
||
|
|
aucData[1] = (uint8_t)(usIndex & 0xFF);
|
||
|
|
aucData[2] = (uint8_t)((usIndex >> 8) & 0xFF);
|
||
|
|
aucData[3] = ucSubIndex;
|
||
|
|
aucData[4] = (uint8_t)(iValue & 0xFF);
|
||
|
|
aucData[5] = (uint8_t)((iValue >> 8) & 0xFF);
|
||
|
|
aucData[6] = (uint8_t)((iValue >> 16) & 0xFF);
|
||
|
|
aucData[7] = (uint8_t)((iValue >> 24) & 0xFF);
|
||
|
|
|
||
|
|
uint32_t uiFrameID = 0x600 + pstMotor->m_stConfig.m_ucMotorID;
|
||
|
|
return rd_ComIDSend(pstMotor->m_ptCAN, uiFrameID, (char *)aucData, 8);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_UpdateDistance
|
||
|
|
功能描述 : 更新距离计算
|
||
|
|
*****************************************************************************/
|
||
|
|
static void LeiSai_UpdateDistance(Motor_Instance_t *pstMotor)
|
||
|
|
{
|
||
|
|
double dCircleLength = 3.14 * pstMotor->m_stConfig.m_fWheelDiameter;
|
||
|
|
double dDeltaCounts = pstMotor->m_stData.m_iRealPosition - pstMotor->m_stData.m_iStartMeasurePos;
|
||
|
|
|
||
|
|
pstMotor->m_stData.m_dRealDistance = (dDeltaCounts / pstMotor->m_stConfig.m_uiPulsePerRound
|
||
|
|
/ pstMotor->m_stConfig.m_uiReductionRatio
|
||
|
|
+ (double)pstMotor->m_stData.m_iRoundCount)
|
||
|
|
* dCircleLength;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_Init
|
||
|
|
功能描述 : 初始化电机
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_Init(Motor_Instance_t *pstMotor)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
pstMotor->m_eState = MOTOR_STATE_IDLE;
|
||
|
|
return RD_SUCCESS;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_ResetAll
|
||
|
|
功能描述 : 复位所有节点（NMT命令）
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_ResetAll(Motor_Instance_t *pstMotor)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor || NULL == pstMotor->m_ptCAN)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint8_t aucData[2] = {0x81, 0x00};
|
||
|
|
return rd_ComIDSend(pstMotor->m_ptCAN, 0x000, (char *)aucData, 2);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_ActivateAll
|
||
|
|
功能描述 : 激活所有节点（NMT命令）
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_ActivateAll(Motor_Instance_t *pstMotor)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor || NULL == pstMotor->m_ptCAN)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint8_t aucData1[2] = {0x01, 0x01};
|
||
|
|
rd_ComIDSend(pstMotor->m_ptCAN, 0x000, (char *)aucData1, 2);
|
||
|
|
|
||
|
|
uint8_t aucData2[2] = {0x01, 0x02};
|
||
|
|
return rd_ComIDSend(pstMotor->m_ptCAN, 0x000, (char *)aucData2, 2);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_SetHome
|
||
|
|
功能描述 : 设置当前位置为零点
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_SetHome(Motor_Instance_t *pstMotor)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_MODE_OF_OPERATION, 0x00, LEISAI_MODE_HOMING);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_HOMING_METHOD, 0x00, 0x23);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SHUTDOWN);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SWITCH_ON);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_ENABLE);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_START_MOVE);
|
||
|
|
|
||
|
|
return RD_SUCCESS;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_SetWatchdog
|
||
|
|
功能描述 : 设置看门狗时间
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_SetWatchdog(Motor_Instance_t *pstMotor, uint32_t uiTimeMs)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_WATCHDOG_TIME, 0x06, (int32_t)uiTimeMs);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_RequestPosition
|
||
|
|
功能描述 : 读取电机位置
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_RequestPosition(Motor_Instance_t *pstMotor)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_READ, LEISAI_OD_POSITION_ACTUAL, 0x00, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_RequestVelocity
|
||
|
|
功能描述 : 读取电机速度
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_RequestVelocity(Motor_Instance_t *pstMotor)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_READ, LEISAI_OD_VELOCITY_ACTUAL, 0x00, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_RequestTorque
|
||
|
|
功能描述 : 读取电机电流/转矩
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_RequestTorque(Motor_Instance_t *pstMotor)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_READ, LEISAI_OD_TORQUE_ACTUAL, 0x00, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_RequestFault
|
||
|
|
功能描述 : 读取故障码
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_RequestFault(Motor_Instance_t *pstMotor)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_READ, LEISAI_OD_FAULT_CODE, 0x00, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_SpeedModeInit
|
||
|
|
功能描述 : 速度模式初始化
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_SpeedModeInit(Motor_Instance_t *pstMotor)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SHUTDOWN);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SWITCH_ON);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_ENABLE);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_MODE_OF_OPERATION, 0x00, LEISAI_MODE_PROFILE_VEL);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_TARGET_VELOCITY, 0x00, 0);
|
||
|
|
|
||
|
|
return RD_SUCCESS;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_SetTargetSpeed
|
||
|
|
功能描述 : 设置目标速度
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_SetTargetSpeed(Motor_Instance_t *pstMotor, int32_t iTargetSpeed)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_TARGET_VELOCITY, 0x00, iTargetSpeed);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_PositionModeInit
|
||
|
|
功能描述 : 位置模式初始化
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_PositionModeInit(Motor_Instance_t *pstMotor, int32_t iAccel, int32_t iDecel, int32_t iTargetSpeed)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SHUTDOWN);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SWITCH_ON);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_ENABLE);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_MODE_OF_OPERATION, 0x00, LEISAI_MODE_PROFILE_POS);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_ACCELERATION, 0x00, iAccel);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_DECELERATION, 0x00, iDecel);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_TARGET_VELOCITY, 0x00, iTargetSpeed);
|
||
|
|
|
||
|
|
return RD_SUCCESS;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_SetTargetPosition
|
||
|
|
功能描述 : 设置目标位置（相对运动）
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_SetTargetPosition(Motor_Instance_t *pstMotor, int32_t iTargetPos)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_TARGET_POSITION, 0x00, iTargetPos);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_RELATIVE_MOVE);
|
||
|
|
LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_START_MOVE);
|
||
|
|
|
||
|
|
return RD_SUCCESS;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_SetDO
|
||
|
|
功能描述 : 设置数字输出
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_SetDO(Motor_Instance_t *pstMotor, uint8_t ucDoIndex, uint8_t ucState)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint16_t usIndex = 0;
|
||
|
|
switch (ucDoIndex)
|
||
|
|
{
|
||
|
|
case 1:
|
||
|
|
usIndex = 0x2410;
|
||
|
|
break;
|
||
|
|
case 2:
|
||
|
|
usIndex = 0x2411;
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
return RD_INVALUE;
|
||
|
|
}
|
||
|
|
|
||
|
|
return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, usIndex, 0x00, ucState ? 1 : 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_ParseResponse
|
||
|
|
功能描述 : 解析CAN响应数据
|
||
|
|
*****************************************************************************/
|
||
|
|
static int LeiSai_ParseResponse(Motor_Instance_t *pstMotor, const uint8_t *pucData, uint32_t uiLen)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor || NULL == pucData || uiLen < 8)
|
||
|
|
{
|
||
|
|
return RD_NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint16_t usFunctionCode = (uint16_t)(pucData[1] | (pucData[2] << 8));
|
||
|
|
|
||
|
|
switch (usFunctionCode)
|
||
|
|
{
|
||
|
|
case LEISAI_OD_POSITION_ACTUAL:
|
||
|
|
{
|
||
|
|
pstMotor->m_stData.m_iRealPosition = (int32_t)(pucData[4]
|
||
|
|
| (pucData[5] << 8)
|
||
|
|
| (pucData[6] << 16)
|
||
|
|
| (pucData[7] << 24));
|
||
|
|
|
||
|
|
if (pstMotor->m_stData.m_ucStartMeasureFlag == 1)
|
||
|
|
{
|
||
|
|
pstMotor->m_stData.m_ucStartMeasureFlag = 0;
|
||
|
|
pstMotor->m_stData.m_iRoundCount = 0;
|
||
|
|
pstMotor->m_stData.m_iStartMeasurePos = pstMotor->m_stData.m_iRealPosition;
|
||
|
|
pstMotor->m_stData.m_iLastPosition = pstMotor->m_stData.m_iRealPosition;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
if (abs(pstMotor->m_stData.m_iRealPosition - pstMotor->m_stData.m_iLastPosition) >= 0x3FFFFFFF)
|
||
|
|
{
|
||
|
|
if (pstMotor->m_stData.m_iRealPosition > 0)
|
||
|
|
{
|
||
|
|
pstMotor->m_stData.m_iRoundCount -= 1;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
pstMotor->m_stData.m_iRoundCount += 1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
pstMotor->m_stData.m_iLastPosition = pstMotor->m_stData.m_iRealPosition;
|
||
|
|
LeiSai_UpdateDistance(pstMotor);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case LEISAI_OD_FAULT_CODE:
|
||
|
|
{
|
||
|
|
pstMotor->m_stData.m_uiFaultCode = (uint32_t)(pucData[4]
|
||
|
|
| (pucData[5] << 8)
|
||
|
|
| (pucData[6] << 16)
|
||
|
|
| (pucData[7] << 24));
|
||
|
|
if (pstMotor->m_stData.m_uiFaultCode != 0)
|
||
|
|
{
|
||
|
|
pstMotor->m_eState = MOTOR_STATE_ERROR;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case LEISAI_OD_VELOCITY_ACTUAL:
|
||
|
|
{
|
||
|
|
pstMotor->m_stData.m_iRealVelocity = (int32_t)(pucData[4]
|
||
|
|
| (pucData[5] << 8)
|
||
|
|
| (pucData[6] << 16)
|
||
|
|
| (pucData[7] << 24));
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case LEISAI_OD_TORQUE_ACTUAL:
|
||
|
|
{
|
||
|
|
pstMotor->m_stData.m_iRealTorque = (int32_t)(pucData[4]
|
||
|
|
| (pucData[5] << 8)
|
||
|
|
| (pucData[6] << 16)
|
||
|
|
| (pucData[7] << 24));
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
return RD_SUCCESS;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*****************************************************************************
|
||
|
|
函 数 名 : LeiSai_SpeedConvert
|
||
|
|
功能描述 : 速度单位转换：0.1m/min -> 脉冲/秒
|
||
|
|
*****************************************************************************/
|
||
|
|
static int32_t LeiSai_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedE01Mmin)
|
||
|
|
{
|
||
|
|
if (NULL == pstMotor)
|
||
|
|
{
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
double dSpeedMs = iSpeedE01Mmin * 0.1 / 60.0;
|
||
|
|
double dWheelCircumference = 3.14 * pstMotor->m_stConfig.m_fWheelDiameter;
|
||
|
|
double dPulsePerSec = dSpeedMs / dWheelCircumference
|
||
|
|
* pstMotor->m_stConfig.m_uiReductionRatio
|
||
|
|
* pstMotor->m_stConfig.m_uiPulsePerRound;
|
||
|
|
|
||
|
|
return (int32_t)dPulsePerSec;
|
||
|
|
}
|