LiZongdi
/
robot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

电机模块删除Motor_Data_t结构体，获取电机状态使用单独命令；增加CAN读取线程

master
Lizongdi 2 days ago
parent
d916c480b8
commit
7a425eb922
6 changed files with 21 additions and 115 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 17
      RBcore/drv_interface.c
  2. 3
      RBcore/include/BHBF.h
  3. 5
      motor/include/motor_manager.h
  4. 14
      motor/include/motor_protocol.h
  5. 27
      motor/motor_manager.c
  6. 68
      motor/msp_motor_leisai.c

17
RBcore/drv_interface.c
View File

@ -246,6 +246,16 @@ void Read_PV(void *argument)
}
}
void Read_motor(void *argument)
{
char pcBuffer[32] = {0};
while(1)
{
rd_ComRead(g_ptCAN1, pcBuffer, 32);
}
}
extern void MotorTask(void *argument);
extern int check_MK32(char *_pBuffer, uint32_t _iSize);
extern void decode_MK32(const char *buf, uint32_t _iSize);
@ -280,6 +290,13 @@ void Drv_InterfaceInit(void)
};
(void)osThreadNew(MotorTask, NULL, &motor_task_attributes);
const osThreadAttr_t Read_motor_attributes = {
.name = "Read_motor",
.stack_size = 2048,
.priority = (osPriority_t) osPriorityRealtime1,
};
(void)osThreadNew(Read_motor, NULL, &Read_motor_attributes);
RBcore_Init();
TUartUserData *ptSbus = UART_userdata_init(5, -1, 512);

3
RBcore/include/BHBF.h
View File

@ -78,7 +78,8 @@ typedef enum {
LUA_SHOW_INFO,
MOTOR_CMD_SET_SPEED, // 设置速度
MOTOR_CMD_SET_POSITION, // 设置位置
MOTOR_CMD_GET_STATUS, // 获取状态
MOTOR_CMD_GET_SPEED, // 获取速度
MOTOR_CMD_GET_FAULT_CODE, // 获取错误码
MOTOR_CMD_SET_HOME, // 设置零点
MOTOR_CMD_SPEED_MODE, // 速度模式
MOTOR_CMD_POSITION_MODE, // 位置模式

5
motor/include/motor_manager.h
View File

@ -164,11 +164,6 @@ void MotorMgr_ParseResponse(uint8_t ucMotorID, const uint8_t *pucData, uint32_t
*/
int32_t MotorMgr_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedUserUnit);
/**
* @brief 获取电机数据
*/
const Motor_Data_t* MotorMgr_GetData(Motor_Instance_t *pstMotor);
/**
* @brief 获取电机状态
*/

14
motor/include/motor_protocol.h
View File

@ -52,19 +52,6 @@ typedef enum {
MOTOR_STATE_ERROR, // 错误
} Motor_State_e;
// 电机运行数据
typedef struct {
int32_t m_iRealPosition; // 实际位置(脉冲)
int32_t m_iRealVelocity; // 实际速度(脉冲/秒)
int32_t m_iRealTorque; // 实际转矩
uint32_t m_uiFaultCode; // 故障码
double m_dRealDistance; // 实际距离(米)
int32_t m_iStartMeasurePos; // 测量起始位置
int32_t m_iLastPosition; // 上次位置
int32_t m_iRoundCount; // 圈数计数
uint8_t m_ucStartMeasureFlag; // 开始测量标志
} Motor_Data_t;
// 电机物理配置
typedef struct {
uint8_t m_ucMotorID; // 电机节点ID
@ -135,7 +122,6 @@ typedef struct Motor_Instance {
rd_slist_t node; // slist链表节点
char m_szName[MOTOR_MAX_NAME_LEN]; // 实例名称
Motor_Config_t m_stConfig; // 配置
Motor_Data_t m_stData; // 运行数据
Motor_State_e m_eState; // 状态
TComCtrl *m_ptCAN; // 绑定的CAN控制器
uint32_t m_uiLastHeartbeat; // 上次心跳时间

27
motor/motor_manager.c
View File

@ -129,17 +129,6 @@ Motor_Instance_t* MotorMgr_Create(const Motor_Config_t *pstConfig,
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;
@ -479,22 +468,6 @@ int32_t MotorMgr_SpeedConvert(Motor_Instance_t *pstMotor, int32_t 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
功能描述 : 获取电机状态

68
motor/msp_motor_leisai.c
View File

@ -129,21 +129,6 @@ static int LeiSai_SendSDO(Motor_Instance_t *pstMotor, uint8_t ucCmd,
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
功能描述 : 初始化电机
@ -406,65 +391,14 @@ static int LeiSai_ParseResponse(Motor_Instance_t *pstMotor, const uint8_t *pucDa
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:

Write Preview
Loading…
Cancel
Save