/*
 * motor.c
 *
 *  Created on: 2026年4月20日
 *      Author: Lizongdi
 */
#ifdef BUILD_MOTOR

#include "bsp_FDCAN.h"
#include "motor.h"
#include "BHBF_ROBOT.h"
#include "msp_TTMotor_ZQ.h"

TMotorCtrl *g_leftmotor;
TMotorCtrl *g_rightmotor;
TMotorCtrl *g_swingmotor;

char a[10];
int32_t fluat_flag=0;

extern TT_MotorParameters *TT_Motor[7];

static void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
{

	memcpy(a,buffer,8);
    fluat_flag = canID - 0x580;
	switch (fluat_flag)
	{

		case 1:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1);
			TT_Analytic_Fun(1, buffer);
		}
			break;
		case 2:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1);
			TT_Analytic_Fun(2, buffer);
		}
			break;
		case 3:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1);
			TT_Analytic_Fun(3, buffer);
		}
			break;
	}

}

static void MotorCommandsLoop()
{
    static char TT_Motor_Need_To_Activate = 1;
//
//	if(start_time<15000)
//	{
//		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1);
//		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1);
//	}
    static int Heartbeat_Flag_01;
    MotorMoveParamInit();

    if (TT_Motor_Need_To_Activate == 1)
	{
		ActivateMotor(g_leftmotor, 6000);
		ActivateMotor(g_rightmotor, 2000);
		ActivateMotor(g_leftmotor, 2000);
		ActivateMotor(g_rightmotor, 2000);
		ActivateMotor(g_leftmotor, 2000);
		ActivateMotor(g_rightmotor, 2000);
		ActivateMotor(g_leftmotor, 2000);
		ActivateMotor(g_rightmotor, 2000);

		Enable_NMT(g_leftmotor,1000);
		Enable_NMT(g_rightmotor,1000);

		Configure_Asynchronous_Mode(g_leftmotor, 7,600);
		Consumer_Or_microcontroller_Heartbeat(g_leftmotor,  4);
		Configure_Asynchronous_Mode(g_rightmotor, 8,600);
		Consumer_Or_microcontroller_Heartbeat(g_leftmotor,  4);
		SpeedModeSetup(g_leftmotor, 6, 500, 500, 0);
		Consumer_Or_microcontroller_Heartbeat(g_rightmotor,  4);
		SpeedModeSetup(g_rightmotor, 6, 500, 500, 0);
		Consumer_Or_microcontroller_Heartbeat(g_leftmotor,  4);
		Consumer_Or_microcontroller_Heartbeat(g_rightmotor,  4);
		TT_Motor_Need_To_Activate = 0;
	}
	else
	{
		TT_Request_Position(g_leftmotor, 6);
		TT_Request_Velocity(g_leftmotor, 6);
		TT_Request_Current(g_leftmotor, 6);
		TT_Request_Fault(g_leftmotor, 6);

        TT_Request_Position(g_rightmotor, 6);
		TT_Request_Velocity(g_rightmotor, 6);
		TT_Request_Current(g_rightmotor, 6);
		TT_Request_Fault(g_rightmotor, 6);
        
		TT_SpeedMode_Set_TargetSpeed(g_leftmotor, 6, GV.LeftMotor.Target_Velcity);
		TT_SpeedMode_Set_TargetSpeed(g_rightmotor, 6, GV.RightMotor.Target_Velcity);
	}

	Heartbeat_Flag_01++;
	if(Heartbeat_Flag_01%7==0)
	{
		Consumer_Or_microcontroller_Heartbeat(g_leftmotor,  4);
		Consumer_Or_microcontroller_Heartbeat(g_rightmotor,  4);
		Heartbeat_Flag_01=0;
	}
}

static void MotorCommandsLoop_2_Position()
{
    static char TT_Motor_Need_To_Activate_1 = 1;
//	if(start_time<15000)
//	{
//	    HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1);
//	}
    MotorMoveCAN2ParamInit();

	if (TT_Motor_Need_To_Activate_1 == 1)
	{
		ActivateMotor(g_swingmotor, 6000);
		ActivateMotor(g_swingmotor, 2000);
		ActivateMotor(g_swingmotor, 2000);
		ActivateMotor(g_swingmotor, 2000);
		ActivateMotor(g_swingmotor, 2000);
		Postion_Velcocity_Run_SetParameter(g_swingmotor, 0, 0, 500, 500, 100);
		TT_Motor_Need_To_Activate_1 = 0;
	}
	else
	{
		TT_Request_Position(g_swingmotor, 10);
		TT_Request_Fault(g_swingmotor, 10);

		switch(GV.SwingMotor.Position_immediately1_Lag2)
		{
			case 1:
				Position_Immediately_Setting(g_swingmotor, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
				break;
			case 2:
				Position_Lag_Setting(g_swingmotor, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
				break;
		}
	}
}

static TMotorCtrl *MotorControllerInit(int32_t MotorID, EMotorType MotorType, 
    int32_t HeartbeatID, FDCANHandler *fHandler)
{
    TMotorCtrl *ptMotorCtrl = malloc(sizeof(TMotorCtrl));
    if (M_TYPE_MOVELEFT == MotorType)
    {
        TT_Motor[M_TYPE_MOVELEFT] = &GV.LeftMotor;  
        HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor",  1, ComError_ZQ_CAN_ID1_LeftMotor);
    }
    else if (M_TYPE_MOVERIGHT == MotorType)
    {
        TT_Motor[M_TYPE_MOVERIGHT] = &GV.RightMotor;
        HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1, ComError_ZQ_CAN_ID2_RightMotor);
    }
    else if (M_TYPE_SWING == MotorType)
    {
        TT_Motor[M_TYPE_SWING] = &GV.SwingMotor; 
        HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID3_SwingMotor", 1, ComError_ZQ_CAN_ID3_SwingMotor);
    }
    TT_Motor[MotorType]->MotorID = MotorID;  
    ptMotorCtrl->MotorID = MotorID;
    ptMotorCtrl->MotorType = MotorType;
    ptMotorCtrl->HeartbeatID = HeartbeatID;
    ptMotorCtrl->fHandler = fHandler;
    
	ptMotorCtrl->fHandler->CAN_Decode = Roughening_MotorDecodeCAN;

    if (M_TYPE_SWING == MotorType)
    {
        ptMotorCtrl->fHandler->dispacherController->Add_Dispatcher_List(ptMotorCtrl->fHandler->dispacherController, MotorCommandsLoop_2_Position);
    }
    else
    {
        ptMotorCtrl->fHandler->dispacherController->Add_Dispatcher_List(ptMotorCtrl->fHandler->dispacherController, MotorCommandsLoop);
    }
	ptMotorCtrl->fHandler->dispacherController->DispacherCallTime = 2;
    
    return ptMotorCtrl;
}

void MotorInit(void)
{
    g_leftmotor = MotorControllerInit(1, M_TYPE_MOVELEFT, 0x707, &FD_CAN_1_Handler);
    g_rightmotor = MotorControllerInit(2, M_TYPE_MOVERIGHT, 0x708, &FD_CAN_1_Handler);
    g_swingmotor = MotorControllerInit(3, M_TYPE_SWING, 0x709, &FD_CAN_2_Handler);
}

#endif