robot/project/paint_robot_new/paint_robot_new_motors.c

/******************************************************************************

                  版权所有 (C), 2018-2099, Radkil

 ******************************************************************************
  文 件 名   : paint_robot_new_motors.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年8月21日
  最近修改   :
  功能描述   : 喷涂机器人电机逻辑
  
  修改历史   :
  1.日    期   : 2026年8月21日
    作    者   : radkil
    修改内容   : 创建文件

******************************************************************************/
#include "BHBF.h"
#include "motor_manager.h"
#include "msp_motor_leisai.h"
#include "msg_center.h"

/*----------------------------------------------*
 * 外部变量说明                                 *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 外部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 内部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 全局变量                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/
static Motor_Instance_t *g_apstMotors[MOTOR_INSTANCE_MAX] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL};
static uint32_t g_uiMotorModuleID = 0;
static int g_aiMotorSpeed[MOTOR_INSTANCE_MAX] = {0};

/*----------------------------------------------*
 * 常量定义                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 宏定义                                       *
 *----------------------------------------------*/

static void Motor_ModuleHandler(const Msg_t *pstMsg)
{
    if (NULL == pstMsg)
    {
        return;
    }

    switch (pstMsg->m_uiMsgID)
    {
        case MOTOR_SET_SPEED:
        {
            int aiMotorSpeed[2] = {0};
            if (pstMsg->m_uiDataLen >= sizeof(aiMotorSpeed))
            {
                RD_MEMCPY(aiMotorSpeed, pstMsg->m_aucData, sizeof(aiMotorSpeed));
            }
            g_aiMotorSpeed[0] = MotorMgr_SpeedConvert(g_apstMotors[0], aiMotorSpeed[0]);
            g_aiMotorSpeed[1] = MotorMgr_SpeedConvert(g_apstMotors[1], aiMotorSpeed[1]);
            break;
        }
        case RBCORE_CMD_STOP_ALL:
        {
            for (int i = 0; i < 2; i++)
            {
            	MotorMgr_SetTargetSpeed(g_apstMotors[i], 0);
            }
            memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed));
            break;
        }
        default:
            break;
    }
}

// 雷赛电机CAN响应检查函数
// 雷赛响应ID范围：0x581 - 0x5FF (0x580 + MotorID)
static int check_LeiSaiMotor(char *_pBuffer, uint32_t _iSize)
{
	TCANUserData *ptCANUserData = (TCANUserData *)g_ptCAN1->m_pUserData;
	uint32_t iCANId = (uint32_t)ptCANUserData->m_canrx->Identifier;

    if (iCANId >= 0x581 && iCANId <= 0x5FF)
    {
        return _iSize;
    }

    return 0;
}

// 雷赛电机CAN响应解码函数
static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize)
{
	TCANUserData *ptCANUserData = (TCANUserData *)g_ptCAN1->m_pUserData;
	uint32_t iCANId = (uint32_t)ptCANUserData->m_canrx->Identifier;

    // 提取电机ID (0x580 + MotorID)
    uint8_t ucMotorID = (uint8_t)(iCANId - 0x580);

    // 调用电机管理模块解析响应
    MotorMgr_ParseResponse(ucMotorID, (const uint8_t *)_pBuffer, _iSize);
}

void MotorTask(void *argument)
{
	// 初始化FDCAN1，使用雷赛电机的回调
	TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 32, 0);
	g_ptCAN1 = rd_ComCreate(check_LeiSaiMotor, decode_LeiSaiMotor, FDCAN1_Send, CONFIG_UART_BUFFER_SIZE, ptCANUserData);
	CAN_IT_init(g_ptCAN1);

    // 初始化电机管理模块
    MotorMgr_Init();

    // 配置左轮电机
    Motor_Config_t stLeftMotorConfig = {
        .m_ucMotorID = 1,
        .m_uiPulsePerRound = g_stCV.pulse_Per_Circle,
        .m_uiReductionRatio = g_stCV.wheel_Reduction_Ratio,
        .m_fWheelDiameter = g_stCV.wheel_Diameter_m
    };
    g_apstMotors[0] = MotorMgr_Create(&stLeftMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL);

    // 配置右轮电机
    Motor_Config_t stRightMotorConfig = {
        .m_ucMotorID = 2,
        .m_uiPulsePerRound = g_stCV.pulse_Per_Circle,
        .m_uiReductionRatio = g_stCV.wheel_Reduction_Ratio,
        .m_fWheelDiameter = g_stCV.wheel_Diameter_m
    };
    g_apstMotors[1] = MotorMgr_Create(&stRightMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL);

    g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler);

	MotorMgr_ResetAll(g_apstMotors[0]);
	Rd_Delay(500);
	MotorMgr_ResetAll(g_apstMotors[1]);
	Rd_Delay(500);

	int iCount = 6;
	while(iCount--)
	{
		MotorMgr_ActivateAll(g_apstMotors[0]);
		Rd_Delay(500);
		MotorMgr_ActivateAll(g_apstMotors[1]);
		Rd_Delay(500);
	}

	MotorMgr_SetHome(g_apstMotors[0]);
	MotorMgr_SetHome(g_apstMotors[1]);
	MotorMgr_SpeedModeInit(g_apstMotors[0]);
	MotorMgr_SpeedModeInit(g_apstMotors[1]);

    while(1)
    {
        // 处理消息
        MsgCenter_ProcessWait(g_uiMotorModuleID, 2);
        Rd_Delay(10);
		MotorMgr_RequestPosition(g_apstMotors[0]);
		Rd_Delay(10);
		MotorMgr_RequestPosition(g_apstMotors[1]);
		Rd_Delay(10);
		MotorMgr_RequestFault(g_apstMotors[0]);
		Rd_Delay(10);
		MotorMgr_RequestFault(g_apstMotors[1]);
		Rd_Delay(10);
		MotorMgr_RequestVelocity(g_apstMotors[0]);
		Rd_Delay(10);
		MotorMgr_RequestVelocity(g_apstMotors[1]);
		Rd_Delay(10);
		MotorMgr_SetTargetSpeed(g_apstMotors[0], g_aiMotorSpeed[0]);
		Rd_Delay(10);
		MotorMgr_SetTargetSpeed(g_apstMotors[1], g_aiMotorSpeed[1]);
        Rd_Delay(10);
    }
}