/*
 * msp_TI5MOTOR.c
 *
 *  Created on: Oct 26, 2023
 *      Author: shiya
 */
#include "MSP/msp_TI5MOTOR.h"
#include "MSP/msp_Water_Cannon.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_motor_Fault_Reset(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_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_motor_Fault_Reset(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=1;
	Pos_can_buff_Motor_To[0]=0x0B;
	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;
}
int32_t count_bm_03=0;
int32_t count_bm_05=0;
void GF_MSP_Ti5Motor_CANRx_Callback()
{
	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==5)
		{
			count_bm_05++;
		}
		if(ID_A_T==3)
		{
			count_bm_03++;
		}

		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;
int count_Flag_bmbm_05=0;

uint8_t Pos_can_buff_Motor1[8];
uint8_t Motor_Run_Mode;
uint8_t Motor_ID_N;
int	Target_Velcity;

int count = 0;

void GF_MSP_Ti5Motor_Control_Timer()
{
	int Length_date;
	Pos_can_buff_Motor1[8];
	Motor_Run_Mode=Motor[Motor_Control_Motor_Index].Run_Mode;
	Motor_ID_N=Motor[Motor_Control_Motor_Index].MotorID;
	Target_Velcity = Motor[Motor_Control_Motor_Index].Target_Velcity;

	Motor_ID_IDD= Motor_ID_N;

	if(Motor_Control_State==5)
	{
		count_Flag_bmbm_05++;
	}

count ++ ;
if(count == 2)
{
	count = 0;
	switch(Motor_Control_State)
	{
	case 0:
		switch( Motor_Run_Mode )  //butong de moshi
		{
		case DF_MSP_Ti5Motor_CurrentMode:// 28(不带返回值)
			Tis_Current_control(Motor_ID_N,Motor[Motor_Control_Motor_Index].Target_Current);
			break;
		case DF_MSP_Ti5Motor_SpeedMode:// 1D(不带返回值)
			Tis_Velocity_control(Motor_ID_N, Target_Velcity);
			break;
		case DF_MSP_Ti5Motor_PositionMode:// 1E(不带返回值)
			Tis_Position_control(Motor_ID_N,Motor[Motor_Control_Motor_Index].Target_Position);
			break;
		case DF_MSP_Ti5Motor_StopMode://
			Tis_motor_Stop(Motor_ID_N);
			break;
		case DF_MSP_Ti5Motor_Fault_Reset://
			Tis_motor_Fault_Reset(Motor_ID_N);
			break;
		default:
		break;

		}
		Motor_Control_State=Motor_Control_State+1;
		break;
	case 1: //采集错误指令
		Length_date=1;
		Pos_can_buff_Motor1[0]=0x0A;
		FDCANfunction(Motor_ID_N, 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_N, 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_N, Length_date,Pos_can_buff_Motor1);
		Motor_Control_Motor_Index++;
		Motor_Control_State=Motor_Control_State+1;

		break;
	case 4:
		GF_MSP_Wate_Cannon_Contronl_1(Motor[3].WC_MotorID, Motor[3].WC_Clean_Swi, Motor[3].WC_Scan_Hor1_Ver2, Motor[3].WC_Rise_down_XX, Motor[3].WC_Turn_Left_Right_YY, Motor[3].WC_Hz_Adjust);
		Motor_Control_State=Motor_Control_State+1;
		break;
	case 5:
		Tis_Velocity_control(5,Motor[4].Target_Velcity);
		if(Motor_Control_Motor_Index >=Num_Motor)
		{
			Motor_Control_Motor_Index=0;
		}
		Motor_Control_State=0;

		break;
	default:
		break;
	}
}



}


