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

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

 ******************************************************************************
  文 件 名   : motor_adapter.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年4月21日
  最近修改   :
  功能描述   : 电机适配层
  
  修改历史   :
  1.日    期   : 2026年4月21日
    作    者   : radkil
    修改内容   : 创建文件

******************************************************************************/
#ifdef BUILD_MOTOR
#include "motor.h"
#include "msp_TTMotor_ZQ.h"

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

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

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

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

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

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

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

static void WheelSendHeartbeat(TMotorCtrl *ptLeftMotorCtrl, TMotorCtrl *ptRightMotorCtrl)
{    
    static int Heartbeat_Flag_01;
    Heartbeat_Flag_01++;
	if(Heartbeat_Flag_01%7==0)
	{
		Consumer_Or_microcontroller_Heartbeat(ptLeftMotorCtrl,  4);
		Consumer_Or_microcontroller_Heartbeat(ptRightMotorCtrl,  4);
		Heartbeat_Flag_01=0;
	}
}

void MotorGetState(TMotorCtrl *ptMotorCtrl, char *buffer)
{
    TT_Analytic_Fun(ptMotorCtrl, buffer);
}

void WheelFuncInit(TMotorCtrl *ptLeftMotorCtrl, TMotorCtrl *ptRightMotorCtrl)
{    
    ActivateMotor(ptLeftMotorCtrl, 6000);
	ActivateMotor(ptRightMotorCtrl, 2000);
	ActivateMotor(ptLeftMotorCtrl, 2000);
	ActivateMotor(ptRightMotorCtrl, 2000);
	ActivateMotor(ptLeftMotorCtrl, 2000);
	ActivateMotor(ptRightMotorCtrl, 2000);
	ActivateMotor(ptLeftMotorCtrl, 2000);
	ActivateMotor(ptRightMotorCtrl, 2000);

	Enable_NMT(ptLeftMotorCtrl,1000);
	Enable_NMT(ptRightMotorCtrl,1000);

	Configure_Asynchronous_Mode(ptLeftMotorCtrl, 7,600);
	Consumer_Or_microcontroller_Heartbeat(ptLeftMotorCtrl,  4);
	Configure_Asynchronous_Mode(ptRightMotorCtrl, 8,600);
	Consumer_Or_microcontroller_Heartbeat(ptLeftMotorCtrl,  4);
	SpeedModeSetup(ptLeftMotorCtrl, 6, 500, 500, 0);
	Consumer_Or_microcontroller_Heartbeat(ptRightMotorCtrl,  4);
	SpeedModeSetup(ptRightMotorCtrl, 6, 500, 500, 0);
	Consumer_Or_microcontroller_Heartbeat(ptLeftMotorCtrl,  4);
	Consumer_Or_microcontroller_Heartbeat(ptRightMotorCtrl,  4);

    WheelSendHeartbeat(ptLeftMotorCtrl, ptRightMotorCtrl);
}

void WheelFuncProc(TMotorCtrl *ptLeftMotorCtrl, TMotorCtrl *ptRightMotorCtrl)
{    
    TT_Request_Position(ptLeftMotorCtrl, 6);
    TT_Request_Velocity(ptLeftMotorCtrl, 6);
    TT_Request_Current(ptLeftMotorCtrl, 6);
    TT_Request_Fault(ptLeftMotorCtrl, 6);

    TT_Request_Position(ptRightMotorCtrl, 6);
    TT_Request_Velocity(ptRightMotorCtrl, 6);
    TT_Request_Current(ptRightMotorCtrl, 6);
    TT_Request_Fault(ptRightMotorCtrl, 6);

    TT_SpeedMode_Set_TargetSpeed(ptLeftMotorCtrl, 6, GV.LeftMotor.Target_Velcity);
    TT_SpeedMode_Set_TargetSpeed(ptRightMotorCtrl, 6, GV.RightMotor.Target_Velcity);
    
    WheelSendHeartbeat(ptLeftMotorCtrl, ptRightMotorCtrl);
}

void WheelSetVelcity(TMotorCtrl *ptMotorCtrl, float fSpeed_M_min)
{
    ptMotorCtrl->pMotorParam->Target_Velcity = (int32_t)(fSpeed_M_min * Move_Base_Speed_Count_m_Min);
}

void SwingFuncInit(TMotorCtrl *ptMotorCtrl)
{
    ActivateMotor(ptMotorCtrl, 6000);
    ActivateMotor(ptMotorCtrl, 2000);
    ActivateMotor(ptMotorCtrl, 2000);
    ActivateMotor(ptMotorCtrl, 2000);
    ActivateMotor(ptMotorCtrl, 2000);
    Postion_Velcocity_Run_SetParameter(ptMotorCtrl, 0, 0, 500, 500, 100);
}

void SwingFuncProc(TMotorCtrl *ptMotorCtrl)
{
    TT_Request_Position(ptMotorCtrl, 10);
    TT_Request_Fault(ptMotorCtrl, 10);
    
    switch(GV.SwingMotor.Position_immediately1_Lag2)
    {
        case 1:
            Position_Immediately_Setting(ptMotorCtrl, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
            break;
        case 2:
            Position_Lag_Setting(ptMotorCtrl, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
            break;
    }
}

void SwingSetPosition(TMotorCtrl *ptMotorCtrl, float fPositionAngle, int32_t iVelcityDegreeS)
{    
    ptMotorCtrl->pMotorParam->Tar_Position_count = (int32_t)(middle_position_motor - fPositionAngle * TT_One_Deg_Count_motor);
    ptMotorCtrl->pMotorParam->Tar_Position_Velcity_RPM = iVelcityDegreeS*Swing_Speed_Deg_Sencond_motor;
}

#endif