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

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

 ******************************************************************************
  文 件 名   : paint_robot_new.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年7月14日
  最近修改   :
  功能描述   : 新版喷漆主逻辑
  
  修改历史   :
  1.日    期   : 2026年7月14日
    作    者   : radkil
    修改内容   : 创建文件

******************************************************************************/
#include "paint_robot_new.h"
#include "com.h"
#include "rd_time.h"
/*----------------------------------------------*
 * 外部变量说明                                 *
 *----------------------------------------------*/
extern TComCtrl *g_ptFDCAN1;

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

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

/*----------------------------------------------*
 * 全局变量                                     *
 *----------------------------------------------*/
static Motor_Instance_t *g_pstLeftMotor;   // 左轮电机实例
static Motor_Instance_t *g_pstRightMotor;  // 右轮电机实例

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/

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

/*----------------------------------------------*
 * 宏定义                                       *
 *----------------------------------------------*/
    
void MotorInit(void)
{
    // 初始化电机管理模块
    MotorMgr_Init();


    // 配置左轮电机
    Motor_Config_t stLeftMotorConfig = {
        .m_ucMotorID = 1,
        .m_uiPulsePerRound = 10000,
        .m_uiReductionRatio = 70,
        .m_fWheelDiameter = 0.26f
    };
    g_pstLeftMotor = MotorMgr_Create(&stLeftMotorConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL);

    // 配置右轮电机
    Motor_Config_t stRightMotorConfig = {
        .m_ucMotorID = 2,
        .m_uiPulsePerRound = 10000,
        .m_uiReductionRatio = 70,
        .m_fWheelDiameter = 0.26f
    };
    g_pstRightMotor = MotorMgr_Create(&stRightMotorConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL);
}

void MotorTask(void *argument)
{
    // 等待系统稳定
    Rd_Delay(1000);

    // 复位并激活所有电机
    if (g_pstLeftMotor != NULL)
    {
        MotorMgr_ResetAll(g_pstLeftMotor);
        Rd_Delay(500);
        MotorMgr_ActivateAll(g_pstLeftMotor);
        Rd_Delay(500);
        MotorMgr_SetHome(g_pstLeftMotor);
        Rd_Delay(100);
        MotorMgr_SpeedModeInit(g_pstLeftMotor);
    }

    while(1)
    {
        if (g_pstLeftMotor != NULL)
        {
            // 读取位置、速度、故障
            MotorMgr_RequestPosition(g_pstLeftMotor);
            MotorMgr_RequestFault(g_pstLeftMotor);
            MotorMgr_RequestVelocity(g_pstLeftMotor);

            // 设置目标速度
            MotorMgr_SetTargetSpeed(g_pstLeftMotor, 0);
        }

        if (g_pstRightMotor != NULL)
        {
            // 读取位置、速度、故障
            MotorMgr_RequestPosition(g_pstRightMotor);
            MotorMgr_RequestFault(g_pstRightMotor);
            MotorMgr_RequestVelocity(g_pstRightMotor);

            // 设置目标速度
            MotorMgr_SetTargetSpeed(g_pstRightMotor, 0);
        }

        Rd_Delay(2); // 2ms周期
    }
}