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

uint8_t Motor_Control_Motor_Index;
uint8_t Motor_Control_State;

MotorParameters*  Motor;
uint8_t Motor_FDCAN_CH;
uint8_t Num_Motor=0;

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

void intToByte_B(int Motor_Velocity,uint8_t *Vel_date);
void ByteToint_B(uint8_t Vel_buff[],int *Num_data);
void Tis_Function_Unscramble_B(uint8_t *FDCAN_Rece_DATA_1);
void Tis_Velocity_control(uint32_t Motor_ID_1,  int T_Motor_Velocity_1);
void Tis_Position_control(uint32_t Motor_ID_1,  int T_Motor_Position_1);
void Tis_motor_Stop(uint32_t Motor_ID_1);
void Tis_Cur_Speed_Pos_Read(uint32_t Motor_ID_1);

#define Steer_Speed 100
/**
  * @brief  Tis Motor speed Contronl
  * @param  Motor_ID,
  * @param  Motor Velocity
  * @retval Nall
  */
void Tis_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(T_Motor_Velocity_1, Pos_can_buff_Motor1);
	int Length_date_1=5;
	Pos_can_buff_Motor_To[0]=0x43;
	for(int i=1;i<5;i++)
	{
		Pos_can_buff_Motor_To[i]=Pos_can_buff_Motor1[i-1];
	}
	FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

/**
  * @brief  Tis Motor position Contronl
  * @param  Motor_ID,
  * @param  Motor position
  * @retval Nall
  */
void Tis_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(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(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void Tis_Cur_Speed_Pos_Read(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(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}




void Tis_motor_Stop(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(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void Tis_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(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(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(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(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(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(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data)
{
	GF_BSP_FDCAN_Senddata(Motor_FDCAN_CH,ID,Length,Data);
}


uint8_t GF_MSP_Ti5Motor_Init(uint8_t FDCAN_CH, MotorParameters* MotorP,uint8_t Num_Motor_p)
{
	Motor_FDCAN_CH=FDCAN_CH;
	Motor_Control_State=0;
	Motor_Control_Motor_Index=0;
	Motor = MotorP;
	Num_Motor = Num_Motor_p;
	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_FDCAN1_RxFifo0Callback, GF_MSP_Ti5Motor_CANRx_Callback);
	return 1;
}

void GF_MSP_Ti5Motor_CANRx_Callback()
{
	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(Motor_Data, &Buff_Value_int);
		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Run_Mode=Buff_Value_int;
	break;
	case 4:
		ByteToint_B(Motor_Data, &Buff_Value_int);
		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Current=Buff_Value_int;
	break;
	case 5:
		ByteToint_B(Motor_Data, &Buff_Value_int);
		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Target_Current=Buff_Value_int;
	break;
	case 6:
		ByteToint_B(Motor_Data, &Buff_Value_int);
		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Velcity=Buff_Value_int;
	break;
	case 7:
		ByteToint_B(Motor_Data, &Buff_Value_int);
		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Target_Velcity=Buff_Value_int;
	break;
	case 8:
		ByteToint_B(Motor_Data, &Buff_Value_int);
		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Position=Buff_Value_int;
	break;
	case 9:
		ByteToint_B(Motor_Data, &Buff_Value_int);
		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Target_Position=Buff_Value_int;
	break;
	case 10:
		ByteToint_B(Motor_Data, &Buff_Value_int);
		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].ERROR_Flag=Buff_Value_int;
	break;
	case 49:
		ByteToint_B(Motor_Data, &Buff_Value_int);
		Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Temperature_Motor=Buff_Value_int;
	break;
	case 50:
		ByteToint_B(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;
		}


//	uint8_t buf[8];
//	uint8_t ID_A_T;
//	FDCAN_RxHeaderTypeDef CAN_RX_HDR;
//	uint8_t Function_code;
//	uint8_t Motor_Data[4];
//	int Buff_Value_int;
//	FDCAN_HandleTypeDef *hfdcan;

//	if(Motor_FDCAN_CH==1)
//	{
//		hfdcan = &hfdcan1;
//	}
//	else
//	{
//		hfdcan = &hfdcan2;
//	}
//	if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &CAN_RX_HDR, buf)== HAL_OK)
//	{
//		ID_A_T=CAN_RX_HDR.Identifier;
//		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=buf[0];
//		for (int i=1;i<4;i++)
//		{
//			Motor_Data[i-1]=buf[i];
//		}
//
//		switch(Function_code)
//		{
//		case 3:
//			ByteToint_B(Motor_Data, &Buff_Value_int);
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Run_Mode=Buff_Value_int;
//		break;
//		case 4:
//			ByteToint_B(Motor_Data, &Buff_Value_int);
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Current=Buff_Value_int;
//		break;
//		case 5:
//			ByteToint_B(Motor_Data, &Buff_Value_int);
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Target_Current=Buff_Value_int;
//		break;
//		case 6:
//			ByteToint_B(Motor_Data, &Buff_Value_int);
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Velcity=Buff_Value_int;
//		break;
//		case 7:
//			ByteToint_B(Motor_Data, &Buff_Value_int);
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Target_Velcity=Buff_Value_int;
//		break;
//		case 8:
//			ByteToint_B(Motor_Data, &Buff_Value_int);
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Position=Buff_Value_int;
//		break;
//		case 9:
//			ByteToint_B(Motor_Data, &Buff_Value_int);
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Target_Position=Buff_Value_int;
//		break;
//		case 10:
//			ByteToint_B(Motor_Data, &Buff_Value_int);
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].ERROR_Flag=Buff_Value_int;
//		break;
//		case 49:
//			ByteToint_B(Motor_Data, &Buff_Value_int);
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Temperature_Motor=Buff_Value_int;
//		break;
//		case 50:
//			ByteToint_B(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)((buf[0]) | ((buf[1]))<<8);
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Velcity=(short)(buf[2]| (buf[3])<<8);
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Position=(int)(buf[4] | (buf[5])<<8| (buf[6])<<16| (buf[7])<<24);
//			break;
//		default:
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Current=(short)((buf[0]) | ((buf[1]))<<8);
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Velcity=(short)(buf[2]| (buf[3])<<8);
//			Motor[ID_A_T-DF_MSP_Ti5Motor_StartID].Position=(int)(buf[4] | (buf[5])<<8| (buf[6])<<16| (buf[7])<<24);
//		break;
//			}
//	}

//	int	ID_A_T=(int)(hfdcan[0]);
//	int Function_code=(int)(hfdcan[1]);
//	uint8_t Motor_Data[4];
//	Motor_Data[0]=hfdcan[2];Motor_Data[1]=hfdcan[3];Motor_Data[2]=hfdcan[4];Motor_Data[3]=hfdcan[5];

}


//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

uint8_t Motor_ID_IDD;
void GF_MSP_Ti5Motor_Control_Timer()
{
	int Length_date;
	uint8_t Pos_can_buff_Motor1[8];
	uint8_t Motor_Run_Mode=Motor[Motor_Control_Motor_Index].Run_Mode;
	uint8_t Motor_ID=Motor[Motor_Control_Motor_Index].MotorID;
	Motor_ID_IDD= Motor_ID;
	switch(Motor_Control_State)
	{
	case 0:
		switch( Motor_Run_Mode )  //butong de moshi
		{
		case DF_MSP_Ti5Motor_CurrentMode:// 28(不带返回值)
			Tis_Current_control(Motor_ID,Motor[Motor_Control_Motor_Index].Target_Current);
			break;
		case DF_MSP_Ti5Motor_SpeedMode:// 1D(不带返回值)
			Tis_Velocity_control(Motor_ID,Motor[Motor_Control_Motor_Index].Target_Velcity);
			break;
		case DF_MSP_Ti5Motor_PositionMode:// 1E(不带返回值)
			Tis_Position_control(Motor_ID,Motor[Motor_Control_Motor_Index].Target_Position);
			break;
		case  DF_MSP_Ti5Motor_State_Read://41 Read motor state
			Tis_Cur_Speed_Pos_Read(Motor_ID);
		break;
		}
		Motor_Control_State=Motor_Control_State+1;
		break;
	case 1: //采集错误指令
		Length_date=1;
		Pos_can_buff_Motor1[0]=0x0A;
		FDCANfunction(Motor_ID, Length_date,Pos_can_buff_Motor1);
		Motor_Control_State=Motor_Control_State+1;
		break;

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

	case 3:
		Length_date=1;
		Pos_can_buff_Motor1[0]=0x32;
		FDCANfunction(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=0;
		break;
	}
}


