LiZongdi
/
robot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
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.
59 Commits
1 Branch
0 Tags
2.1 MiB
 
 
 
Tree: 12f5da68f1
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '12f5da68f1'
${ noResults }
robot/motor/msp_motor_leisai.c

487 lines
19 KiB
Raw Blame History

/******************************************************************************
版权所有 (C), 2018-2099, Radkil
******************************************************************************
文 件 名 : msp_motor_leisai.c
版 本 号 : 初稿
作 者 : radkil
生成日期 : 2026年7月13日
最近修改 :
功能描述 : 雷赛ISV2电机CAN协议实现
修改历史 :
1.日 期 : 2026年7月13日
作 者 : radkil
修改内容 : 创建文件
******************************************************************************/
#include "msp_motor_leisai.h"
#include <math.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;
}