/*TI5_Motor_MOdel_70_90_Pro_81,减速比为81，One_circle_resolution 5308416
 * Robot_Size[]; Robot_Size[0] Left wheel and right Wheel distance,  158mm，wheel width 46mm,total wheel width 250mm ; Robot_Size[1] wheel Radius 180mm ;
 *电机1，前轮电机（转向电机下）；电机2,后轮电机；电机3,转向电机。
 * BHBF_ROBOT.c
 *
 *  Created on: Oct 26, 2023
 *      Author: shiya
 */
#include "BHBF_ROBOT.h"

struct CV_struct_define CV;
struct GV_struct_define GV;
struct PV_struct_define PV;

void GF_MainLoop_Jump(int32_t Next_Index_U,int32_t Next_Index_N);

void Mobile_Robot_Manual_Con();

void Robot_hult_Fuction();
int32_t Speed_Zero_Falg=0;

double Angle_Threshold=0.17453;//10°误差就关枪
//double Path_1_End=40/180*3.1415926;
double Path_1_End=97.183068/180*3.1415926;

double Path_2_End=-14.457436/180*3.1415926;

void CV_GV_Init()
{
	CV.Speed_1 = 1;
//	GV.Motor_P[0].MotorID=1;
//	GV.Motor_P[1].MotorID=2;
//	GV.Motor_P[2].MotorID=3;
}

void GF_Robot_Init()
{
	uint8_t _state=1;
	memset(&GV, 0, sizeof(struct GV_struct_define));
	memset(&PV, 0, sizeof(struct PV_struct_define));

	CV_GV_Init();
	_state = _state & GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM1_100ms_PeriodElapsedCallback,GF_WatchDog_Loop);
	_state = _state & GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback,GF_Robot_MainLoop);
	_state = _state & GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback,GF_MSP_OBD_Run_Timer);
	_state = _state & GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback,GF_MSP_Elevated_Timer);

	//由钛虎电机//锋华电机

	_state = _state & GF_BSP_GPIO_Init();
	_state = _state & GF_BSP_FDCAN_Init();
	_state = _state & GF_BSP_EEPROM_Init();
	_state = _state & GF_BSP_UART_Init();
	_state = _state & GF_MSP_Elevated_Vehicle_Init(1, (Elevated_Vhicle_Prarameters*)&GV.Robot_Openration_Mode);
	_state = _state & GF_MSP_Elite_Init(&huart1 , (Robot_Joint_Angle*)&GV.Robot_Angle_Pi);

	_state = _state & GF_MSP_OBD_Init(2);
	_state = _state & GF_BSP_TIMER_Init(); //定时器最后启动
	if(_state == 1)//APP启动成功
	{
		GF_BSP_FDCAN_Senddata(2, 0x1FF, 5, (uint8_t*)GV.NC_ARRAY_u8_0);
	}
}


int32_t Bat_falg=5;
int32_t Bat_falg_Last=1;
//100ms执行一次
void GF_WatchDog_Loop()
{
//	int number=10;

	//硬件看门狗复位
	HAL_GPIO_TogglePin(RST_WDI_GPIO_Port, RST_WDI_Pin);//Hardware Watchdog,kicking the dog

	//SSTEST
	//GF_BSP_FDCAN_Senddata(2, 0x123, 1, (uint8_t*)GV.NC_ARRAY_u8_0);
//	GF_MSP_Elite_Inquiry(GV.Robot_Openration_Mode.Robot_Mode);

}

void GF_MainLoop_Jump(int32_t Next_Index_U,int32_t Next_Index_N)
{
	PV.State_Index_U_Last=GV.State_Index_U;
	PV.State_Index_N_Last=GV.State_Index_N;
	GV.State_Index_U=Next_Index_U;
	GV.State_Index_N=Next_Index_N;

	if(PV.State_Index_U_Last != GV.State_Index_U)
	{
		GV.State_Index_U_Elapse=0;
		GV.State_Index_N_Elapse=0;
		Speed_Zero_Falg=0;
	}
	else if(PV.State_Index_N_Last != GV.State_Index_N)
	{
		GV.State_Index_N_Elapse=0;
		Speed_Zero_Falg=0;
	}
}

//2ms 执行一次
//GV.State_Main_Loop_Primary=-1 错误状态
//-1-0 机器人初始步骤，瞬态，初始化机器人，停止
//-1-1 机器人初始完成了，常态
//
//0-0 机器人初始步骤，瞬态，初始化机器人，停止
//0-1 机器人初始完成了，常态
//
//1-0 手动控制模式初始化，瞬态
//1-1 手动控制模式，常态，根据遥控器控制电机
//
//
int Cont_Flag=0;
int Cont_Flag_1=1;


int Path_First_End=1;
int Path_Sencond_End=2;

int Close_Gun_Flag=0;

int Turn_On_Count=0;
int Turn_On_Delay=20000/2;
int Robot_Program_Flag_1=1;
int Robot_Program_Flag_2=1;
void GF_Robot_MainLoop()
{
	switch(GV.Robot_Openration_Mode.Robot_Mode)
	{
	case 1: case 2: case 3:case 4:case 5: case 6:case 7: case 8:case 9: case 10:case 13:
		GF_MSP_Elite_Inquiry(GV.Robot_Openration_Mode.Robot_Mode);
		GV.Robot_Openration_Mode.Robot_Mode=0;
		Robot_Program_Flag_1=1;
		Robot_Program_Flag_2=1;

	break;
	case 11:
		Robot_Program_1();
		Robot_Program_Flag_2=1;
		break;
	case 12:
		Robot_Program_2();
		Robot_Program_Flag_1=1;
		break;
	default:
		GV.Robot_Openration_Mode.Robot_Mode=0;
		Robot_Program_Flag_1=1;
		Robot_Program_Flag_2=1;
		Cont_Flag=0;
	break;
	}

	if(Turn_On_Count<Turn_On_Delay)
	{
		Turn_On_Count++;
		GV.Robot_Openration_Mode.Device_Status=0;

	}
	else
	{
		GV.Robot_Openration_Mode.Device_Status=1;
		GV.Robot_Openration_Mode.Ranging_Sensor_Data[0]=0;
		GV.Robot_Openration_Mode.Ranging_Sensor_Data[1]=0;
		GV.Robot_Openration_Mode.Ranging_Sensor_Data[2]=0;
		GV.Robot_Openration_Mode.Ranging_Sensor_Data[3]=0;
	}
}

double Angle_Difference=0;

void Robot_Program_1()
{
	switch(Robot_Program_Flag_1)
	{
	case 1: //case 2:
		GF_MSP_Elite_Inquiry(GV.Robot_Openration_Mode.Robot_Mode);
		Robot_Program_Flag_1++;
	break;
	case 20://机械臂开动之后40ms后，打开
		HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, GPIO_PIN_RESET);
		Robot_Program_Flag_1++;
		break;
	case 50://40ms查询一次
		GF_MSP_Elite_Joint_Position_Inquiry();
		Robot_Program_Flag_1=30;
		break;
	case 100: case 109:
		GF_MSP_Elite_Inquiry(13);
		Robot_Program_Flag_1++;
		break;

	case 101:case 102:case 103:case 104:case 105:case 106:case 107:case 108:
		Robot_Program_Flag_1++;
		break;
	case 110:
		break;
	default:
		Robot_Program_Flag_1++;

		if(GV.Robot_Angle_Pi.Joint_Angle[0]>Path_1_End)
		{
			HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, GPIO_PIN_SET);
			Robot_Program_Flag_1=100;
		}
		break;
	}
}


void Robot_Program_2()
{
	switch(Robot_Program_Flag_2)
	{
	case 1: //case 2:
		GF_MSP_Elite_Inquiry(GV.Robot_Openration_Mode.Robot_Mode);
		Robot_Program_Flag_2++;
	break;
	case 20://机械臂开动之后40ms后，打开
		HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, GPIO_PIN_RESET);
		Robot_Program_Flag_2++;
		break;
	case 50://40ms查询一次
		GF_MSP_Elite_Joint_Position_Inquiry();
		Robot_Program_Flag_2=30;
		break;
	case 100: case 109:
		GF_MSP_Elite_Inquiry(13);
		Robot_Program_Flag_2++;
		break;
	case 101:case 102:case 103:case 104:case 105:case 106:case 107:case 108:
		Robot_Program_Flag_2++;
		break;
	case 110:
		break;
	default:
		Robot_Program_Flag_2++;
		if(GV.Robot_Angle_Pi.Joint_Angle[0]<Path_2_End)
		{
			HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, GPIO_PIN_SET);
			Robot_Program_Flag_2=100;
		}

	break;
	}
}




















