/*
 * 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,double 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);

	int16_t value = 0;
//	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;

		value = (int16_t)round(Back_Distance_CM * 10);
		secondDigit = value % 10; // 87 % 10 = 7  将87拆分成8和7
		value /= 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;
	}

//	int Speed_Flag=10;//开放式喷砂
	int Speed_Flag=1;//回收式喷砂
	if(((CAN_Buf[1])&(0X80))>0)
	{
		//DH_Handset_40022->Swing_Speed=1;
		//DH_Handset_40022->Swing_Speed=6;
		DH_Handset_40022->Swing_Speed=5*1*Speed_Flag;

	}
	else if(((CAN_Buf[2])&(0X01))>0)
	{
//		DH_Handset_40022->Swing_Speed=2;
		//DH_Handset_40022->Swing_Speed=8;
		DH_Handset_40022->Swing_Speed=10*1*Speed_Flag;
	}
	else if(((CAN_Buf[2])&(0X02))>0)
	{
//		DH_Handset_40022->Swing_Speed=3;
		//DH_Handset_40022->Swing_Speed=10;
		DH_Handset_40022->Swing_Speed=15*1*Speed_Flag;

	}
	else if(((CAN_Buf[2])&(0X04))>0)
	{
		//DH_Handset_40022->Swing_Speed=4;
		//DH_Handset_40022->Swing_Speed=12;
		DH_Handset_40022->Swing_Speed=20*1*Speed_Flag;
	}
	else if(((CAN_Buf[2])&(0X08))>0)
	{
//		DH_Handset_40022->Swing_Speed=5;
		//DH_Handset_40022->Swing_Speed=14;
		DH_Handset_40022->Swing_Speed=25*1*Speed_Flag;
	}
	else if(((CAN_Buf[2])&(0X10))>0)
	{
		//DH_Handset_40022->Swing_Speed=6;
		//DH_Handset_40022->Swing_Speed=16;
		DH_Handset_40022->Swing_Speed=30*1*Speed_Flag;
	}
	else if(((CAN_Buf[2])&(0X20))>0)
	{
		//DH_Handset_40022->Swing_Speed=7;
		//DH_Handset_40022->Swing_Speed=18;
		DH_Handset_40022->Swing_Speed=35*1;

	}
	else if(((CAN_Buf[2])&(0X40))>0)
	{
	//	DH_Handset_40022->Swing_Speed=8;
		//DH_Handset_40022->Swing_Speed=20;

		DH_Handset_40022->Swing_Speed=40*1;
	}
	else if(((CAN_Buf[2])&(0X80))>0)
	{
		//DH_Handset_40022->Swing_Speed=9;
		//DH_Handset_40022->Swing_Speed=22;
		DH_Handset_40022->Swing_Speed=45*1;
	}
	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;
	}
}
