/*
 * msp_DC_CAN.c
 *
 *  Created on: 2024年6月21日
 *      Author: SQ-X
 */


#include "MSP/msp_DC_CAN.h"
uint8_t  DC_FDCAN_CH;//德驰can通道
SP_MSP_DH_CAN_Parameters* DC_CAN;

void GF_MSP_DH_40022_CANRx_Callback();
void DC_Decoding_Func_2();
void DH_40022_FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data);

//const int16_t Speed[11]={0,2,5,10,15,20,22,24,26,30,30}; //m/min

const int16_t Speed[11]={0,3,6,9,12,15,18,21,24,27,30}; //m/min HJB

//从C#中拷贝过来的每个档位对应的速度大小（此处为初始值，后续可以根据手机APP来改变此值）
//轮子直径为0.942m，传动比70，500/3为(P     X=500*V/3)(V为电机的转速RPM/min)
int32_t speed_level_0 = Speed[0]/0.942*70*500/3; 	//2/0.942*70  148
int32_t speed_level_1 = Speed[1]/0.942*70*500/3; 	//2/0.942*70  148
int32_t speed_level_2 = Speed[2]/0.942*70*500/3 ;	//371
int32_t speed_level_3 = Speed[3]/0.942*70*500/3 ;	//743
int32_t speed_level_4 = Speed[4]/0.942*70*500/3 ;	//1114
int32_t speed_level_5 = Speed[5]/0.942*70*500/3 ;	//1486
int32_t speed_level_6 = Speed[6]/0.942*70*500/3 ;	//1634
int32_t speed_level_7 = Speed[7]/0.942*70*500/3 ;	//1783
int32_t speed_level_8 = Speed[8]/0.942*70*500/3 ;	//1932
int32_t speed_level_9 = Speed[9]/0.942*70*500/3 ;	//2080
int32_t speed_level_10 = Speed[10]/0.942*70*500/3 ;	//2229

int8_t Speed_increase_value = 0; //HJB


uint8_t GF_MSP_DH_CAN_Init(uint8_t FDCAN_CH, SP_MSP_DH_CAN_Parameters* MotorP)
{
	DC_FDCAN_CH=FDCAN_CH;
	DC_CAN = 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:   			//1E4
			DC_Decoding_Func_3();
			break;
		case 740:				//2E4
			DC_Decoding_Func_2();
			break;
	default:
		break;
	}
}


int DH402_CAN_Inquiry=0;
void  GF_MSP_DH_40022_CAN_Inquiry(double Gyro_angle)
{

	Gyro_angle=fabs(Gyro_angle);
	int16_t Angle_deg=(int16_t)Gyro_angle;
	int32_t Motor_ID_1_AC;
	uint8_t Pos_can_buff_Motor_TT[8];
	int Length_date_1=8;


	switch(DH402_CAN_Inquiry)
	{
	case 0:
		Pos_can_buff_Motor_TT[0]=(uint8_t)(Angle_deg);//写角度
		Pos_can_buff_Motor_TT[1]=(uint8_t)(Angle_deg>>8);
		Pos_can_buff_Motor_TT[2]=(uint8_t)(DC_CAN->Motor_Speed_Level_Meter);//写速度
		Pos_can_buff_Motor_TT[3]=(uint8_t)( 0);

		Pos_can_buff_Motor_TT[4]=(uint8_t)(DC_CAN->Change_road_dis_ten);
		Pos_can_buff_Motor_TT[5]=(uint8_t)(0);
		Pos_can_buff_Motor_TT[6]=(uint8_t)( DC_CAN->Change_road_dis_single);
		Pos_can_buff_Motor_TT[7]=(uint8_t)(0);

		Motor_ID_1_AC=0X264;
		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_3()//遥控器接收解码
{
	DC_CAN->Fore_Back_Value =  CAN_Buf[0]-0X7F;
	DC_CAN->Left_Right_Value = CAN_Buf[1]-0X7F;
}

uint8_t timedelay = 0;

void Speed_regulation_function(uint8_t num)
{
	   DC_CAN->Motor_Speed_Level_Meter = Speed[num] + Speed_increase_value;
	   DC_CAN->Motor_Speed_Level = Speed[num]/0.942*70*500/3 + Speed_increase_value/0.942*70*500/3;

	   if(DC_CAN->button_A == 1)
	   {
		   timedelay ++;
		   if(timedelay >= 2)
		   {
			   Speed_increase_value = Speed_increase_value + 1;
			   if(Speed_increase_value >= 2)
			   {
				   Speed_increase_value = 2;
			   }
			   timedelay = 0;
		   }
	   }
	   if(DC_CAN->button_B == 1)
	   {
		   timedelay ++;
		   if(timedelay >= 2)
		   {
			   Speed_increase_value = Speed_increase_value - 1;
			   if(Speed_increase_value <= 0)
			   {
				   Speed_increase_value = 0;
			   }
			   timedelay = 0;
		   }
	   }
}


void Speed_regulation_function1(uint8_t num)
{
	   DC_CAN->Motor_Speed_Level_Meter = Speed[num] + Speed_increase_value;
	   DC_CAN->Motor_Speed_Level = Speed[num]/0.942*70*500/3 + Speed_increase_value/0.942*70*500/3;

	   if(DC_CAN->button_A == 1)
	   {
		   timedelay ++;
		   if(timedelay >= 2)
		   {
			   Speed_increase_value = Speed_increase_value + 1;
			   if(Speed_increase_value >= 3)
			   {
				   Speed_increase_value = 3;
			   }
			   timedelay = 0;
		   }
	   }
	   if(DC_CAN->button_B == 1)
	   {
		   timedelay ++;
		   if(timedelay >= 2)
		   {
			   Speed_increase_value = Speed_increase_value - 1;
			   if(Speed_increase_value <= 0)
			   {
				   Speed_increase_value = 0;
			   }
			   timedelay = 0;
		   }
	   }
}

uint8_t Speed_change_flag = 0;
uint8_t Speed_change_dect = 0;
uint8_t Speed_change;
uint8_t Motor_Speed_Meter = 0;

void DC_Decoding_Func_2()//遥控器接收解码
{
	if(Speed_change_flag == 0)
	{
		Speed_change = Motor_Speed_Meter;
		Speed_change_flag = 1;
	}
	Speed_change_dect++;
	if(Speed_change_dect >= 3)
	{
		if(Speed_change != Motor_Speed_Meter)
		{
			Speed_increase_value = 0;
		}
		Speed_change_dect = 0;
		Speed_change_flag = 0;
	}

//	DC_CAN->Fore_Back_Value =  CAN_Buf[0]-0X7F-0XFF;
//	DC_CAN->Left_Right_Value = CAN_Buf[1]-0X7F-0XFF;

	if( (CAN_Buf[0]&0X01)==0 )
	{
		DC_CAN->EMERGENCY_STOP=1;
	}
	else if( (CAN_Buf[0]&0X01)==1  )
	{
		DC_CAN->EMERGENCY_STOP=0;
	}

	//前后左右位
	if(CAN_Buf[0]&0x02)    //前进被置位
   {
		DC_CAN->for_flag=1;
		DC_CAN->back_flag=0;
		DC_CAN->left_flag=0;
		DC_CAN->right_flag=0;

   }
   else if(CAN_Buf[0]&0x04)   //后退被置位
   {
	   DC_CAN->back_flag=1;
	   DC_CAN->for_flag=0;
	   DC_CAN->left_flag=0;
	   DC_CAN->right_flag=0;
   }
   else if(CAN_Buf[0]&0x08)   //左转被置位
      {
	   DC_CAN->back_flag=0;
	   DC_CAN->for_flag=0;
	   DC_CAN->left_flag=1;
	   DC_CAN->right_flag=0;
      }
   else if(CAN_Buf[0]&0x10)  //右转被置位
   {
	   DC_CAN->back_flag=0;
	   DC_CAN->for_flag=0;
	   DC_CAN->left_flag=0;
	   DC_CAN->right_flag=1;
   }
   else if(!(CAN_Buf[0]&0x1E))//没拨摇杆
   {
	   DC_CAN->back_flag=0;
	   DC_CAN->for_flag=0;
	   DC_CAN->left_flag=0;
	   DC_CAN->right_flag=0;
   }

	//速度档位
	   if(CAN_Buf[0]&0x20)
	   {
		   Speed_regulation_function(0);
		   Motor_Speed_Meter = 1;
//		   DC_CAN->Motor_Speed_Level = speed_level_0;
//		   DC_CAN->Motor_Speed_Level_Meter = Speed[0];
	   }
	   else if(CAN_Buf[0]&0x40)
	   {
		   Speed_regulation_function(1);
		   Motor_Speed_Meter = 2;
//		   DC_CAN->Motor_Speed_Level = speed_level_1;
//		   DC_CAN->Motor_Speed_Level_Meter = Speed[1];
	   }
	   else if(CAN_Buf[0]&0x80)
	   {
		   Speed_regulation_function(2);
		   Motor_Speed_Meter = 3;
//		   DC_CAN->Motor_Speed_Level = speed_level_2;
//		   DC_CAN->Motor_Speed_Level_Meter = Speed[2];
	   }
	   else if(CAN_Buf[1]&0x01)
	   {
		   Speed_regulation_function(3);
		   Motor_Speed_Meter = 4;
//		   DC_CAN->Motor_Speed_Level = speed_level_3;
//		   DC_CAN->Motor_Speed_Level_Meter = Speed[3];
	   }
	   else if(CAN_Buf[1]&0x02)
	   {
		   Speed_regulation_function(4);
		   Motor_Speed_Meter = 5;
//		   DC_CAN->Motor_Speed_Level = speed_level_4;
//		   DC_CAN->Motor_Speed_Level_Meter = Speed[4];
	   }
	   else if(CAN_Buf[1]&0x04)
	   {
		   Speed_regulation_function(5);
		   Motor_Speed_Meter = 6;
//		   DC_CAN->Motor_Speed_Level = speed_level_5;
//		   DC_CAN->Motor_Speed_Level_Meter = Speed[5];
	   }
	   else if(CAN_Buf[1]&0x08)
	   {
		   Speed_regulation_function(6);
		   Motor_Speed_Meter = 7;
//		   DC_CAN->Motor_Speed_Level = speed_level_6;
//		   DC_CAN->Motor_Speed_Level_Meter = Speed[6];
	   }
	   else if(CAN_Buf[1]&0x10)
	   {
		   Speed_regulation_function(7);
		   Motor_Speed_Meter = 8;
//		   DC_CAN->Motor_Speed_Level = speed_level_7;
//		   DC_CAN->Motor_Speed_Level_Meter = Speed[7];
	   }
	   else if(CAN_Buf[1]&0x20)
	   {
		   Speed_regulation_function(8);
		   Motor_Speed_Meter = 9;
//		   DC_CAN->Motor_Speed_Level = speed_level_8;
//		   DC_CAN->Motor_Speed_Level_Meter = Speed[8];
	   }
	   else if(CAN_Buf[1]&0x40)
	   {
		   Speed_regulation_function1(9);
		   Motor_Speed_Meter = 10;
//		   DC_CAN->Motor_Speed_Level = speed_level_9;
//		   DC_CAN->Motor_Speed_Level_Meter = Speed[9];
	   }
	   else
	   {
		   //Speed_increase_value = 0;
		   DC_CAN->Motor_Speed_Level=0;
		   DC_CAN->Motor_Speed_Level_Meter = Speed[0];
	   }
/**************************/
	     //作业开启/停止
	      if(CAN_Buf[4]&0x20)
	      {
	    	  DC_CAN->working_start1_stop2=1;

	      }
	      else if(CAN_Buf[4]&0x40)
	      {

	    	  DC_CAN->working_start1_stop2=2;
	      }
	      else
	      {
	    	  DC_CAN->working_start1_stop2=0;
	      }
	      //测试开启/关闭
	         if(CAN_Buf[4]&0x80)
	         {
	        	 DC_CAN->test_start1_stop0=1;

	         }
	         else if(CAN_Buf[5]&0x01)
	         {
	        	 DC_CAN->test_start1_stop0=0;
	         }

	      //开启上/下
	         if(CAN_Buf[5]&0x02)
	         {
	        	 DC_CAN->Open_Up1_Down2=1;
	         }
	         else if(CAN_Buf[5]&0x04)
	         {
	        	 DC_CAN->Open_Up1_Down2=2;
	         }
	         else {
	        	 DC_CAN->Open_Up1_Down2 = 0;
			}

	         if(CAN_Buf[5]&0x08)
	         {
	        	 DC_CAN->button_A = 1;
	         }
	         else if(CAN_Buf[5]&0x10)
	         {
	        	 DC_CAN->button_B = 1;
	         }
	         else {
	        	 DC_CAN->button_A = 0;
	        	 DC_CAN->button_B = 0;
			}



/*******************/
		 //竖直上/下换道
			if(CAN_Buf[4]&0x08)
			{
				DC_CAN->Vertical_change_road_up1_down2_close0=1;
			}
			else if(CAN_Buf[4]&0x10)
			{
				DC_CAN->Vertical_change_road_up1_down2_close0=2;
			}
			else if(!(CAN_Buf[4]&0x18))    //  上下都没按，此处是为了实现上下换道拨杆拨回中间位置 在上下拨动才会再次触发换道
			{
				DC_CAN->Vertical_change_road_up1_down2_close0=0;
			}

			//换道距离
			if(CAN_Buf[1]&0x80)//换道距离个位
			{
				DC_CAN->Change_road_dis_single=0;
			}
			else if(CAN_Buf[2]&0x01)
			{
				DC_CAN->Change_road_dis_single=1;
			}
			else if(CAN_Buf[2]&0x02)
			{
				DC_CAN->Change_road_dis_single=2;
			}
			else if(CAN_Buf[2]&0x04)
			{
				DC_CAN->Change_road_dis_single=3;
			}
			else if(CAN_Buf[2]&0x08)
			{
				DC_CAN->Change_road_dis_single=4;
			}
			else if(CAN_Buf[2]&0x10)
			{
				DC_CAN->Change_road_dis_single=5;
			}
			else if(CAN_Buf[2]&0x20)
			{
				DC_CAN->Change_road_dis_single=6;
			}
			else if(CAN_Buf[2]&0x40)
			{
				DC_CAN->Change_road_dis_single=7;
			}
			else if(CAN_Buf[2]&0x80)
			{
				DC_CAN->Change_road_dis_single=8;
			}
			else if(CAN_Buf[3]&0x01)
			{
				DC_CAN->Change_road_dis_single=9;
			}
			else
			{
				DC_CAN->Change_road_dis_single=0;
			}
			//换道距离
						if(CAN_Buf[3]&0x02)//换道距离十位
						{
							DC_CAN->Change_road_dis_ten=0;
						}
						else if(CAN_Buf[3]&0x04)
						{
							DC_CAN->Change_road_dis_ten=1;
						}
						else if(CAN_Buf[3]&0x08)
						{
							DC_CAN->Change_road_dis_ten=2;
						}
						else if(CAN_Buf[3]&0x10)
						{
							DC_CAN->Change_road_dis_ten=3;
						}
						else if(CAN_Buf[3]&0x20)
						{
							DC_CAN->Change_road_dis_ten=4;
						}
						else if(CAN_Buf[3]&0x40)
						{
							DC_CAN->Change_road_dis_ten=5;
						}
						else if(CAN_Buf[3]&0x80)
						{
							DC_CAN->Change_road_dis_ten=6;
						}
						else if(CAN_Buf[4]&0x01)
						{
							DC_CAN->Change_road_dis_ten=7;
						}
						else if(CAN_Buf[4]&0x02)
						{
							DC_CAN->Change_road_dis_ten=8;
						}
						else if(CAN_Buf[5]&0x04)
						{
							DC_CAN->Change_road_dis_ten=9;
						}
						else
						{
							DC_CAN->Change_road_dis_ten=0;
						}


}





