//
//
//
//
//
//
//// * msp_DH_W_R_40022.c
//// *
//// *  Created on: 2023年12月12日
//// *      Author: Administrator
//
////#include "BHBF_ROBOT.h"
//#include "MSP/msp_DH_W_R_40022_CAN.h"
//
//uint8_t  DC_FDCAN_CH;
//SP_MSP_DH_CAN_Parameters* DH_Handset_40022;
//void GF_MSP_DH_40022_CANRx_Callback();
//void DC_Decoding_Func_1();
//void DC_Decoding_Func_2();
//void DH_40022_FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data);
//int Vertical_Error_Con=50;
//
//uint8_t GF_MSP_DH_CAN_Init(uint8_t FDCAN_CH, SP_MSP_DH_CAN_Parameters* MotorP)
//{
//	DC_FDCAN_CH=FDCAN_CH;
//	DH_Handset_40022 = MotorP;
//	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_FDCAN1_RxFifo0Callback, GF_MSP_DH_40022_CANRx_Callback);
//	return 1;
//}
//
//void GF_MSP_DH_40022_CANRx_Callback()
//{
//	int DH_40022_Flag=(int)CAN_ID;
//	switch(DH_40022_Flag)
//	{
//	case 484:
//		DC_Decoding_Func_1();
//		break;
//	case 740:
//		DC_Decoding_Func_2();
//		break;
//	default:
//		break;
//	}
//}
//
//
//int DH402_CAN_Inquiry=0;
//void  GF_MSP_DH_40022_CAN_Inquiry(double Arm_Radius,int Back_Distance_CM, int Downforce)
//{
//	int32_t Motor_ID_1_AC;
//	uint8_t Pos_can_buff_Motor_TT[8];
//	int Length_date_1=8;
//    double Value=3*(double)DH_Handset_40022->Swing_Speed*Arm_Radius*2*3.1415926/6;
//
//
//    int firstDigit, secondDigit;
//
//	int Swi_Speed_AAA=(int)Value;
//	int Swi_Angle_AAA=(int)DH_Handset_40022->Swing_Angle*100;
//	int Trav_Speed_AAA=(int)(DH_Handset_40022->Car_Speed);
////	int Downforce;
//	int Left_Comp_AAA=(int)(((double)DH_Handset_40022->Left_Comp)/3.5);
//	int Right_Comp_AAA=(int)(((double)DH_Handset_40022->Right_Comp)/3.5);
//
//
////	switch(Status_Flag)
////	{
////	case 0:
////		break;
////	case 1:
////		break;
////	case 2:
////		break;
////	case 3:
////		break;
////	case 5:
////		break;
////	case 6:
////		break;
////	}
//
//
//	switch(DH402_CAN_Inquiry)
//	{
//	case 0:
//		Pos_can_buff_Motor_TT[0]=(uint8_t)( Swi_Speed_AAA);
//		Pos_can_buff_Motor_TT[1]=(uint8_t)( Swi_Speed_AAA>>8);
//		Pos_can_buff_Motor_TT[2]=(uint8_t)( Swi_Angle_AAA);
//		Pos_can_buff_Motor_TT[3]=(uint8_t)( Swi_Angle_AAA>>8);
//
//		Pos_can_buff_Motor_TT[4]=(uint8_t)( Trav_Speed_AAA);
//		Pos_can_buff_Motor_TT[5]=(uint8_t)( Trav_Speed_AAA>>8);
//		Pos_can_buff_Motor_TT[6]=(uint8_t)( Downforce);
//		Pos_can_buff_Motor_TT[7]=(uint8_t)( Downforce>>8);
//
//		Motor_ID_1_AC=0X264;
//		DH_40022_FDCANfunction(Motor_ID_1_AC, Length_date_1,Pos_can_buff_Motor_TT);
//		DH402_CAN_Inquiry++;
//		break;
//
//	case 1:
//		Pos_can_buff_Motor_TT[0]=(uint8_t)( Left_Comp_AAA);
//		Pos_can_buff_Motor_TT[1]=(uint8_t)( Left_Comp_AAA>>8);
//		Pos_can_buff_Motor_TT[2]=(uint8_t)( Right_Comp_AAA);
//		Pos_can_buff_Motor_TT[3]=(uint8_t)( Right_Comp_AAA>>8);
//
//
//		Pos_can_buff_Motor_TT[4]=0XBA;
//		Pos_can_buff_Motor_TT[5]=0XF3;
//		Pos_can_buff_Motor_TT[6]=0XCD;
//		Pos_can_buff_Motor_TT[7]=0XCB;
//
//
//		Motor_ID_1_AC=0X364;
//		DH_40022_FDCANfunction(Motor_ID_1_AC, Length_date_1,Pos_can_buff_Motor_TT);
//		DH402_CAN_Inquiry++;
//		break;
//	case 2:
//		Pos_can_buff_Motor_TT[0]=0XBE;
//		Pos_can_buff_Motor_TT[1]=0XE0;
//		Pos_can_buff_Motor_TT[2]=0XC0;
//		Pos_can_buff_Motor_TT[3]=0XEB;
//
//		secondDigit = Back_Distance_CM % 10; // 87 % 10 = 7  将87拆分成8和7
//		Back_Distance_CM /= 10;
//		firstDigit = Back_Distance_CM;
//		Pos_can_buff_Motor_TT[4]=0X30+(uint8_t)firstDigit ;
//		Pos_can_buff_Motor_TT[5]=0X30+(uint8_t)secondDigit ;
//		Pos_can_buff_Motor_TT[6]=0XC0;
//		Pos_can_buff_Motor_TT[7]=0XE5;
//		Motor_ID_1_AC=0X464;
//		DH_40022_FDCANfunction(Motor_ID_1_AC, Length_date_1,Pos_can_buff_Motor_TT);
//		DH402_CAN_Inquiry++;
//		break;
//	case 3:
//		Pos_can_buff_Motor_TT[0]=0XC3;
//		Pos_can_buff_Motor_TT[1]=0XD7;
//
//		Pos_can_buff_Motor_TT[2]=0X00;
//		Pos_can_buff_Motor_TT[3]=0X00;
//
//
//		Pos_can_buff_Motor_TT[4]=0X00;
//		Pos_can_buff_Motor_TT[5]=0X00;
//		Pos_can_buff_Motor_TT[6]=0X00;
//		Pos_can_buff_Motor_TT[7]=0X00;
//		Motor_ID_1_AC=0X564;
//		DH_40022_FDCANfunction(Motor_ID_1_AC, Length_date_1,Pos_can_buff_Motor_TT);
//		DH402_CAN_Inquiry++;
//		break;
//
//
//	default:
//		DH402_CAN_Inquiry=0;
//		break;
//	}
//
//
//}
//
//
//void DH_40022_FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data)
//{
//	GF_BSP_FDCAN_Senddata(DC_FDCAN_CH,ID,Length,Data);
//}
//
//
//void DC_Decoding_Func_1()
//{
//	DH_Handset_40022->For_Back_Value_X=(int32_t)CAN_Buf[0]-127;
//	DH_Handset_40022->Left_Right_Value_Y=(int32_t)CAN_Buf[1]-127;
//	DH_Handset_40022->Left_Comp=(int32_t)CAN_Buf[2];
//	DH_Handset_40022->Right_Comp=(int32_t)CAN_Buf[3];
//}
//
//void DC_Decoding_Func_2()
//{
//	if(((CAN_Buf[0])&(0X01))>0)
//	{
//	DH_Handset_40022->EMERGENCY_STOP=1;
//	}
//	else
//	{
//	DH_Handset_40022->EMERGENCY_STOP=0;
//	}
//
//	if(((CAN_Buf[0])&(0X20))>0)
//	{
//		DH_Handset_40022->Car_Speed=1;
//	}
//	else if(((CAN_Buf[0])&(0X40))>0)
//	{
//		DH_Handset_40022->Car_Speed=2;
//	}
//	else if(((CAN_Buf[0])&(0X80))>0)
//	{
//		DH_Handset_40022->Car_Speed=3;
//	}
//	else if(((CAN_Buf[1])&(0X01))>0)
//	{
//		DH_Handset_40022->Car_Speed=4;
//	}
//	else if(((CAN_Buf[1])&(0X02))>0)
//	{
//		DH_Handset_40022->Car_Speed=5;
//	}
//	else if(((CAN_Buf[1])&(0X04))>0)
//	{
//		DH_Handset_40022->Car_Speed=6;
//	}
//	else if(((CAN_Buf[1])&(0X08))>0)
//	{
//		DH_Handset_40022->Car_Speed=7;
//	}
//	else if(((CAN_Buf[1])&(0X10))>0)
//	{
//		DH_Handset_40022->Car_Speed=8;
//	}
//	else if(((CAN_Buf[1])&(0X20))>0)
//	{
//		DH_Handset_40022->Car_Speed=10;
//	}
//	else if(((CAN_Buf[1])&(0X40))>0)
//	{
//		DH_Handset_40022->Car_Speed=14;
//	}
//	else
//	{
//		DH_Handset_40022->Car_Speed=0;
//	}
//
//	if(((CAN_Buf[1])&(0X80))>0)
//	{
//		//DH_Handset_40022->Swing_Speed=1;
//		DH_Handset_40022->Swing_Speed=8;
//	}
//	else if(((CAN_Buf[2])&(0X01))>0)
//	{
//		//DH_Handset_40022->Swing_Speed=2;
//		DH_Handset_40022->Swing_Speed=10;
//	}
//	else if(((CAN_Buf[2])&(0X02))>0)
//	{
//		//DH_Handset_40022->Swing_Speed=3;
//		DH_Handset_40022->Swing_Speed=12;
//	}
//	else if(((CAN_Buf[2])&(0X04))>0)
//	{
////		DH_Handset_40022->Swing_Speed=4;
//		DH_Handset_40022->Swing_Speed=14;
//	}
//	else if(((CAN_Buf[2])&(0X08))>0)
//	{
//		//DH_Handset_40022->Swing_Speed=5;
//		DH_Handset_40022->Swing_Speed=16;
//	}
//	else if(((CAN_Buf[2])&(0X10))>0)
//	{
//		//DH_Handset_40022->Swing_Speed=6;
//		DH_Handset_40022->Swing_Speed=18;
//	}
//	else if(((CAN_Buf[2])&(0X20))>0)
//	{
//		//DH_Handset_40022->Swing_Speed=7;
//		DH_Handset_40022->Swing_Speed=20;
//	}
//	else if(((CAN_Buf[2])&(0X40))>0)
//	{
//		//DH_Handset_40022->Swing_Speed=8;
//		DH_Handset_40022->Swing_Speed=22;
//	}
//	else if(((CAN_Buf[2])&(0X80))>0)
//	{
//		//DH_Handset_40022->Swing_Speed=9;
//		DH_Handset_40022->Swing_Speed=24;
//	}
//	else if(((CAN_Buf[3])&(0X01))>0)
//	{
//		//DH_Handset_40022->Swing_Speed=10;
//		DH_Handset_40022->Swing_Speed=11;
//	}
//	else
//	{
//		DH_Handset_40022->Swing_Speed=0;
//	}
//
//	if(((CAN_Buf[3])&(0X02))>0)
//	{
//		//DH_Handset_40022->Swing_Angle=1;
//		DH_Handset_40022->Swing_Angle=10;
//		DH_Handset_40022->Turn_Road_Back_Dis=1359;
//
//		DH_Handset_40022->Vertic_Turn_Road_Back_Dis=1975-Vertical_Error_Con;
//	}
//	else if(((CAN_Buf[3])&(0X04))>0)
//	{
//		//DH_Handset_40022->Swing_Angle=2;
//		DH_Handset_40022->Swing_Angle=15;
//		DH_Handset_40022->Turn_Road_Back_Dis=2036;
//
//		DH_Handset_40022->Vertic_Turn_Road_Back_Dis=2629-Vertical_Error_Con;
//	}
//	else if(((CAN_Buf[3])&(0X08))>0)
//	{
//		//DH_Handset_40022->Swing_Angle=3;
//		DH_Handset_40022->Swing_Angle=20;
//		DH_Handset_40022->Turn_Road_Back_Dis=2709;
//
//		DH_Handset_40022->Vertic_Turn_Road_Back_Dis=3268-Vertical_Error_Con;
//	}
//	else if(((CAN_Buf[3])&(0X10))>0)
//	{
//		//DH_Handset_40022->Swing_Angle=4;
//		DH_Handset_40022->Swing_Angle=25;
//		DH_Handset_40022->Turn_Road_Back_Dis=3376;
//
//		DH_Handset_40022->Vertic_Turn_Road_Back_Dis=3909-Vertical_Error_Con;
//	}
//	else if(((CAN_Buf[3])&(0X20))>0)
//	{
//		//DH_Handset_40022->Swing_Angle=5;
//		DH_Handset_40022->Swing_Angle=30;
//		DH_Handset_40022->Turn_Road_Back_Dis=4038;
//
//		DH_Handset_40022->Vertic_Turn_Road_Back_Dis=4566-Vertical_Error_Con;
//	}
//	else if(((CAN_Buf[3])&(0X40))>0)
//	{
//		//DH_Handset_40022->Swing_Angle=6;
//		DH_Handset_40022->Swing_Angle=40;
//		DH_Handset_40022->Turn_Road_Back_Dis=5336;
//
//		DH_Handset_40022->Vertic_Turn_Road_Back_Dis=5739-Vertical_Error_Con;
//	}
//	else if(((CAN_Buf[3])&(0X80))>0)
//	{
//		//DH_Handset_40022->Swing_Angle=7;
//		DH_Handset_40022->Swing_Angle=60;
//		DH_Handset_40022->Turn_Road_Back_Dis=7800;
//
//
//		DH_Handset_40022->Vertic_Turn_Road_Back_Dis=8197-Vertical_Error_Con;
//	}
//	else if(((CAN_Buf[4])&(0X01))>0)
//	{
//		//DH_Handset_40022->Swing_Angle=8;
//		DH_Handset_40022->Swing_Angle=90;
//		DH_Handset_40022->Turn_Road_Back_Dis=11030;
//
//
//		DH_Handset_40022->Vertic_Turn_Road_Back_Dis=11353-Vertical_Error_Con;
//	}
//	else if(((CAN_Buf[4])&(0X02))>0)
//	{
//		///DH_Handset_40022->Swing_Angle=9;
//		DH_Handset_40022->Swing_Angle=120;
//		DH_Handset_40022->Turn_Road_Back_Dis=13510;
//
//
//		DH_Handset_40022->Vertic_Turn_Road_Back_Dis=13777-Vertical_Error_Con;
//	}
//	else if(((CAN_Buf[4])&(0X04))>0)
//	{
//		//DH_Handset_40022->Swing_Angle=10;
//		DH_Handset_40022->Swing_Angle=150;
//		DH_Handset_40022->Turn_Road_Back_Dis=15068;
//
//		DH_Handset_40022->Vertic_Turn_Road_Back_Dis=15209-Vertical_Error_Con;
//	}
//	else
//	{
//		DH_Handset_40022->Swing_Angle=0;
//	}
//
//	if(((CAN_Buf[4])&(0X08))>0)
//	{
//		DH_Handset_40022->Vert2_Horiz1=1;
//	}
//	else if(((CAN_Buf[4])&(0X10))>0)
//	{
//		DH_Handset_40022->Vert2_Horiz1=2;
//	}
//	else
//	{
//		DH_Handset_40022->Vert2_Horiz1=0;
//	}
//
//
//     if(((CAN_Buf[4])&(0X20))>0)
//	{
//		DH_Handset_40022->Left_Up1_Right_Down2=1;
//	}
//	else if(((CAN_Buf[4])&(0X40))>0)
//	{
//		DH_Handset_40022->Left_Up1_Right_Down2=2;
//	}
//	else
//	{
//		DH_Handset_40022->Left_Up1_Right_Down2=0;
//	}
//
//     if(((CAN_Buf[4])&(0X80))>0)
//	{
//		DH_Handset_40022->Tast_Init1_Close2=1;
//	}
//    else
//    {
//    	DH_Handset_40022->Tast_Init1_Close2=2;
//    }
//
//     if(((CAN_Buf[5])&(0X02))>0)
//	{
//		DH_Handset_40022->UP1=1;
//	}
//     else if(((CAN_Buf[5])&(0X04))>0)
//	{
//		DH_Handset_40022->DOWN2=1;
//	}
//    else if(((CAN_Buf[5])&(0X08))>0)
//	{
//		DH_Handset_40022->LEFT3=1;
//	}
//    else if(((CAN_Buf[5])&(0X10))>0)
//	{
//		DH_Handset_40022->RIGHT4=1;
//	}
//    else
//	{
//    	DH_Handset_40022->UP1=0;
//    	DH_Handset_40022->DOWN2=0;
//    	DH_Handset_40022->LEFT3=0;
//    	DH_Handset_40022->RIGHT4=0;
//	}
//}
//
//
//
//
//
