/*
 * msp_TT_Motor.c
 *
 *  Created on: 2023骞�12��12��
 *      Author: Administrator
 */
//#include "BHBF_ROBOT.h"
#include "MSP/msp_TTMotor_ZQ.h"


uint8_t  TT_Motor_FDCAN_CH;
uint8_t TT_Motor_Control_State;
int32_t TT_Motor_Index;
TT_MotorParameters*  TT_Motor;
int32_t TT_Num_Motor=0;

int32_t Num_Motor_TT=0;
uint16_t ID_Sent_Offset_TT=0X600;
int32_t TT_ID_Recv_Offset=0X580;


void TT_FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data);/*///FrameID, int Length_d,uint8_t *Txdata  //CAN data transmission function*/
void TT_Current_control(uint32_t Motor_ID_1,  int T_Motor_Current_1);
void TT_Velocity_control(uint32_t Motor_ID_1,  int T_Motor_Velocity_1);
void TT_Position_control(uint32_t Motor_ID_1);
void intToByte_B_FH(int Motor_Velocity,uint8_t *Vel_date);
void Enter_CAN_operation();
void TT_Request_Read_Position(uint32_t Motor_ID_1);
void Request_Read_Velocity(uint32_t Motor_ID_1);
void Request_Read_Current(uint32_t Motor_ID_1);
void GF_MSP_TT_Motor_CANRx_Callback();
void Motor_Fault_Read(uint32_t Motor_ID_1);
void ByteToint_B_FH(uint8_t Vel_buff[],int *Num_data);
void Fault_Anal(uint16_t ID_Value,uint8_t Data_Value[]);
void TT_Motor_Node_SET_1(uint32_t Motor_ID_1);
void TT_Motor_Node_SET_2(uint32_t Motor_ID_1);
void TT_Position_Mode(uint32_t Motor_ID_1);
void TT_Velocity_Mode(uint32_t Motor_ID_1);
void TT_Motor_PowerON(uint32_t Motor_ID_1);
void TT_Motor_PowerOFF(uint32_t Motor_ID_1);
void TT_Motor_Error_Clean(uint32_t Motor_ID_1);
void TT_Motor_Servo_Prepare(uint32_t Motor_ID_1);
void TT_Wait_Poweron(uint32_t Motor_ID_1);
void TT_Poweron(uint32_t Motor_ID_1);
void TT_Servo_Begin(uint32_t Motor_ID_1);
void TT_Analytic_Fun();
void TT_Position_Set(uint32_t Motor_ID_1,  int T_Motor_Position_1);
void TT_Velocity_Set(uint32_t Motor_ID_1,  int T_Motor_Velocity_1);
void GF_MSP_TT_Activation_Pos(int32_t Motor_ID_1_AC);
void TT_Poweron_Promptly(uint32_t Motor_ID_1);
void TT_Servo_Begin_Promptly(uint32_t Motor_ID_1);
void TT_Request_Read_Velocity(uint32_t Motor_ID_1);
int Pos_Flag_Now=1;
void TT_Velocity_Set_Speed(uint32_t Motor_ID_1);
void TT_Servo_Begin_S(uint32_t Motor_ID_1);
void GF_MSP_TT_Activation_Speed(int32_t Motor_ID_1_AC);
void TT_Position_control_Now(uint32_t Motor_ID_1);
void TT_Request_Read_Current(uint32_t Motor_ID_1);
void TT_Request_Read_Fault(uint32_t Motor_ID_1);

int32_t TT_ID_Start;
uint8_t GF_MSP_TT_Motor_Init(uint8_t FDCAN_CH, TT_MotorParameters* MotorP,uint8_t ID_Start,uint8_t Num_Motor_p)
{
	TT_Motor_FDCAN_CH=FDCAN_CH;
	TT_Motor_Control_State=0;
	TT_Motor_Index=ID_Start;
	TT_Motor = MotorP;
	TT_Num_Motor = Num_Motor_p;
	TT_ID_Start=ID_Start;
	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_FDCAN1_RxFifo0Callback, GF_MSP_TT_Motor_CANRx_Callback);
	Enter_CAN_operation();
	return 1;
}

int Activa_Flag=0;
void GF_MSP_TT_Activation_Pos(int32_t Motor_ID_1_AC)
{
	uint8_t Pos_can_buff_Motor_TT[8];
	int Length_date_1=8;
	int Position_Order=0;
	int CASE_0;

	switch(Activa_Flag)
	{
	case 0: //涓��垫�娲�
		Pos_can_buff_Motor_TT[0]=0x10;
		Pos_can_buff_Motor_TT[1]=0x10;
		Length_date_1=2;
		GF_BSP_FDCAN_Senddata(TT_Motor_FDCAN_CH,Motor_ID_1_AC,Length_date_1,Pos_can_buff_Motor_TT);

		Activa_Flag++;
		break;
	case 1000:
		Activa_Flag=0;
		CASE_0=TT_Motor[Motor_ID_1_AC].Act_Suc;
		if(CASE_0==1)
		{
			Activa_Flag=1001;
		}
		else
		{
			Activa_Flag=0;
		}
		break;

	case 1001://璁剧疆杞�寤���搴�妯″�
		Pos_can_buff_Motor_TT[0]=0x2F;
		Pos_can_buff_Motor_TT[1]=0x60;
		Pos_can_buff_Motor_TT[2]=0x60;
		Pos_can_buff_Motor_TT[3]=0x00;

		Pos_can_buff_Motor_TT[4]=0x01;
		Pos_can_buff_Motor_TT[5]=0x00;
		Pos_can_buff_Motor_TT[6]=0x00;
		Pos_can_buff_Motor_TT[7]=0x00;
		Length_date_1=8;

		TT_FDCANfunction(Motor_ID_1_AC, Length_date_1,Pos_can_buff_Motor_TT);
		Activa_Flag++;
		break;

	case 2000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Cont_Posi_Suc;
		if(CASE_0==1)
		{
			Activa_Flag=2001;
		}
		else
		{
			Activa_Flag=1001;
		}
		break;
	case 2001://璁剧疆�������堕��

		Pos_can_buff_Motor_TT[0]=0x23;
		Pos_can_buff_Motor_TT[1]=0x83;
		Pos_can_buff_Motor_TT[2]=0x60;
		Pos_can_buff_Motor_TT[3]=0x00;

		Pos_can_buff_Motor_TT[4]=0x64;

		Pos_can_buff_Motor_TT[5]=0x00;
		Pos_can_buff_Motor_TT[6]=0x00;

//		Pos_can_buff_Motor_TT[5]=0xE8;
//		Pos_can_buff_Motor_TT[6]=0x03;
		Pos_can_buff_Motor_TT[7]=0x00;

		Length_date_1=8;

		TT_FDCANfunction(Motor_ID_1_AC, Length_date_1,Pos_can_buff_Motor_TT);

		Activa_Flag++;
		break;
	case 3000:

		CASE_0=TT_Motor[Motor_ID_1_AC].Acc_Suc;
		if(CASE_0==1)
		{
			Activa_Flag=3001;
		}
		else
		{
			Activa_Flag=2001;
		}
		break;

	case 3001://璁剧疆�����堕��
		Pos_can_buff_Motor_TT[0]=0x23;
		Pos_can_buff_Motor_TT[1]=0x84;
		Pos_can_buff_Motor_TT[2]=0x60;
		Pos_can_buff_Motor_TT[3]=0x00;

		Pos_can_buff_Motor_TT[4]=0x64;

		Pos_can_buff_Motor_TT[5]=0x00;
		Pos_can_buff_Motor_TT[6]=0x00;

//		Pos_can_buff_Motor_TT[5]=0xE8;
//		Pos_can_buff_Motor_TT[6]=0x03;
		Pos_can_buff_Motor_TT[7]=0x00;

		Length_date_1=8;

		TT_FDCANfunction(Motor_ID_1_AC, Length_date_1,Pos_can_buff_Motor_TT);
		Activa_Flag++;
		break;
	case 4000:
		 CASE_0=TT_Motor[Motor_ID_1_AC].Dec_Suc;

		if(CASE_0==1)
		{
			Activa_Flag=4001;
		}
		else
		{
			Activa_Flag=3001;
		}
		break;

	case 4001://璁剧疆����浣�缃�

		TT_Position_Set(Motor_ID_1_AC,  Position_Order);
		Activa_Flag++;
		break;
	case 5000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Target_Posi_Suc;

		if(CASE_0==1)
		{
			Activa_Flag=5001;
		}
		else
		{
			Activa_Flag=4001;
		}
		break;
	case 5001://璁剧疆������搴�
		TT_Velocity_Set(Motor_ID_1_AC,  0);

		Activa_Flag++;
		break;
	case 6000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Run_Speed_Suc;

		if(CASE_0==1)
		{
			Activa_Flag=6001;
		}
		else
		{
			Activa_Flag=5001;
		}
		break;
	case 6001://娓��ゅ�甯�

		TT_Motor_Error_Clean(Motor_ID_1_AC);

		Activa_Flag++;
		break;
	case 7000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Clear_Suc;

		if(CASE_0==1)
		{
			Activa_Flag=7001;
		}
		else
		{
			Activa_Flag=6001;
		}

		TT_Motor[Motor_ID_1_AC].Clear_Suc=0;
		break;
	case 7001://浼烘����澶�

		TT_Motor_Servo_Prepare(Motor_ID_1_AC);

		Activa_Flag++;
		break;
	case 8000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Clear_Suc;

		if(CASE_0==1)
		{
			Activa_Flag=8001;
		}
		else
		{
			Activa_Flag=7001;
		}
		TT_Motor[Motor_ID_1_AC].Clear_Suc=0;
		break;
	case 8001://娓��ゅ�甯�

		TT_Wait_Poweron(Motor_ID_1_AC);
		Activa_Flag++;

		break;
	case 9000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Clear_Suc;

		if(CASE_0==1)
		{
			Activa_Flag=9001;
		}
		else
		{
			Activa_Flag=8001;
		}

		TT_Motor[Motor_ID_1_AC].Clear_Suc=0;
		break;
	case 9001://浼烘����澶�

		TT_Poweron(Motor_ID_1_AC);
		Activa_Flag++;
		break;
	case 10000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Clear_Suc;

		if(CASE_0==1)
		{
			Activa_Flag=11001;
		}
		else
		{
			Activa_Flag=9001;
		}
		TT_Motor[Motor_ID_1_AC].Clear_Suc=0;
		break;
	case 10001://寮���浼烘��
		TT_Servo_Begin(Motor_ID_1_AC);

		Activa_Flag++;
		break;
	case 11000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Clear_Suc;
		if(CASE_0==1)
		{
			Activa_Flag++;
		}
		else
		{
			Activa_Flag=10001;
		}
		TT_Motor[Motor_ID_1_AC].Clear_Suc=0;
		break;
	default:
		Activa_Flag++;
		break;
	}
}


void GF_MSP_TT_Motor_CANRx_Callback()
{
	int32_t ID_Value=(int32_t)TT_ID_Recv_Offset+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();
	}
	else
	{
	}
}



void TT_Motor_Error_Clean(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]=0x80;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void TT_Motor_Servo_Prepare(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]=0x06;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void TT_Wait_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]=0x07;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void TT_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;

	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void TT_Poweron_Promptly(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]=0x2F;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


void TT_Servo_Begin_Promptly(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]=0x3F;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void TT_Servo_Begin(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]=0x1F;
	Pos_can_buff_Motor_To[5]=0x00;
	Pos_can_buff_Motor_To[6]=0x00;
	Pos_can_buff_Motor_To[7]=0x00;

	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void TT_Servo_Begin_S(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;

	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


uint32_t Node_Offset_TT=700;
void TT_Motor_Node_Prot(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[0];
	int Length_date_1=0;
	TT_FDCANfunction(Motor_ID_1+Node_Offset_TT, Length_date_1,Pos_can_buff_Motor_To);
}

void TT_Motor_Node_SET_1(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]=0x0C;
	Pos_can_buff_Motor_To[2]=0x10;
	Pos_can_buff_Motor_To[3]=0x00;

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

	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

void TT_Motor_Node_SET_2(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0x2F;
	Pos_can_buff_Motor_To[1]=0x0D;
	Pos_can_buff_Motor_To[2]=0x10;
	Pos_can_buff_Motor_To[3]=0x00;

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

	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}

int TT_Motor_Init_Flag=0;  int TT_Motor_Order=1;




void TT_Analytic_Fun()
{
	int32_t ID_A_T_A;
	ID_A_T_A=CAN_ID-TT_ID_Recv_Offset;
	int  Function_code=(int)((int16_t)(CAN_Buf[1]|CAN_Buf[2]<<8));
	switch(Function_code)
	{
		case 24672:
			TT_Motor[ID_A_T_A].Cont_Posi_Suc=1;
			break;
		case 24707:
			TT_Motor[ID_A_T_A].Acc_Suc=1;
			break;
		case 24708:
			TT_Motor[ID_A_T_A].Dec_Suc=1;
			break;
		case 24698:
			TT_Motor[ID_A_T_A].Target_Posi_Suc=1;
			break;
		case 24705:
			TT_Motor[ID_A_T_A].Run_Speed_Suc=1;
			break;
		case 24640:
			TT_Motor[ID_A_T_A].Clear_Suc=1;
			break;
		case 8220:
			TT_Motor[ID_A_T_A].Acc_Suc_Speed=1;
		break;
		case 24831:
			TT_Motor[ID_A_T_A].Suc_Speed_S=1;
			break;
		case 8221:
			TT_Motor[ID_A_T_A].Dec_Suc_Speed=1;
		break;
		case 24676:
			TT_Motor[ID_A_T_A].Real_Position=(int)(CAN_Buf[4] | (CAN_Buf[5])<<8| (CAN_Buf[6])<<16| (CAN_Buf[7]<<24));
			break;
		case 24684:
			TT_Motor[ID_A_T_A].Real_Velcity=(int)(CAN_Buf[4] | (CAN_Buf[5])<<8| (CAN_Buf[6])<<16| (CAN_Buf[7]<<24));
			break;

		case 24696:
			TT_Motor[ID_A_T_A].Real_Current=(int)(CAN_Buf[4] | (CAN_Buf[5])<<8| (CAN_Buf[6])<<16| (CAN_Buf[7]<<24));//0.1A
			break;
		case 12298:
			TT_Motor[ID_A_T_A].TT_Motor_Fault=(int)(CAN_Buf[4] | (CAN_Buf[5])<<8| (CAN_Buf[6])<<16| (CAN_Buf[7]<<24));//0.1A
			break;

		default:
			if((CAN_Buf[0]==(0XAA))|(CAN_Buf[1]==(0XBB)))
			{
				TT_Motor[ID_A_T_A].Act_Suc=1;
			}
			break;
	}
}


void TT_FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data)
{
	ID=ID+ID_Sent_Offset_TT;
	GF_BSP_FDCAN_Senddata(TT_Motor_FDCAN_CH,ID,Length,Data);
}


int Pos_Flag=0;

void GF_MSP_TT_Motor_Control_Timer()
{
	int32_t Motor_Run_Mode=TT_Motor[TT_Motor_Index].Run_Mode;
	int32_t Motor_ID=TT_Motor[TT_Motor_Index].MotorID;

	int All_motor;
	All_motor=(int)Num_Motor_TT+(int)TT_ID_Start;

	switch(TT_Motor_Control_State)
	{
	case 0:
		GF_MSP_TT_Activation_Speed(Motor_ID);
		if(Activa_Flag>11001)
		{
			TT_Motor_Control_State=TT_Motor_Control_State+1;
		}
		break;

	case 1:
		TT_Motor_Index++;
		if(TT_Motor_Index >=All_motor)
		{
			TT_Motor_Control_State=TT_Motor_Control_State+1;
			TT_Motor_Index=TT_ID_Start;
		}
		else
		{
			TT_Motor_Control_State=0;
		}
		break;

	case 2:
		switch( Motor_Run_Mode )  //
		{
		case DF_MSP_TT_Motor_CurrentMode://
			//TT_Current_control(Motor_ID,TT_Motor[TT_Motor_Control_Motor_Index].Target_Current);
			Pos_Flag_Now=0;
			break;
		case DF_MSP_TT_Motor_SpeedMode://
			//TT_Velocity_control(Motor_ID,TT_Motor[TT_Motor_Control_Motor_Index].Target_Velcity);

			TT_Velocity_Set_Speed(Motor_ID);

			Pos_Flag_Now=0;
			TT_Motor_Control_State=TT_Motor_Control_State+1;
			break;
		case DF_MSP_TT_Motor_PositionMode://
			TT_Position_control(Motor_ID);
//			if(Pos_Flag>3)
//			{
//
//			}
			TT_Motor_Control_State=TT_Motor_Control_State+1;
			Pos_Flag_Now=0;
			break;

		case DF_MSP_TT_Motor_Posi_Now://
			TT_Position_control_Now(Motor_ID);
			//TT_Motor_Control_State=TT_Motor_Control_State+1;
			break;

		default:
			TT_Motor_Control_State=TT_Motor_Control_State+1;
			break;
		}
		break;

	case 3: //璇锋�璇诲���垫�轰�缃�
		TT_Request_Read_Position(Motor_ID);
		TT_Motor_Control_State=4;
		break;
//	case 4:
//		TT_Request_Read_Velocity(Motor_ID);
//		TT_Motor_Control_State=5;
//		break;
	case 4:
		TT_Request_Read_Fault(Motor_ID);
		TT_Motor_Control_State=5;
		break;
	case 5:
		TT_Request_Read_Current(Motor_ID);
		TT_Motor_Index++;
		if(TT_Motor_Index >=All_motor)
		{
			TT_Motor_Index=TT_ID_Start;
		}
		TT_Motor_Control_State=2;
		break;

	}
}

void TT_Velocity_Set(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]=0x81;
	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];
	}
	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}





void TT_Position_control(uint32_t Motor_ID_1)
{
	int Position_1=(int)TT_Motor[TT_Motor_Index].Target_Position;

	int Speed_1=(int)TT_Motor[ TT_Motor_Index].Target_Velcity;

	int CASE_1;

	switch(Pos_Flag)
	{
	case 0:
		TT_Position_Set(Motor_ID_1,  Position_1);
		CASE_1=TT_Motor[TT_Motor_Index].Target_Posi_Suc;
		if(CASE_1==1)
		{
			Pos_Flag=1;
		}
		break;
	case 1:
		TT_Velocity_Set(Motor_ID_1,  Speed_1);
		CASE_1=TT_Motor[TT_Motor_Index].Run_Speed_Suc;
		if(CASE_1==1)
		{
			Pos_Flag=2;
		}
		break;
	case 2:
		TT_Poweron(Motor_ID_1);
		CASE_1=TT_Motor[TT_Motor_Index].Clear_Suc;

		if(CASE_1==1)
		{
			Pos_Flag=3;
		}
		TT_Motor[TT_Motor_Index].Clear_Suc=0;
		break;
	case 3:
		TT_Servo_Begin(Motor_ID_1);
		CASE_1=TT_Motor[TT_Motor_Index].Clear_Suc;
		if(CASE_1==1)
		{
			Pos_Flag=4;
		}
		TT_Motor[TT_Motor_Index].Clear_Suc=0;
		break;
	default:
		Pos_Flag=0;
		break;
	}
}




void TT_Position_control_Now(uint32_t Motor_ID_1)
{
	int Position_1=(int)TT_Motor[TT_Motor_Index].Target_Position;

	int Speed_1=(int)TT_Motor[TT_Motor_Index].Target_Velcity;

	int CASE_1;

	switch(Pos_Flag_Now)
	{
	case 0:
		TT_Position_Set(Motor_ID_1,  Position_1);
		CASE_1=TT_Motor[TT_Motor_Index].Target_Posi_Suc;
		if(CASE_1==1)
		{
			Pos_Flag_Now=1;
		}
		break;
	case 1:
		TT_Velocity_Set(Motor_ID_1,  Speed_1);
		CASE_1=TT_Motor[TT_Motor_Index].Run_Speed_Suc;
		if(CASE_1==1)
		{
			Pos_Flag_Now=2;
		}
		Pos_Flag_Now=2;
		break;
	case 2:
		TT_Poweron_Promptly(Motor_ID_1);
		CASE_1=TT_Motor[TT_Motor_Index].Clear_Suc;

		if(CASE_1==1)
		{
			Pos_Flag_Now=3;
		}
		TT_Motor[TT_Motor_Index].Clear_Suc=0;
		Pos_Flag_Now=3;
		break;
	case 3:
		TT_Servo_Begin_Promptly(Motor_ID_1);
		CASE_1=TT_Motor[TT_Motor_Index].Clear_Suc;
		if(CASE_1==1)
		{
			Pos_Flag_Now=4;
		}
		TT_Motor[TT_Motor_Index].Clear_Suc=0;
		Pos_Flag_Now=4;
		break;
	default:
		Pos_Flag_Now=0;
		break;
	}
}














void TT_Position_Set(uint32_t Motor_ID_1,  int T_Motor_Position_2)
{
	uint8_t Pos_can_buff_Motor1[4];uint8_t Pos_can_buff_Motor_To[8];
	intToByte_B_FH(T_Motor_Position_2, 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];
	}
	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


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


void TT_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]=0x6C;
	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;
	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


void TT_Request_Read_Fault(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]=0x0A;
	Pos_can_buff_Motor_To[2]=0x30;
	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;
	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}


void TT_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]=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;
	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}




void GF_MSP_TT_Activation_Speed(int32_t Motor_ID_1_AC)
{
	uint8_t Pos_can_buff_Motor_TT[8];
	int Length_date_1=8;

	int CASE_0;

	switch(Activa_Flag)
	{
	case 0: //涓��垫�娲�
		Pos_can_buff_Motor_TT[0]=0x10;
		Pos_can_buff_Motor_TT[1]=0x10;
		Length_date_1=2;
		GF_BSP_FDCAN_Senddata(TT_Motor_FDCAN_CH,Motor_ID_1_AC,Length_date_1,Pos_can_buff_Motor_TT);
		Activa_Flag++;
		break;
	case 1000:
		Activa_Flag=0;
		CASE_0=TT_Motor[Motor_ID_1_AC].Act_Suc;
		if(CASE_0==1)
		{
			Activa_Flag=1001;
		}
		else
		{
			Activa_Flag=0;
		}
		break;
	case 1001://璁剧疆杞�寤���搴�妯″�
		Pos_can_buff_Motor_TT[0]=0x2F;
		Pos_can_buff_Motor_TT[1]=0x60;
		Pos_can_buff_Motor_TT[2]=0x60;
		Pos_can_buff_Motor_TT[3]=0x00;

		Pos_can_buff_Motor_TT[4]=0x03;
		Pos_can_buff_Motor_TT[5]=0x00;
		Pos_can_buff_Motor_TT[6]=0x00;
		Pos_can_buff_Motor_TT[7]=0x00;
		Length_date_1=8;

		TT_FDCANfunction(Motor_ID_1_AC, Length_date_1,Pos_can_buff_Motor_TT);
		Activa_Flag++;
		break;

	case 2000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Cont_Posi_Suc;
		if(CASE_0==1)
		{
			Activa_Flag=2001;
		}
		else
		{
			Activa_Flag=1001;
		}
		break;

	case 2001://娓�妤�寮�甯�
		TT_Motor_Error_Clean(Motor_ID_1_AC);
		Activa_Flag++;
		break;
	case 3000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Clear_Suc;

		if(CASE_0==1)
		{
			Activa_Flag=3001;
		}
		else
		{
			Activa_Flag=2001;
		}

		TT_Motor[Motor_ID_1_AC].Clear_Suc=0;
		break;


	case 3001://浼烘����澶�

		TT_Motor_Servo_Prepare(Motor_ID_1_AC);

		Activa_Flag++;
		break;
	case 4000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Clear_Suc;

		if(CASE_0==1)
		{
			Activa_Flag=4001;
		}
		else
		{
			Activa_Flag=3001;
		}
		TT_Motor[Motor_ID_1_AC].Clear_Suc=0;
		break;
	case 5001://娓��ゅ�甯�

		TT_Wait_Poweron(Motor_ID_1_AC);
		Activa_Flag++;

		break;
	case 6000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Clear_Suc;

		if(CASE_0==1)
		{
			Activa_Flag=6001;
		}
		else
		{
			Activa_Flag=5001;
		}

		TT_Motor[Motor_ID_1_AC].Clear_Suc=0;
		break;
	case 6001://浼烘����澶�

		TT_Poweron(Motor_ID_1_AC);
		Activa_Flag++;
		break;
	case 7000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Clear_Suc;

		if(CASE_0==1)
		{
			Activa_Flag=7001;
		}
		else
		{
			Activa_Flag=6001;
		}
		TT_Motor[Motor_ID_1_AC].Clear_Suc=0;
		break;
	case 7001://寮���浼烘��

		TT_Servo_Begin_S(Motor_ID_1_AC);
		Activa_Flag++;
		break;
	case 8000:
		CASE_0=TT_Motor[Motor_ID_1_AC].Clear_Suc;
		if(CASE_0==1)
		{
			Activa_Flag=8001;
		}
		else
		{
			Activa_Flag=7001;
		}
		TT_Motor[Motor_ID_1_AC].Clear_Suc=0;
		break;
	case 8001: //璁剧疆�����堕��
		Pos_can_buff_Motor_TT[0]=0x23;
		Pos_can_buff_Motor_TT[1]=0x1C;
		Pos_can_buff_Motor_TT[2]=0x20;
		Pos_can_buff_Motor_TT[3]=0x00;

		Pos_can_buff_Motor_TT[4]=0xF4;
		Pos_can_buff_Motor_TT[5]=0x01;
		Pos_can_buff_Motor_TT[6]=0x00;
		Pos_can_buff_Motor_TT[7]=0x00;

		Length_date_1=8;
		TT_FDCANfunction(Motor_ID_1_AC, Length_date_1,Pos_can_buff_Motor_TT);
		Activa_Flag++;
		break;
	case 9000:

		CASE_0=TT_Motor[Motor_ID_1_AC].Acc_Suc_Speed;
		if(CASE_0==1)
		{
			Activa_Flag=9001;
		}
		else
		{
			Activa_Flag=8001;
		}
		break;

	case 9001://璁剧疆�����堕��
		Pos_can_buff_Motor_TT[0]=0x23;
		Pos_can_buff_Motor_TT[1]=0x1D;
		Pos_can_buff_Motor_TT[2]=0x20;
		Pos_can_buff_Motor_TT[3]=0x00;

		Pos_can_buff_Motor_TT[4]=0xF4;
		Pos_can_buff_Motor_TT[5]=0x01;
		Pos_can_buff_Motor_TT[6]=0x00;
		Pos_can_buff_Motor_TT[7]=0x00;

		Length_date_1=8;

		TT_FDCANfunction(Motor_ID_1_AC, Length_date_1,Pos_can_buff_Motor_TT);
		Activa_Flag++;
		break;
	case 10000:
		 CASE_0=TT_Motor[Motor_ID_1_AC].Dec_Suc_Speed;

		if(CASE_0==1)
		{
			Activa_Flag=10001;
		}
		else
		{
			Activa_Flag=9001;
		}
		break;
	case 10001:
		TT_Motor[Motor_ID_1_AC].Target_Velcity=0;
		TT_Velocity_Set_Speed(Motor_ID_1_AC);
		Activa_Flag++;
		break;

	case 11001:
		CASE_0=TT_Motor[Motor_ID_1_AC].Suc_Speed_S;
		if(CASE_0==1)
		{
			Activa_Flag++;
		}
		else
		{
			Activa_Flag=10001;
		}
		break;

	default:
		Activa_Flag++;
		break;
	}
}







void TT_Velocity_Set_Speed(uint32_t Motor_ID_1)
{
	int Speed_1=(int)TT_Motor[Motor_ID_1].Target_Velcity;

	uint8_t Pos_can_buff_Motor1[4];uint8_t Pos_can_buff_Motor_To[8];
	intToByte_B_FH(Speed_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];
	}
	TT_FDCANfunction(Motor_ID_1, Length_date_1,Pos_can_buff_Motor_To);
}



















