/*
 * motor.c
 *
 *  Created on: 2026年1月29日
 *      Author: bm673
 */
#include "motors.h"
#include "msp_TTMotor_ZQ.h"

FDCANHandler *Roughening_Motor_Controller;
FDCANHandler *Roughening_Motor_Controller_CAN2;
DispacherController *Roughening_DispacherController;
DispacherController *Roughening_DispacherController_CAN2;


char TT_Motor_Need_To_Activate = 1;
char TT_Motor_Need_To_Activate_1 = 1;

double Move_Base_Speed_Count_m_Min=201.7*7.64;//天太电机1m/min


void MotorCommandsLoop();

void MotorCommandsLoop_2();
void MotorCommandsLoop_2_Position();
void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length);


void Motor_Controller_intialize(FDCANHandler *Handler)
{
	//初始化
	Roughening_Motor_Controller = Handler;
	Roughening_Motor_Controller->CAN_Decode = Roughening_MotorDecodeCAN;
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor",  0, ComError_ZQ_CAN_ID1_LeftMotor);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 0, ComError_ZQ_CAN_ID2_RightMotor);
	Roughening_DispacherController = Handler->dispacherController;
	Roughening_DispacherController->DispacherCallTime = 2;
	Roughening_DispacherController->Add_Dispatcher_List(Roughening_DispacherController, MotorCommandsLoop);




	//电机绑定
//	LS_Motor[1]=&GV.LeftMotor;
//	LS_Motor[2]=&GV.RightMotor;

//	LS_Motor[1]->MotorID=1;
//	LS_Motor[2]->MotorID=2;
}


void Motor_Controller_intialize_CAN2(FDCANHandler *Handler)
{
	//初始化
	Roughening_Motor_Controller_CAN2 = Handler;
	Roughening_Motor_Controller_CAN2->CAN_Decode = Roughening_MotorDecodeCAN;
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID3_SwingMotor", 0, ComError_ZQ_CAN_ID3_SwingMotor);
	Roughening_DispacherController_CAN2 = Handler->dispacherController;
	Roughening_DispacherController_CAN2->DispacherCallTime = 2;
	Roughening_DispacherController_CAN2->Add_Dispatcher_List(Roughening_DispacherController_CAN2, MotorCommandsLoop_2_Position);
}


void MotorCommandsLoop()
{

	GV.LeftMotor.Target_Velcity=(int)(GV.Left_Speed_M_min*Move_Base_Speed_Count_m_Min);
	GV.RightMotor.Target_Velcity=(int)(GV.Right_Speed_M_min*Move_Base_Speed_Count_m_Min);

	static int Heartbeat_Flag_01;
	if (TT_Motor_Need_To_Activate == 1)
	{
		ActivateMotor(1, Roughening_Motor_Controller, 6000);
		ActivateMotor(2, Roughening_Motor_Controller, 2000);

		Enable_NMT(000,Roughening_Motor_Controller,01,1000);
		Enable_NMT(000,Roughening_Motor_Controller,02,1000);

		Configure_Asynchronous_Mode(1, Roughening_Motor_Controller, 7,600);
		Consumer_Or_microcontroller_Heartbeat(0x707, Roughening_Motor_Controller,  4);
		Configure_Asynchronous_Mode(2, Roughening_Motor_Controller, 8,600);
		Consumer_Or_microcontroller_Heartbeat(0x707, Roughening_Motor_Controller,  4);
		SpeedModeSetup(1, Roughening_Motor_Controller, 6, 500, 500, 0);
		Consumer_Or_microcontroller_Heartbeat(0x708, Roughening_Motor_Controller,  4);
		SpeedModeSetup(2, Roughening_Motor_Controller, 6, 500, 500, 0);
		Consumer_Or_microcontroller_Heartbeat(0x707, Roughening_Motor_Controller,  4);
		Consumer_Or_microcontroller_Heartbeat(0x708, Roughening_Motor_Controller,  4);
		TT_Motor_Need_To_Activate = 0;
	}
	else
	{
		for (int i = 1; i < 3; i++)//前两个电机
		{
			TT_Request_Position(i, Roughening_Motor_Controller, 6);
			TT_Request_Velocity(i, Roughening_Motor_Controller, 6);
			TT_Request_Current(i, Roughening_Motor_Controller, 6);
			TT_Request_Fault(i, Roughening_Motor_Controller, 6);
		}
		TT_SpeedMode_Set_TargetSpeed(1, Roughening_Motor_Controller, 6,GV.LeftMotor.Target_Velcity);
		TT_SpeedMode_Set_TargetSpeed(2, Roughening_Motor_Controller, 6,GV.RightMotor.Target_Velcity);
	}

	Heartbeat_Flag_01++;
	if(Heartbeat_Flag_01%7==0)
	{
		Consumer_Or_microcontroller_Heartbeat(0x707, Roughening_Motor_Controller,  4);
		Consumer_Or_microcontroller_Heartbeat(0x708, Roughening_Motor_Controller,  4);
		Heartbeat_Flag_01=0;
	}
}



void MotorCommandsLoop_2_Position()
{

	if (TT_Motor_Need_To_Activate_1 == 1)
	{
		ActivateMotor(3, Roughening_Motor_Controller_CAN2, 6000);
		ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000);
		ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000);
		Postion_Velcocity_Run_SetParameter(                   3,                      0,                    0,             500,             500,  Roughening_Motor_Controller_CAN2,    100   );
		TT_Motor_Need_To_Activate_1 = 0;
	}
	else
	{
		for (int i = 3; i <4 ; i++)//前两个电机
		{
			TT_Request_Position(3, Roughening_Motor_Controller_CAN2, 10);

			TT_Request_Fault(3, Roughening_Motor_Controller_CAN2, 10);
		}

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



void MotorCommandsLoop_2()
{
	static int Heartbeat_Flag;
	if (TT_Motor_Need_To_Activate_1 == 1)
	{

		ActivateMotor(3, Roughening_Motor_Controller_CAN2, 6000);
		ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000);
		ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000);
		Enable_NMT(000,Roughening_Motor_Controller_CAN2,03,1000);
		Configure_Asynchronous_Mode(3, Roughening_Motor_Controller_CAN2, 9,1000);
		SpeedModeSetup(3, Roughening_Motor_Controller_CAN2, 12, 500, 500, 0);
		TT_Motor_Need_To_Activate_1 = 0;
	}
	else
	{
		for (int i = 3; i <4 ; i++)//前两个电机
		{
			TT_Request_Position(3, Roughening_Motor_Controller_CAN2, 6);
			TT_Request_Current(3, Roughening_Motor_Controller_CAN2, 6);
			TT_Request_Fault(3, Roughening_Motor_Controller_CAN2, 6);
		}
//		TT_SpeedMode_Set_TargetSpeed(3, Roughening_Motor_Controller_CAN2, 6,GV.SwingMotor.Target_Velcity);
		Heartbeat_Flag++;
		if(Heartbeat_Flag%15==0)
		{
			Consumer_Or_microcontroller_Heartbeat(0x709, Roughening_Motor_Controller_CAN2,  4);
			Heartbeat_Flag=0;
		}
	}
}



char a[10];
void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
{

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

		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;


	}

}