#include "MSP/msp_LeiSaiCan_Motor.h"
#include<stdbool.h>

SP_MSP_LeiSaiCan_MotorParameters* Can_LeiSai;
extern uint8_t DO1_ON[8];
extern uint8_t DO1_OFF[8];

extern bool isOpen;
extern bool isIn ;
int b=0;
uint8_t  LeiSai_FDCAN_CH;//雷赛can通道
uint32_t  ID0=0x0000;
uint16_t  ID1=0x601;
uint16_t  ID2=0x602;
uint8_t   LSCan_state=0;
uint8_t   Rflag=0;//右轮停车标志

uint32_t GF_MSP_LeiSaiCan_Motor_Init(uint8_t FDCAN_CH, SP_MSP_LeiSaiCan_MotorParameters *_Bat_LeiSai)
{
	LeiSai_FDCAN_CH=FDCAN_CH;
	Can_LeiSai=_Bat_LeiSai;
	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_FDCAN2_RxFifo0Callback, GF_LeiSaiCan_Unscramble_Callback);
	return 1;
}

void  GF_LeiSaiCan_Unscramble_Callback()
{
	uint8_t ID_A_T;
	int Function_code;
	uint8_t Motor_Data[4];
	int Buff_Value_int;
	int32_t Error_Vlaue;
	ID_A_T=CAN_ID;
	//581//582
	if(ID_A_T>=581+ID1 && ID_A_T<580+ID2)
	{
		Function_code=(int)(CAN_Buf[1]|(CAN_Buf[2])<<8);
		for (int i=1;i<4;i++)
		{
			Motor_Data[i-1]=CAN_Buf[i];
		}
		switch(Function_code)
		{
		case 24639:
			Error_Vlaue=(int)(CAN_Buf[4]|(CAN_Buf[5])<<8);
			Can_LeiSai[ID1].Error_Flag = Error_Vlaue;
			break;
		default:
			break;
		}
	}
}

void LeisaiCan_FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data)
{
	GF_BSP_FDCAN_Senddata(LeiSai_FDCAN_CH,ID,Length,Data);
}

uint8_t Order[8];
uint8_t Order2[8];
void GF_MSP_LeisaiCan_Motor_Control_Timer()
{
	int32_t Speed_Value=0;
	//每（2）ms发一条指令，这个逻辑可以自己带入switch串一遍，便于理解
	switch(LSCan_state)
	{
	case 0://复位所有节点。
		Order[0]=0X81;
		Order[1]=0X00;
		Order[2]=0X00;
		Order[3]=0X00;
		Order[4]=0X00;
		Order[5]=0X00;
		Order[6]=0x00;
		Order[7]=0x00;
		LeisaiCan_FDCANfunction(ID0,8,Order);//发送指令 ID0

		LSCan_state++;
		break;
	case 1://启动所有节点的远程控制
		Order[0]=0x01;
		Order[1]=0X00;
		Order[2]=0X00;
		Order[3]=0X00;
		Order[4]=0X00;
		Order[5]=0X00;
		Order[6]=0x00;
		Order[7]=0x00;

		LeisaiCan_FDCANfunction(ID0,8,Order);//发送指令ID0
		LSCan_state++;
		break;
	case 2:     //左轮写控制字为 06H，
		Order[0]=0X2b;
		Order[1]=0X40;
		Order[2]=0X60;
		Order[3]=0X00;
		Order[4]=0X06;
		Order[5]=0X00;
		Order[6]=0x00;
		Order[7]=0x00;

		LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
		LSCan_state++;
		break;
	case 3:
		//左轮写控制字为 07H，
		Order[0]=0X2b;
		Order[1]=0X40;
		Order[2]=0X60;
		Order[3]=0X00;
		Order[4]=0X07;
		Order[5]=0X00;
		Order[6]=0x00;
		Order[7]=0x00;

		LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
		LSCan_state++;
		break;
	case 4:
		//左轮写控制字为 0fH，电机使能
		Order[0]=0X2b;
		Order[1]=0X40;
		Order[2]=0X60;
		Order[3]=0X00;
		Order[4]=0X0f;
		Order[5]=0X00;
		Order[6]=0x00;
		Order[7]=0x00;

		LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
		LSCan_state++;
		break;
	case 5:     //右轮写控制字为 06H，
		Order2[0]=0X2b;
		Order2[1]=0X40;
		Order2[2]=0X60;
		Order2[3]=0X00;
		Order2[4]=0X06;
		Order2[5]=0X00;
		Order2[6]=0x00;
		Order2[7]=0x00;

		LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
		LSCan_state++;
		break;
	case 6:
		//右轮写控制字为 07H，
		Order2[0]=0X2b;
		Order2[1]=0X40;
		Order2[2]=0X60;
		Order2[3]=0X00;
		Order2[4]=0X07;
		Order2[5]=0X00;
		Order2[6]=0x00;
		Order2[7]=0x00;

		LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
		LSCan_state++;
		break;
	case 7:
		//右轮写控制字为 0fH，电机使能
		Order2[0]=0X2b;
		Order2[1]=0X40;
		Order2[2]=0X60;
		Order2[3]=0X00;
		Order2[4]=0X0f;
		Order2[5]=0X00;
		Order2[6]=0x00;
		Order2[7]=0x00;

		LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
		LSCan_state++;
		break;
	case 8:
	  //左轮速度模式
		Order[0]=0X2f;
		Order[1]=0X60;
		Order[2]=0X60;
		Order[3]=0X00;
		Order[4]=0X03;
		Order[5]=0X00;
		Order[6]=0x00;
		Order[7]=0x00;

		LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
		LSCan_state++;
		break;
	case 9:
	  //右轮速度模式
		Order2[0]=0X2f;
		Order2[1]=0X60;
		Order2[2]=0X60;
		Order2[3]=0X00;
		Order2[4]=0X03;
		Order2[5]=0X00;
		Order2[6]=0x00;
		Order2[7]=0x00;

		LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
		LSCan_state++;
		break;
	case 10:
	  //左轮加速度
		Order[0]=0X23;
		Order[1]=0X83;
		Order[2]=0X60;
		Order[3]=0X00;
		Order[4]=0X60;
		Order[5]=0XE3;
		Order[6]=0x16;
		Order[7]=0x00;

		//LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
		LSCan_state++;
		break;
	case 11:
	  //右轮加速度 111ms/1000rpm
		Order2[0]=0X23;
		Order2[1]=0X83;
		Order2[2]=0X60;
		Order2[3]=0X00;
		Order2[4]=0X60;
		Order2[5]=0XE3;
		Order2[6]=0x16;
		Order2[7]=0x00;

		//LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
		LSCan_state++;
		break;
	case 12:
	  //左轮减速度
		Order[0]=0X23;
		Order[1]=0X84;
		Order[2]=0X60;
		Order[3]=0X00;
		Order[4]=0X60;
		Order[5]=0XE3;
		Order[6]=0x16;
		Order[7]=0x00;

		//LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
		LSCan_state++;
		break;
	case 13:
	  //右轮减速度 111ms/1000rpm
		Order2[0]=0X23;
		Order2[1]=0X84;
		Order2[2]=0X60;
		Order2[3]=0X00;
		Order2[4]=0X60;
		Order2[5]=0XE3;
		Order2[6]=0x16;
		Order2[7]=0x00;

		//LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
		LSCan_state++;
		break;
	case 14:
	  //左轮速度
		Order[0]=0X23;
		Order[1]=0Xff;
		Order[2]=0X60;
		Order[3]=0X00;

		Speed_Value=Can_LeiSai[0].Target_Velcity;
		Order[4]=(uint8_t)Speed_Value;
		Order[5]=(uint8_t)(Speed_Value>>8);
		Order[6]=(uint8_t)(Speed_Value>>16);
		Order[7]=(uint8_t)(Speed_Value>>24);
		LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
		LSCan_state++;
		break;
	case 15:
	  //右轮速度
		Order2[0]=0X23;
		Order2[1]=0Xff;
		Order2[2]=0X60;
		Order2[3]=0X00;

		Speed_Value=Can_LeiSai[1].Target_Velcity;
		Order2[4]=(uint8_t)Speed_Value;
		Order2[5]=(uint8_t)(Speed_Value>>8);
		Order2[6]=(uint8_t)(Speed_Value>>16);
		Order2[7]=(uint8_t)(Speed_Value>>24);
		LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2

		if((LSCan_state%10)!=0)
		{
			LSCan_state=14;
		}
		else
		{
			LSCan_state++;
		}
		break;
	case 16:
		Order2[0]=0X40;
		Order2[1]=0X3f;
		Order2[2]=0X60;
		Order2[3]=0X00;

		Order2[4]=0;
		Order2[5]=0;
		Order2[6]=0;
		Order2[7]=0;
		LSCan_state++;
		LeisaiCan_FDCANfunction(ID1,8,Order2);//发送指令ID2
		break;
	case 17:
		Order2[0]=0X40;
		Order2[1]=0X3f;
		Order2[2]=0X60;
		Order2[3]=0X00;

		Order2[4]=0;
		Order2[5]=0;
		Order2[6]=0;
		Order2[7]=0;

		LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
		LSCan_state=14;
		break;
	default:
		break;
	}
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//#include "MSP/msp_LeiSaiCan_Motor.h"
//
//
//
//#include<stdbool.h>
//
//SP_MSP_LeiSaiCan_MotorParameters* Can_LeiSai;
//extern uint8_t DO1_ON[8];
//extern uint8_t DO1_OFF[8];
//
//extern bool isOpen;
//extern bool isIn ;
//int b=0;
//uint8_t  LeiSai_FDCAN_CH;//雷赛can通道
//uint32_t  ID0=0x0000;
//
//uint16_t  ID1=0x601;
//uint16_t  ID2=0x602;
//uint8_t   LSCan_state=0;
//uint8_t   Rflag=0;//右轮停车标志
//
//uint32_t GF_MSP_LeiSaiCan_Motor_Init(uint8_t FDCAN_CH, SP_MSP_LeiSaiCan_MotorParameters *_Bat_LeiSai)
//{
//	LeiSai_FDCAN_CH=FDCAN_CH;
//	Can_LeiSai=_Bat_LeiSai;
//	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_FDCAN2_RxFifo0Callback, GF_LeiSaiCan_Unscramble_Callback);
//	return 1;
//}
//
//
//void  GF_LeiSaiCan_Unscramble_Callback()
//{
//
//}
//
//void LeisaiCan_FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data)
//{
//	GF_BSP_FDCAN_Senddata(LeiSai_FDCAN_CH,ID,Length,Data);
//}
//
//uint8_t Order[8];
//uint8_t Order2[8];
//void GF_MSP_LeisaiCan_Motor_Control_Timer()
//{
//
//
//
//	int32_t Speed_Value=0;
//	switch( LSCan_state)                  //每（2）ms发一条指令，这个逻辑可以自己带入switch串一遍，便于理解
//	{
//			case 0:
//					      //复位所有节点。
//						Order[0]=0X81;
//						Order[1]=0X00;
//						Order[2]=0X00;
//						Order[3]=0X00;
//						Order[4]=0X00;
//						Order[5]=0X00;
//						Order[6]=0x00;
//						Order[7]=0x00;
//						LeisaiCan_FDCANfunction(ID0,8,Order);//发送指令 ID0
//
//						LSCan_state++;
//						break;
//			case 1: //  //启动所有节点的远程控制
//						Order[0]=0x01;
//						Order[1]=0X00;
//						Order[2]=0X00;
//						Order[3]=0X00;
//						Order[4]=0X00;
//						Order[5]=0X00;
//						Order[6]=0x00;
//						Order[7]=0x00;
//						LeisaiCan_FDCANfunction(ID0,8,Order);//发送指令ID0
//						LSCan_state++;
//						break;
//			case 2:     //左轮写控制字为 06H，
//						Order[0]=0X2b;
//						Order[1]=0X40;
//						Order[2]=0X60;
//						Order[3]=0X00;
//						Order[4]=0X06;
//						Order[5]=0X00;
//						Order[6]=0x00;
//						Order[7]=0x00;
//
//						LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
//						LSCan_state++;
//						break;
//
//			case 3:
//				 	 	//左轮写控制字为 07H，
//						Order[0]=0X2b;
//						Order[1]=0X40;
//						Order[2]=0X60;
//						Order[3]=0X00;
//						Order[4]=0X07;
//						Order[5]=0X00;
//						Order[6]=0x00;
//						Order[7]=0x00;
//
//						LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
//						LSCan_state++;
//						break;
//		   case 4:
//						//左轮写控制字为 0fH，电机使能
//						Order[0]=0X2b;
//						Order[1]=0X40;
//						Order[2]=0X60;
//						Order[3]=0X00;
//						Order[4]=0X0f;
//						Order[5]=0X00;
//						Order[6]=0x00;
//						Order[7]=0x00;
//
//						LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
//						LSCan_state++;
//						break;
//
//			case 5:     //右轮写控制字为 06H，
//						Order2[0]=0X2b;
//						Order2[1]=0X40;
//						Order2[2]=0X60;
//						Order2[3]=0X00;
//						Order2[4]=0X06;
//						Order2[5]=0X00;
//						Order2[6]=0x00;
//						Order2[7]=0x00;
//
//						LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
//						LSCan_state++;
//						break;
//
//			case 6:
//				 	 	//右轮写控制字为 07H，
//						Order2[0]=0X2b;
//						Order2[1]=0X40;
//						Order2[2]=0X60;
//						Order2[3]=0X00;
//						Order2[4]=0X07;
//						Order2[5]=0X00;
//						Order2[6]=0x00;
//						Order2[7]=0x00;
//
//						LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
//						LSCan_state++;
//						break;
//		   case 7:
//						//右轮写控制字为 0fH，电机使能
//						Order2[0]=0X2b;
//						Order2[1]=0X40;
//						Order2[2]=0X60;
//						Order2[3]=0X00;
//						Order2[4]=0X0f;
//						Order2[5]=0X00;
//						Order2[6]=0x00;
//						Order2[7]=0x00;
//
//						LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
//						LSCan_state++;
//						break;
//
//			case 8:
//				      //左轮速度模式
//						Order[0]=0X2f;
//						Order[1]=0X60;
//						Order[2]=0X60;
//						Order[3]=0X00;
//						Order[4]=0X03;
//						Order[5]=0X00;
//						Order[6]=0x00;
//						Order[7]=0x00;
//
//						LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
//						LSCan_state++;
//				        break;
//			case 9:
//				      //右轮速度模式
//						Order2[0]=0X2f;
//						Order2[1]=0X60;
//						Order2[2]=0X60;
//						Order2[3]=0X00;
//						Order2[4]=0X03;
//						Order2[5]=0X00;
//						Order2[6]=0x00;
//						Order2[7]=0x00;
//
//						LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
//						LSCan_state++;
//				        break;
//			case 10:
//				      //左轮加速度
//						Order[0]=0X23;
//						Order[1]=0X83;
//						Order[2]=0X60;
//						Order[3]=0X00;
//						Order[4]=0X60;
//						Order[5]=0XE3;
//						Order[6]=0x16;
//						Order[7]=0x00;
//
//						//LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
//						LSCan_state++;
//				        break;
//			case 11:
//				      //右轮加速度 111ms/1000rpm
//						Order2[0]=0X23;
//						Order2[1]=0X83;
//						Order2[2]=0X60;
//						Order2[3]=0X00;
//						Order2[4]=0X60;
//						Order2[5]=0XE3;
//						Order2[6]=0x16;
//						Order2[7]=0x00;
//
//						//LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
//						LSCan_state++;
//				        break;
//			case 12:
//				      //左轮减速度
//						Order[0]=0X23;
//						Order[1]=0X84;
//						Order[2]=0X60;
//						Order[3]=0X00;
//						Order[4]=0X60;
//						Order[5]=0XE3;
//						Order[6]=0x16;
//						Order[7]=0x00;
//
//						//LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
//						LSCan_state++;
//				        break;
//			case 13:
//				      //右轮减速度 111ms/1000rpm
//						Order2[0]=0X23;
//						Order2[1]=0X84;
//						Order2[2]=0X60;
//						Order2[3]=0X00;
//						Order2[4]=0X60;
//						Order2[5]=0XE3;
//						Order2[6]=0x16;
//						Order2[7]=0x00;
//
//						//LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
//						LSCan_state++;
//				        break;
//			case 14:
//				      //左轮速度
//						Order[0]=0X23;
//						Order[1]=0Xff;
//						Order[2]=0X60;
//						Order[3]=0X00;
//
//						Speed_Value=Can_LeiSai[0].Target_Velcity;
//
//
//						Order[4]=(uint8_t)Speed_Value;
//						Order[5]=(uint8_t)(Speed_Value>>8);
//						Order[6]=(uint8_t)(Speed_Value>>16);
//						Order[7]=(uint8_t)(Speed_Value>>24);
//						LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
//
////						if(Speed_Value==0)//速度为0时，停车时
////						{
////							if(isOpen) //喷枪开了
////							{
////								b++;
////								//LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);//关
////								if(b>=16)//值*6为延迟时间220
////								{
////									isOpen = false;//判断作业开启时的是否开关枪，防止作业关闭时无法测试开关枪
////									Order[4]= 0;           // (uint8_t)Speed_Value;
////									Order[5]= 0 ;          // (uint8_t)(Speed_Value>>8);
////									Order[6]= 0 ;           //(uint8_t)(Speed_Value>>16);
////									Order[7]= 0 ;          //(uint8_t)(Speed_Value>>24);
////									b=0;
////									Rflag=1;
////								}
////							}
////							else if(!isOpen)//喷枪没开
////							{
////									Order[4]=(uint8_t)0;
////									Order[5]=(uint8_t)(0);
////									Order[6]=(uint8_t)(0);
////									Order[7]=(uint8_t)(0);
////									Rflag=1;
////							}
////						}
////
////						else //速度不为0时
////						{
////							Order[4]=(uint8_t)Speed_Value;
////							Order[5]=(uint8_t)(Speed_Value>>8);
////							Order[6]=(uint8_t)(Speed_Value>>16);
////							Order[7]=(uint8_t)(Speed_Value>>24);
////						}
////						if(b==0)
////						{
////							LeisaiCan_FDCANfunction(ID1,8,Order);//发送指令ID1
////						}
//
//						LSCan_state++;
//				        break;
//			case 15:
//				      //右轮速度
//						Order2[0]=0X23;
//						Order2[1]=0Xff;
//						Order2[2]=0X60;
//						Order2[3]=0X00;
//
//						Speed_Value=Can_LeiSai[1].Target_Velcity;
//						Order2[4]=(uint8_t)Speed_Value;
//						Order2[5]=(uint8_t)(Speed_Value>>8);
//						Order2[6]=(uint8_t)(Speed_Value>>16);
//						Order2[7]=(uint8_t)(Speed_Value>>24);
//						LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
//
////						if(Speed_Value==0)//速度为0时
////						{
////							if(Rflag==1)//检查左轮是否停车
////							{
////								Order2[4]=  0;			//(uint8_t)Speed_Value;
////								Order2[5]=	0;		//(uint8_t)(Speed_Value>>8);
////								Order2[6]=	0;		//(uint8_t)(Speed_Value>>16);
////								Order2[7]=	0;		//(uint8_t)(Speed_Value>>24);
////								Rflag=0;
////							}
////
////						}
////						else
////						{
////							Order2[4]=(uint8_t)Speed_Value;
////							Order2[5]=(uint8_t)(Speed_Value>>8);
////							Order2[6]=(uint8_t)(Speed_Value>>16);
////							Order2[7]=(uint8_t)(Speed_Value>>24);
////						}
////
////						if(b==0)
////						{
////							LeisaiCan_FDCANfunction(ID2,8,Order2);//发送指令ID2
////						}
//
//						LSCan_state=14;
//				        break;
//			case 16:
////						if(DC_CAN_Paint->open==1)//开启上
////						{
////
////							if(DC_CAN_Paint->Open_Up==1)//作业开启，能开喷枪
////							{
////								if(!isIn)
////								{
////									LeisaiCan_FDCANfunction(ID1,8,DO1_ON);
////									isOpen = true;
////									isIn = true;
////								}
////							}
////						}
////						else if(DC_CAN_Paint->Open_Down==1)//开启下,也是开喷抢
////						{
////
////							if(DC_CAN_Paint->Open_Up==1)//作业开启，开喷枪
////							{
////
////								if(isIn)
////								{
////									LeisaiCan_FDCANfunction(ID1,8,DO1_ON);
////									isOpen = true;
////									isIn = false;
////								}
////
////							}
////						}
//						/*****************/
//
////						if(DC_CAN_Paint->test_start==1)//测试开启，在作业关闭时有用
////						{
////							if(DC_CAN_Paint->working_stop==1)//作业关闭
////							{
////
////								LeisaiCan_FDCANfunction(ID1,8,DO1_ON);
////							}
////						}
////						else if(DC_CAN_Paint->test_stop==1)//测试关闭，在作业关闭时有用
////						{
////							if(DC_CAN_Paint->working_stop==1)//作业关闭
////							{
////								LeisaiCan_FDCANfunction(ID1,8,DO1_OFF);
////							}
////						}
////						LSCan_state=14;
//						break;
//
//
//			default:
//						break;
//
//	}
//}

