/*
 * BHBF_ROBOT.c
 *
 *  Created on: Oct 26, 2023
 *      Author: shiya
 */
#include "fsm.h"
#include "BHBF_ROBOT.h"
#include <string.h>


#include "bsp_FDCAN.h"






int32_t* SystemTimeMiliSeconds;  //似乎未用
int32_t SystemTimeMiliCount;  //2ms加一

int32_t* SystemErrorCode;
ErrorData* SystemErrorData;





//Declare a CV and initialize CV
CV_struct_define CV =
{ 0 };




//Declare a GV and initialize      CV
GV_struct_define GV =
{ 0 };

PV_struct_define decoded_PV_variable = { 0 };
PV_struct_define decoded_PV = { 0 };


IV_struct_define IV ={ 0 };



void SET_BIT_1(int32_t* num,int32_t k)
{
	*num=((*num) | (1 << (k)));
}
void SET_BIT_0(int32_t* num,int32_t k)
{
	*num=((*num) & ~(1 << (k)));
}
int32_t Get_BIT(int32_t* num,int32_t k)
{
   return (*num >> (k)) & 1;
}



void SystemTimer_Intialize()
{
	SystemTimeMiliCount=0;
	GF_BSP_Interrupt_Add_CallBack(
			DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Timer_Count);
}

void GF_Timer_Count()
{
	SystemTimeMiliCount++;
}


Sys_timer_handler timer_handler_1;
Sys_timer_handler timer_handler_2;
Sys_timer_handler timer_handler_3;
Sys_timer_handler timer_handler_4;



int sys_timer_1_flag_count[4];




bool CompareTimer(int32_t DelayMiliSeconds,Sys_timer_handler * timer_handler)
{
	if(timer_handler->start_timer==1)
	{
		timer_handler->sys_current_timer_count=DelayMiliSeconds/2+SystemTimeMiliCount;
		timer_handler->start_timer=0;
	}
	if(timer_handler->sys_current_timer_count<=SystemTimeMiliCount)
	{
		return true;
	}else
	{
		return false;
	}

}
bool CompareTimer_Delay(int32_t Delay)
{


	int32_t tickstart = SystemTimeMiliCount;
	int32_t wait = Delay;

	  /* Add a freq to guarantee minimum wait */
	  if (wait < HAL_MAX_DELAY)
	  {
		  int32_t wait = Delay;
		  while ((SystemTimeMiliCount - tickstart) < wait/2)
		 	  {
		 	  }
	  }

}
/*m/min--->0.1m/min 其实就是×10*/
int32_t speed_M_min_toE01_M_min(int32_t speedm_min)
{
	return speedm_min*10;
}