/*
 * motors.c
 *
 *  Created on: Nov 11, 2024
 *      Author: akeguo
 */
#include "motors.h"
#include "msp_TTMotor_ZQ.h"

char TT_Motor_Need_To_Activate = 1;
char TT_Motor_Need_To_Activate_1 = 1;
FDCANHandler *Roughening_Motor_Controller;

FDCANHandler *Roughening_Motor_Controller_CAN2;

DispacherController *Roughening_DispacherController;
DispacherController *Roughening_DispacherController_CAN2;
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);
//	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID3_SwingMotor", 0, ComError_ZQ_CAN_ID3_SwingMotor);

	Roughening_DispacherController = Handler->dispacherController;
	Roughening_DispacherController->DispacherCallTime = 2;
	Roughening_DispacherController->Add_Dispatcher_List(Roughening_DispacherController, MotorCommandsLoop);

}
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()
{
	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()
{
	static int Heartbeat_Flag;
	static int Lag_Sent_Num=0;
	static int32_t Speed_Last=0;
	static int32_t Position_Last=0;

	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,500);
//		Consumer_Or_microcontroller_Heartbeat(0x709, Roughening_Motor_Controller_CAN2,  4);


//		void SpeedModeSetup(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller,    int32_t WaitTime, int32_t Acc, int32_t Dec, int32_t TargetVelocity) //设定速度模式，并更改相关速度
//		SpeedModeSetup(3, Roughening_Motor_Controller_CAN2, 12, 500, 500, 0);

		Postion_Velcocity_Run_SetParameter(                   3,                      0,                    0,             500,             500,  Roughening_Motor_Controller_CAN2,    100   );
//		void Postion_Velcocity_Run_SetParameter(int32_t MotorID, int32_t TargetPosition,  int32_t TargetSpeed, int32_t AccTime, int32_t DecTime, FDCANHandler *ZQ_Motor_Controller, int32_t WaitTime)
		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_Current(3, Roughening_Motor_Controller_CAN2, 10);
//			TT_Request_Velocity(3, Roughening_Motor_Controller_CAN2, 10);
			TT_Request_Fault(3, Roughening_Motor_Controller_CAN2, 10);
		}

		switch(GV.SwingMotor.Position_immediately1_Lag2)
		{
			case 1:
//				if((Position_Last!=GV.SwingMotor.Tar_Position_count)||(Speed_Last!=GV.SwingMotor.Tar_Position_Velcity_RPM))
//				{
//				Position_Immediately_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,20);
//				Position_Immediately_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
//				}
//				Position_Last=GV.SwingMotor.Tar_Position_count;
//				Speed_Last=GV.SwingMotor.Tar_Position_Velcity_RPM;
				Position_Immediately_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
				break;
			case 2:
//				if((Position_Last!=GV.SwingMotor.Tar_Position_count)||(Speed_Last!=GV.SwingMotor.Tar_Position_Velcity_RPM))
//				{
//					Position_Lag_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
////					Position_Lag_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
//				}
//				Position_Last=GV.SwingMotor.Tar_Position_count;
//				Speed_Last=GV.SwingMotor.Tar_Position_Velcity_RPM;

				Position_Lag_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);

				break;
		}
	}

//	Heartbeat_Flag++;
//	if(Heartbeat_Flag%5==0)
//	{
//		Consumer_Or_microcontroller_Heartbeat(0x709, Roughening_Motor_Controller_CAN2,  4);
//		Heartbeat_Flag=0;
//	}
}









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;


	}

}