/*
 * msp_FH_Motor.c
 *
 *  Created on: 2023年12月12日
 *      Author: Administrator
 */
//#include "BHBF_ROBOT.h"
#include "MSP/msp_FH_Motor.h"


uint8_t  Motor_FDCAN_CH_1;
uint8_t FH_Motor_Control_State;
uint16_t FH_Motor_Control_Motor_Index;
FH_MotorParameters*  FH_Motor;
uint8_t FH_Num_Motor=0;


uint16_t  ID_Sent_Offset=600;
uint16_t  ID_Recv_Offset=580;


void FH_FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data);/*///FrameID, int Length_d,uint8_t *Txdata  //CAN data transmission function*/
uint8_t GF_MSP_FH_Motor_Init(uint8_t FDCAN_CH, FH_MotorParameters* MotorP,uint8_t Num_Motor_p);
void GF_MSP_FH_Motor_Control_Timer();
void FH_Current_control(uint32_t Motor_ID_1,  int T_Motor_Current_1);
void FH_Velocity_control(uint32_t Motor_ID_1,  int T_Motor_Velocity_1);
void FH_Position_control(uint32_t Motor_ID_1,  int T_Motor_Position_1);
void intToByte_B_FH(int Motor_Velocity,uint8_t *Vel_date);
void Enter_CAN_operation();

void Motor_Fault_Read(uint32_t Motor_ID_1);


uint8_t GF_MSP_FH_Motor_Init(uint8_t FDCAN_CH, FH_MotorParameters* MotorP,uint8_t Num_Motor_p)
{
	Motor_FDCAN_CH_1=FDCAN_CH;
	FH_Motor_Control_State=0;
	FH_Motor_Control_Motor_Index=ID_Sent_Offset;
	FH_Motor = MotorP;
	FH_Num_Motor = Num_Motor_p;
	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_FDCAN1_RxFifo0Callback, 1);
	Enter_CAN_operation();
	return 1;
}

//开启FH电机CAN通讯 ID 000 数据01 00
void Enter_CAN_operation()
{
	uint32_t Motor_ID_1=000;
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=2;
	Pos_can_buff_Motor_To[0]=0x01;
	Pos_can_buff_Motor_To[1]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}



void FH_FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data)
{
	GF_BSP_FDCAN_Senddata(Motor_FDCAN_CH_1,ID,Length,Data);
}


uint8_t FH_Motor_ID_IDD;

void GF_MSP_FH_Motor_Control_Timer()
{
	int Length_date;
	uint8_t Pos_can_buff_Motor1[8];
	uint8_t Motor_Run_Mode=FH_Motor[FH_Motor_Control_Motor_Index].Run_Mode;
	uint8_t Motor_ID=FH_Motor[FH_Motor_Control_Motor_Index].MotorID;
	FH_Motor_ID_IDD= Motor_ID;
	switch(FH_Motor_Control_State)
	{
	case 0:
		switch( Motor_Run_Mode )  //butong de moshi
		{
		case DF_MSP_FH_Motor_CurrentMode:// 28(不带返回值)
			FH_Current_control(Motor_ID,FH_Motor[FH_Motor_Control_Motor_Index].Target_Current);
			break;
		case DF_MSP_FH_Motor_SpeedMode:// 1D(不带返回值)
			FH_Velocity_control(Motor_ID,FH_Motor[FH_Motor_Control_Motor_Index].Target_Velcity);
			break;
		case DF_MSP_FH_Motor_PositionMode:// 1E(不带返回值)
			FH_Position_control(Motor_ID,FH_Motor[FH_Motor_Control_Motor_Index].Target_Position);
			break;
		}
		FH_Motor_Control_State=FH_Motor_Control_State+1;
		break;
	case 1: //采集错误指令
		Length_date=1;
		Pos_can_buff_Motor1[0]=0x0A;
		FH_FDCANfunction(Motor_ID, Length_date,Pos_can_buff_Motor1);
		FH_Motor_Control_State=FH_Motor_Control_State+1;
		break;

	case 2: //采集电机温度
		Length_date=1;
		Pos_can_buff_Motor1[0]=0x31;
		FH_FDCANfunction(Motor_ID, Length_date,Pos_can_buff_Motor1);
		FH_Motor_Control_State=FH_Motor_Control_State+1;
		break;

	case 3:
		Length_date=1;
		Pos_can_buff_Motor1[0]=0x32;
		FH_FDCANfunction(Motor_ID, Length_date,Pos_can_buff_Motor1);
		FH_Motor_Control_Motor_Index++;
		if(FH_Motor_Control_Motor_Index >= FH_Num_Motor)
		{
			FH_Motor_Control_Motor_Index=0;
		}
		FH_Motor_Control_State=0;
		break;
	}
}

void FH_Motor_PowerON(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x2B;
	Pos_can_buff_Motor_To[1]=0x40;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0x0F;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void FH_Motor_PowerOFF(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x2B;
	Pos_can_buff_Motor_To[1]=0x40;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0x00;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void All_motors_Fault_reset()
{
	uint32_t Motor_ID_1=000;
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=2;
	Pos_can_buff_Motor_To[0]=0x81;
	Pos_can_buff_Motor_To[1]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}



void FH_Velocity_control(uint32_t Motor_ID_1,  int T_Motor_Velocity_1)
{
	uint8_t Pos_can_buff_Motor1[4];uint8_t Pos_can_buff_Motor_To[8];
	intToByte_B_FH(T_Motor_Velocity_1, Pos_can_buff_Motor1);
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x23;
	Pos_can_buff_Motor_To[1]=0xFF;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;

	for(int i=4;i<Length_date_1;i++)
	{
		Pos_can_buff_Motor_To[i]=Pos_can_buff_Motor1[i-4];
	}
	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


void FH_Current_control(uint32_t Motor_ID_1,  int T_Motor_Current_1)
{
	uint8_t Pos_can_buff_Motor1[4];uint8_t Pos_can_buff_Motor_To[8];
	intToByte_B_FH(T_Motor_Current_1, Pos_can_buff_Motor1);
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x2B;
	Pos_can_buff_Motor_To[1]=0x40;
	Pos_can_buff_Motor_To[2]=0x23;
	Pos_can_buff_Motor_To[3]=0x00;

	for(int i=4;i<Length_date_1;i++)
	{
		Pos_can_buff_Motor_To[i]=Pos_can_buff_Motor1[i-4];
	}
	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


void FH_Position_control(uint32_t Motor_ID_1,  int T_Motor_Position_1)
{
	uint8_t Pos_can_buff_Motor1[4];uint8_t Pos_can_buff_Motor_To[8];
	intToByte_B_FH(T_Motor_Position_1, Pos_can_buff_Motor1);
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x23;
	Pos_can_buff_Motor_To[1]=0x7A;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;

	for(int i=4;i<Length_date_1;i++)
	{
		Pos_can_buff_Motor_To[i]=Pos_can_buff_Motor1[i-4];
	}
	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


void intToByte_B_FH(int Motor_Velocity,uint8_t *Vel_date)
{
	uint32_t Motor_Velocity_U= (uint32_t)Motor_Velocity*10;
	//int2byte
	Vel_date[0] =Motor_Velocity_U & 0xff;
	Vel_date[1] =(Motor_Velocity_U & 0xff00)>>8;
	Vel_date[2] =(Motor_Velocity_U & 0xff0000)>>16;
	Vel_date[3] =(Motor_Velocity_U & 0xff000000)>>24;
}


void Request_Read_Position(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x40;
	Pos_can_buff_Motor_To[1]=0x64;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0x00;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


void Request_Read_Velocity(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x40;
	Pos_can_buff_Motor_To[1]=0x69;
	Pos_can_buff_Motor_To[2]=0x60;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0x00;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void Request_Read_Current(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x40;
	Pos_can_buff_Motor_To[1]=0x1C;
	Pos_can_buff_Motor_To[2]=0x22;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0x00;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void Motor_Fault_Read(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x23;
	Pos_can_buff_Motor_To[1]=0x83;
	Pos_can_buff_Motor_To[2]=0x21;
	Pos_can_buff_Motor_To[3]=0x00;

	Pos_can_buff_Motor_To[4]=0x00;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	FH_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


void Fault_Anal(uint8_t Data_Value[])
{
		int32_t Checksum_Sum=(int32_t)((int32_t)Data_Value[3]<<24|Data_Value[2]<<16|Data_Value[1]<<8|Data_Value[0]);

		//0
		if(((Data_Value[0])&(0X01))>0)
		{
			FH_Motor->CRC_Error=1;
		}
		else
		{
			FH_Motor->CRC_Error=0;
		}
		//1
		if(((Data_Value[0])&(0X02))>0)
		{
			FH_Motor->Dri_Inter_Error=1;
		}
		else
		{
			FH_Motor->Dri_Inter_Error=0;
		}
		//2
		if(((Data_Value[0])&(0X04))>0)
		{
			FH_Motor->Short_circuit_error=1;
		}
		else
		{
			FH_Motor->Short_circuit_error=0;
		}
		//3
		if(((Data_Value[0])&(0X08))>0)
		{
			FH_Motor->Drive_overheating=1;
		}
		else
		{
			FH_Motor->Drive_overheating=0;
		}
		//4
		if(((Data_Value[0])&(0X10))>0)
		{
			FH_Motor->Motor_overheating=1;
		}
		else
		{
			FH_Motor->Motor_overheating=0;
		}

		//5
		if(((Data_Value[0])&(0X20))>0)
		{
			FH_Motor->Overvoltage=1;
		}
		else
		{
			FH_Motor->Overvoltage=0;
		}

		//6
		if(((Data_Value[0])&(0X40))>0)
		{
			FH_Motor->Undervoltage=1;
		}
		else
		{
			FH_Motor->Undervoltage=0;
		}
		//7
		if(((Data_Value[0])&(0X80))>0)
		{
			FH_Motor->Feedback_error=1;
		}
		else
		{
			FH_Motor->Feedback_error=0;
		}

		//8
		if(((Checksum_Sum)&(0X100))>0)
		{
			FH_Motor->Phase_error=1;
		}
		else
		{
			FH_Motor->Phase_error=0;
		}
		//9
		if(((Checksum_Sum)&(0X200))>0)
		{
			FH_Motor->Follow_error=1;
		}
		else
		{
			FH_Motor->Follow_error=0;
		}
		//10
		if(((Checksum_Sum)&(0X400))>0)
		{
			FH_Motor->Overcurrent=1;
		}
		else
		{
			FH_Motor->Overcurrent=0;
		}

		//11
		if(((Checksum_Sum)&(0X800))>0)
		{
			FH_Motor->FPGA_error=1;
		}
		else
		{
			FH_Motor->FPGA_error=0;
		}
		//12
		if(((Checksum_Sum)&(0X1000))>0)
		{
			FH_Motor->Input_command_error=1;
		}
		else
		{
			FH_Motor->Input_command_error=0;
		}

}




















