/*
 * msp_TI5MOTOR.c
 *
 *  Created on: Oct 26, 2023
 *      Author: shiya
 */
#include "MSP/msp_TI5MOTOR_CANOPEN.h"


uint8_t Motor_Control_State_CANOPEN;

CANOPENPMotorarameters*  Motor_CANOPEN;
uint8_t Motor_FDCAN_CH_CANOPEN;
uint8_t Num_Motor_CANOPEN=0;

void GF_MSP_Ti5Motor_CANRx_Callback_CANOPEN();

void FDCANfunction_CANOPEN(uint32_t ID,uint8_t Length,uint8_t *Data);/*///FrameID, int Length_d,uint8_t *Txdata  //CAN data transmission function*/

void intToByte_B_CANOPEN(int Motor_Velocity,uint8_t *Vel_date);
void ByteToint_B_CANOPEN(uint8_t Vel_buff[],int *Num_data);
void Tis_Function_Unscramble_B_CANOPEN(uint8_t *FDCAN_Rece_DATA_1);
void Tis_Velocity_control_CANOPEN(uint32_t Motor_ID_1,  int T_Motor_Velocity_1);
void Tis_Position_control_CANOPEN(uint32_t Motor_ID_1,  int T_Motor_Position_1);
void Tis_motor_Stop_CANOPEN(uint32_t Motor_ID_1);
void Tis_Cur_Speed_Pos_Read_CANOPEN(uint32_t Motor_ID_1);

void Tis_Motor_Powon_CANOPEN();

/**
  * @brief  Tis Motor speed Contronl
  * @param  Motor_ID,
  * @param  Motor Velocity
  * @retval Nall
  */
int Powon_Flag=1;
void Tis_Velocity_control_CANOPEN(uint32_t Motor_ID_1,  int T_Motor_Velocity_1)
{
	Tis_Motor_Powon_CANOPEN();
	uint8_t Pos_can_buff_Motor1[4];uint8_t Pos_can_buff_Motor_To[8];
	intToByte_B_CANOPEN(T_Motor_Velocity_1, Pos_can_buff_Motor1);
	int Length_date_1=8;

	if(Powon_Flag==4022)
	{
		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<8;i++)
		{
			Pos_can_buff_Motor_To[i]=Pos_can_buff_Motor1[i-4];
		}
		FDCANfunction_CANOPEN(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
	}

}




void Tis_Motor_Powon_CANOPEN()
{
	uint8_t Pos_can_buff_Motor_To_1[8];
	int Length_date_1=8;
	Powon_Flag++;
	if(Powon_Flag>4022)
	{
	Powon_Flag=4022;
	}

	switch(Powon_Flag)  //butong de moshi
	{
	case 4001:// 28(不带返回值)
	case 4002:case 4003:case 4004:case 4005:
		Pos_can_buff_Motor_To_1[0]=0x2B;
		Pos_can_buff_Motor_To_1[1]=0x40;
		Pos_can_buff_Motor_To_1[2]=0x60;
		Pos_can_buff_Motor_To_1[3]=0x00;

		Pos_can_buff_Motor_To_1[4]=0x0F;
		Pos_can_buff_Motor_To_1[5]=0x00;
		Pos_can_buff_Motor_To_1[6]=0x00;
		Pos_can_buff_Motor_To_1[7]=0x00;
		FDCANfunction_CANOPEN(1537, Length_date_1,Pos_can_buff_Motor_To_1);

		break;
	case 4006:// 1D(不带返回值)
	case 4007:case 4008:case 4009:case 4010:
		Pos_can_buff_Motor_To_1[0]=0x2F;
		Pos_can_buff_Motor_To_1[1]=0x60;
		Pos_can_buff_Motor_To_1[2]=0x60;
		Pos_can_buff_Motor_To_1[3]=0x00;

		Pos_can_buff_Motor_To_1[4]=0x03;
		Pos_can_buff_Motor_To_1[5]=0x00;
		Pos_can_buff_Motor_To_1[6]=0x00;
		Pos_can_buff_Motor_To_1[7]=0x00;
		FDCANfunction_CANOPEN(1537, Length_date_1,Pos_can_buff_Motor_To_1);

		break;
	case 4011:// 28(不带返回值)
	case 4012:case 4013:case 4014:case 4015:
		Pos_can_buff_Motor_To_1[0]=0x2B;
		Pos_can_buff_Motor_To_1[1]=0x40;
		Pos_can_buff_Motor_To_1[2]=0x60;
		Pos_can_buff_Motor_To_1[3]=0x00;

		Pos_can_buff_Motor_To_1[4]=0x0F;
		Pos_can_buff_Motor_To_1[5]=0x00;
		Pos_can_buff_Motor_To_1[6]=0x00;
		Pos_can_buff_Motor_To_1[7]=0x00;
		FDCANfunction_CANOPEN(1538, Length_date_1,Pos_can_buff_Motor_To_1);

		break;
	case 4016:// 1D(不带返回值)
	case 4017:case 4018:case 4019:case 4020:
		Pos_can_buff_Motor_To_1[0]=0x2F;
		Pos_can_buff_Motor_To_1[1]=0x60;
		Pos_can_buff_Motor_To_1[2]=0x60;
		Pos_can_buff_Motor_To_1[3]=0x00;

		Pos_can_buff_Motor_To_1[4]=0x03;
		Pos_can_buff_Motor_To_1[5]=0x00;
		Pos_can_buff_Motor_To_1[6]=0x00;
		Pos_can_buff_Motor_To_1[7]=0x00;
		FDCANfunction_CANOPEN(1538, Length_date_1,Pos_can_buff_Motor_To_1);

		break;
		default:

		break;
	}

}




int TI5_ZQ_ID_Start=0;

uint8_t Motor_Control_Motor_Index_CANOPEN;
uint8_t GF_MSP_Ti5Motor_CANOPEN_Init(uint8_t FDCAN_CH, CANOPENPMotorarameters* MotorP,uint8_t ID_Start,uint8_t Num_Motor_p)
{
	Motor_FDCAN_CH_CANOPEN=FDCAN_CH;
	Motor_Control_State_CANOPEN=0;
	Motor_Control_Motor_Index_CANOPEN=1;
	Motor_CANOPEN = MotorP;
	TI5_ZQ_ID_Start=ID_Start;
	Num_Motor_CANOPEN = Num_Motor_p;
	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_FDCAN1_RxFifo0Callback, GF_MSP_Ti5Motor_CANRx_Callback_CANOPEN);
	return 1;
}



/**
  * @brief  Tis Motor position Contronl
  * @param  Motor_ID,
  * @param  Motor position
  * @retval Nall
  */
void Tis_Position_control_CANOPEN(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_CANOPEN(T_Motor_Position_1, Pos_can_buff_Motor1);
	int Length_date_1=5;
	Pos_can_buff_Motor_To[0]=0x44;
	for(int i=1;i<5;i++)
	{
		Pos_can_buff_Motor_To[i]=Pos_can_buff_Motor1[i-1];
	}
	FDCANfunction_CANOPEN(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void Tis_Cur_Speed_Pos_Read_CANOPEN(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=1;
	Pos_can_buff_Motor_To[0]=0x41;
	FDCANfunction_CANOPEN(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void Tis_Cur_Read_CANOPEN(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]=0x78;
	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;

	FDCANfunction_CANOPEN(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}



void Tis_Fault_Read_CANOPEN(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]=0x3F;
	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;

	FDCANfunction_CANOPEN(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


void Tis_motor_Stop_CANOPEN(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=1;
	Pos_can_buff_Motor_To[0]=0x02;
	FDCANfunction_CANOPEN(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void Tis_Current_control_CANOPEN(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_CANOPEN(T_Motor_Current_1, Pos_can_buff_Motor1);
	int Length_date_1=5;
	Pos_can_buff_Motor_To[0]=0x42;
	for(int i=1;i<5;i++)
	{
		Pos_can_buff_Motor_To[i]=Pos_can_buff_Motor1[i-1];
	}
	FDCANfunction_CANOPEN(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


/**
  * @brief  int to byte transfer function
  * @param  int value
  * @param  4 byte value
  * @retval input int ;output byte value
  */
void intToByte_B_CANOPEN(int Motor_Velocity,uint8_t *Vel_date)
{
	uint32_t Motor_Velocity_U= (uint32_t)Motor_Velocity;
	//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;
}

/**
  * @brief  byte to int transfer function
  * @param  4 byte value
  * @param  int value
  * @retval input byte ;output int
  */
void ByteToint_B_CANOPEN(uint8_t Vel_buff[],int *Num_data)
{
	*Num_data =(int)(Vel_buff[0] | (Vel_buff[1])<<8|(Vel_buff[2])<<16|(Vel_buff[3])<<24);
}

/**
  * @brief  Tis Motor current position reading
  * @param  Motor_ID,
  * @retval Motor position
  */
void Tis_Position_Read_CANOPEN(uint32_t Motor_ID_1,  int T_Motor_Position_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=1;
	Pos_can_buff_Motor_To[0]=0x08;
	FDCANfunction_CANOPEN(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void FDCANfunction_CANOPEN(uint32_t ID,uint8_t Length,uint8_t *Data)
{
	GF_BSP_FDCAN_Senddata(Motor_FDCAN_CH_CANOPEN,ID,Length,Data);
}




void GF_MSP_Ti5Motor_CANRx_Callback_CANOPEN()
{

	int32_t TI5_ID_Value=0X580+Motor_CANOPEN[TI5_ZQ_ID_Start].MotorID;
	int32_t TI5_ID_Value_max=TI5_ID_Value+Num_Motor_CANOPEN;
	if((CAN_ID>=TI5_ID_Value)&&(CAN_ID<TI5_ID_Value_max))
	{
		int32_t TI5_ID_A_T;
		TI5_ID_A_T=CAN_ID-0X580;
		int  TI5_Function_code=(int)((int16_t)(CAN_Buf[1]|CAN_Buf[2]<<8));
		switch(TI5_Function_code)
		{
			case 24696:
				//Motor_CANOPEN[TI5_ID_A_T].Current=(int)(CAN_Buf[4] | (CAN_Buf[5])<<8| (CAN_Buf[6])<<16| (CAN_Buf[7]<<24));//0.1A
				Motor_CANOPEN[TI5_ID_A_T].Current=(int)((int16_t)(CAN_Buf[4] | (CAN_Buf[5]<<8)));//0.1A
				break;
			case 24639:
				Motor_CANOPEN[TI5_ID_A_T].ERROR_Flag=(int)(CAN_Buf[4] | (CAN_Buf[5])<<8| (CAN_Buf[6])<<16| (CAN_Buf[7]<<24));
				break;
		}
	}


//			TT_Motor[TT_ID_Start].MotorID;
//	int32_t ID_Value_max=ID_Value+TT_Num_Motor;
//	if((CAN_ID>=ID_Value)&&(CAN_ID<=ID_Value_max))
//	{
//	TT_Analytic_Fun();
//	}










//	uint8_t ID_A_T;
//	uint8_t Function_code;
//	uint8_t Motor_Data[4];
//	int Buff_Value_int;
//
//	ID_A_T=CAN_ID;
//	if(ID_A_T>=DF_MSP_Ti5Motor_StartID && ID_A_T<DF_MSP_Ti5Motor_StartID+Num_Motor)
//	{
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].RxIndex++;
//	}
//	Function_code=CAN_Buf[0];
//	for (int i=1;i<4;i++)
//	{
//		Motor_Data[i-1]=CAN_Buf[i];
//	}
//	switch(Function_code)
//	{
//	case 3:
//		ByteToint_B_CANOPEN(Motor_Data, &Buff_Value_int);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Run_Mode=Buff_Value_int;
//	break;
//	case 4:
//		ByteToint_B_CANOPEN(Motor_Data, &Buff_Value_int);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Current=Buff_Value_int;
//	break;
//	case 5:
//		ByteToint_B_CANOPEN(Motor_Data, &Buff_Value_int);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Target_Current=Buff_Value_int;
//	break;
//	case 6:
//		ByteToint_B_CANOPEN(Motor_Data, &Buff_Value_int);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Velcity=Buff_Value_int;
//	break;
//	case 7:
//		ByteToint_B_CANOPEN(Motor_Data, &Buff_Value_int);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Target_Velcity=Buff_Value_int;
//	break;
//	case 8:
//		ByteToint_B_CANOPEN(Motor_Data, &Buff_Value_int);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Position=Buff_Value_int;
//	break;
//	case 9:
//		ByteToint_B_CANOPEN(Motor_Data, &Buff_Value_int);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Target_Position=Buff_Value_int;
//	break;
//	case 10:
//		ByteToint_B_CANOPEN(Motor_Data, &Buff_Value_int);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].ERROR_Flag=Buff_Value_int;
//	break;
//	case 49:
//		ByteToint_B_CANOPEN(Motor_Data, &Buff_Value_int);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Temperature_Motor=Buff_Value_int;
//	break;
//	case 50:
//		ByteToint_B_CANOPEN(Motor_Data, &Buff_Value_int);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Temperature_PCB=Buff_Value_int;
//	break;
//		case 65:case 66:
//		case 67:case 68:
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Current=(short)((CAN_Buf[0]) | ((CAN_Buf[1]))<<8);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Velcity=(short)(CAN_Buf[2]| (CAN_Buf[3])<<8);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Position=(int)(CAN_Buf[4] | (CAN_Buf[5])<<8| (CAN_Buf[6])<<16| (CAN_Buf[7])<<24);
//		break;
//	default:
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Current=(short)((CAN_Buf[0]) | ((CAN_Buf[1]))<<8);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Velcity=(short)(CAN_Buf[2]| (CAN_Buf[3])<<8);
//		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Position=(int)(CAN_Buf[4] | (CAN_Buf[5])<<8| (CAN_Buf[6])<<16| (CAN_Buf[7])<<24);
//	break;
//		}


}


//Motor_Control_State::
//0.control 
//1.capture -1
//2.capture -2
//3.capture -3
//4.Motor_Control_Motor_Index++

//Motor_Control_Motor_Index::
//start is 0; 0\1\2\3\4


void GF_MSP_Ti5Motor_Control_Timer_CANOPEN()
{

	uint8_t Motor_Run_Mode=Motor_CANOPEN[Motor_Control_Motor_Index_CANOPEN].Run_Mode;
	int16_t Motor_ID=Motor_CANOPEN[Motor_Control_Motor_Index_CANOPEN].MotorID+1536;

	switch(Motor_Control_State_CANOPEN)
	{
	case 0:
		switch( Motor_Run_Mode )  //butong de moshi
		{
		case DF_MSP_Ti5Motor_Current_CANOPEN:// 28(不带返回值)
			Tis_Current_control_CANOPEN(Motor_ID,Motor_CANOPEN[Motor_Control_Motor_Index_CANOPEN].Target_Current);
			break;
		case DF_MSP_Ti5Motor_Speed_CANOPEN:// 1D(不带返回值)
			Tis_Velocity_control_CANOPEN(Motor_ID,Motor_CANOPEN[Motor_Control_Motor_Index_CANOPEN].Target_Velcity);
			break;
		case DF_MSP_Ti5Motor_Position_CANOPEN:// 1E(不带返回值)
			Tis_Position_control_CANOPEN(Motor_ID,Motor_CANOPEN[Motor_Control_Motor_Index_CANOPEN].Target_Position);
			break;
		case  DF_MSP_Ti5Motor_State_CANOPEN://41 Read motor state
			Tis_Cur_Speed_Pos_Read_CANOPEN(Motor_ID);
		break;
		}
		Motor_Control_State_CANOPEN=1;
		break;
	case 1:
		Tis_Fault_Read_CANOPEN(Motor_ID);
		Motor_Control_State_CANOPEN=2;
		break;
	case 2:
		Tis_Cur_Read_CANOPEN(Motor_ID);
		Motor_Control_State_CANOPEN=0;

		Motor_Control_Motor_Index_CANOPEN++;
		if(Motor_Control_Motor_Index_CANOPEN > Num_Motor_CANOPEN)
		{
		Motor_Control_Motor_Index_CANOPEN=1;
		}
		break;



//	case 1: //采集错误指令
//		Length_date=1;
//		Pos_can_buff_Motor1[0]=0x0A;
//		FDCANfunction_CANOPEN(Motor_ID, Length_date,Pos_can_buff_Motor1);
//		Motor_Control_State_CANOPEN=Motor_Control_State_CANOPEN+1;
//		break;
//
//	case 2: //采集电机温度
//		Length_date=1;
//		Pos_can_buff_Motor1[0]=0x31;
//		FDCANfunction_CANOPEN(Motor_ID, Length_date,Pos_can_buff_Motor1);
//		Motor_Control_State_CANOPEN=Motor_Control_State_CANOPEN+1;
//		break;
//
//	case 3:
//		Length_date=1;
//		Pos_can_buff_Motor1[0]=0x32;
//		FDCANfunction_CANOPEN(Motor_ID, Length_date,Pos_can_buff_Motor1);
//		Motor_Control_Motor_Index++;
//		if(Motor_Control_Motor_Index >= Num_Motor)
//		{
//			Motor_Control_Motor_Index=0;
//		}
//		Motor_Control_State_CANOPEN=0;
//		break;
	}
}


