void SERVO_CONTROLLER_Start (void)
{
    //Start timers
    HAL_TIM_Base_Start_IT(&htim1);
    HAL_TIM_Base_Start_IT(&htim2);
    HAL_TIM_Base_Start_IT(&htim3);
    HAL_TIM_Base_Start_IT(&htim4);
    
    //Start PWMs
    for (int i = 0; i < SERVO_TotalChannelsNum; i++)
    {
        TIM_HandleTypeDef *htim = tim_handlers_table[i];
        int32_t channel = tim_channels_table[i];
        
        if (!htim || channel < 0)
        {
            continue;
        }
        
        HAL_TIM_PWM_Start(htim, (uint32_t)channel);
        
        SERVO_Channel *ch = &servo_channels[i];
        SERVO_CONTROLLER_SetChannel((SERVO_ChannelId)i, ch->pulseCycles);
    }
}

/* TIM2 init function */
void MX_TIM2_Init(void)
{

  TIM_ClockConfigTypeDef sClockSourceConfig;
  TIM_MasterConfigTypeDef sMasterConfig;

  htim2.Instance = TIM2;
  htim2.Init.Prescaler = 1;
  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim2.Init.Period = 875;
  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim2.Init.RepetitionCounter = 0;
  HAL_TIM_Base_Init(&htim2);
  // Initialize interrupt triggering
//  HAL_NVIC_EnableIRQ(TIM2_IRQn);

  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig);

  sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig);
  if (HAL_TIM_Base_Start_IT(&htim2) != HAL_OK) {
      while(1);
  }

}

/**
* @brief This function handles EXTI line0 interrupt.
*/
void EXTI0_IRQHandler(void)
{
  /* USER CODE BEGIN EXTI0_IRQn 0 */
  // Checks if the interrupt has been triggered by a rising edge( first 
  // pressing of button), starts a 2secs timer and make the line falling
  // edge sensitive.
  if ((EXTI-> RTSR) & (GPIO_PIN_0))
  {
    HAL_TIM_Base_Start_IT(&htim6);
    EXTI-> RTSR &= (~GPIO_PIN_0);
    EXTI-> FTSR |= GPIO_PIN_0;
  }
  else
  {
    // If a falling edge is detected( button is released) before the countdown
    // ends, we stop the countdown, reset it and make the line rising
    // edge sensitive again.
    HAL_TIM_Base_Stop_IT(&htim6);
    __HAL_TIM_SET_COUNTER(&htim6,0);
    EXTI-> FTSR &= (~GPIO_PIN_0);
    EXTI-> RTSR |= GPIO_PIN_0;
  }
  /* USER CODE END EXTI0_IRQn 0 */
  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_0);
  /* USER CODE BEGIN EXTI0_IRQn 1 */

  /* USER CODE END EXTI0_IRQn 1 */
}

/**
  * @brief  Initializes the CDC media low layer      
  * @param  None
  * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
  */
static int8_t CDC_Itf_Init(void)
{
  /*##-6- Enable TIM peripherals Clock #######################################*/
  USBCDCTIMx_CLK_ENABLE();

  /*##-7- Configure the NVIC for TIMx ########################################*/
  /* Set Interrupt Group Priority */
  HAL_NVIC_SetPriority(USBCDCTIMx_IRQn, 6, 0);

  /* Enable the TIMx global Interrupt */
  HAL_NVIC_EnableIRQ(USBCDCTIMx_IRQn);
  /*##-3- Configure the TIM Base generation  #################################*/
  TIM_Config();
  
  /*##-4- Start the TIM Base generation in interrupt mode ####################*/
  /* Start Channel1 */
  if(HAL_TIM_Base_Start_IT(&USBCDCTimHandle) != HAL_OK)
  {
    /* Starting Error */
    Error_Handler();
  }
  
  /*##-5- Set Application Buffers ############################################*/
  USBD_CDC_SetTxBuffer(&usb_cdc_dev_param, UserTxBuffer, 0);
  USBD_CDC_SetRxBuffer(&usb_cdc_dev_param, UserRxBuffer);
  
  return (USBD_OK);
}

/**
  * @brief  Initialize the hardware timer 4.
  * @param  None
  * @retval None
  */
void HardwareTimer4_Init(void)
{
	__TIM4_CLK_ENABLE();	/* Enable TIM4 interface clock */
		
	/* Desired_timer_frequency = Timer_input_frequency / (prescaler * period)
		 Timer_input_frequency = 84 MHz (same as TIM3, TIM4 is also on APB1)
		 Desired_timer_frequency = 8 Hz (Rotate the LEDs at that frequency for the visuals)
		 ---> prescaler * period = 84 MHz / 8 Hz = 10500000 = 2100 * 5000
		 ---> prescaler = 5000, period = 2100 */
	
	timmy4.Instance = TIM4;																/* Use TIM4 */
	timmy4.Init.Prescaler = 2100;													/* Set prescaler to 2100 */
	timmy4.Init.CounterMode = TIM_COUNTERMODE_DOWN;				/* Count down */
	timmy4.Init.Period = 5000;														/* Set period count register to 5000 */
	timmy4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;		/* Set clock division to 1 */
	timmy4.Init.RepetitionCounter = 0;										/* Not valid for TIM4 */
	timmy4.Channel = HAL_TIM_ACTIVE_CHANNEL_CLEARED;			/* Not using channels */
	
	/* Initialize the TIM Time Base Unit */
	HAL_TIM_Base_Init(&timmy4);
	
	/* Start TIM3 in interrupt mode */
	HAL_TIM_Base_Start_IT(&timmy4);
	
	/* Set priority for TIM3 IRQ */
	HAL_NVIC_SetPriority(TIM4_IRQn, 0, 0);
	
	/* Enable IRQ for the TIM3 Timer*/
	HAL_NVIC_EnableIRQ(TIM4_IRQn);
}

/* --- Load and start periodic event counter --- */
void startPerEvent(uint32_t rateFactor)
{
	htim2.Init.Period = rateFactor;
	HAL_TIM_Base_Init(&htim2);

	HAL_TIM_Base_Start_IT(&htim2);		
}

void Timer_For_Main_Loop_Init()
{
	__TIM6_CLK_ENABLE();
  /* Compute the prescaler value to have TIMx counter clock equal to 1 KHz */
  uwPrescalerValue = (uint32_t) ((SystemCoreClock / 1000) - 1);
  
  /*##-1- Configure the TIM peripheral #######################################*/ 
  /* Set TIMx instance */
  TimHandle.Instance = TIM6;
    
  /* Initialize TIMx peripheral as follow:
       + Period = 10000 - 1
       + Prescaler = SystemCoreClock/10000 Note that APB clock = TIMx clock if
                     APB prescaler = 1.
       + ClockDivision = 0
       + Counter direction = Up
  */
  TimHandle.Init.Period = 10000 - 1;
  TimHandle.Init.Prescaler = uwPrescalerValue;
  TimHandle.Init.ClockDivision = 0;
  TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
  HAL_TIM_Base_Init(&TimHandle);

  /*##-2- Start the TIM Base generation in interrupt mode ####################*/
  /* Start Channel1 */
  HAL_TIM_Base_Start_IT(&TimHandle);
	HAL_NVIC_EnableIRQ(TIM6_IRQn);
}

int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration----------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* Configure the system clock */
  SystemClock_Config();

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_ADC1_Init();
  MX_ADC2_Init();
  MX_TIM1_Init();
  MX_TIM2_Init();
  MX_USART1_UART_Init();

  /* USER CODE BEGIN 2 */

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  HAL_TIM_Base_Start_IT(&htim1);
  HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
  HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
  HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
  HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_3);
  HAL_TIM_Base_Start_IT(&htim2);
  while (1)
  {
  /* USER CODE END WHILE */
  /* USER CODE BEGIN 3 */

  }
  /* USER CODE END 3 */

}

int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration----------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* Configure the system clock */
  SystemClock_Config();

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_TIM1_Init();

  HAL_TIM_Base_Start_IT(&htim1);

  /* USER CODE BEGIN 2 */

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {

//	  uint32_t ticksPassed = 0;
//	  if (GPIOA->IDR & 0x01) {
//		  uint32_t ticksPassed = 0;
//		  while (GPIOA->IDR & 0x01) {
//			  ticksPassed++;
//		  }
//		  SysTick_Config(ticksPassed);
//	  }



//	  if (GPIOA->IDR & 0x01) {
//
//	  }
  /* USER CODE END WHILE */
//	  GPIOE->ODR ^= 0x0000FF00;
//	  int i = 0;
//	  while (i < 1000000) {
//		  i++;
//	  }

//	  HAL_GPIO_TogglePin(GPIOE, 0xFF00);
//	  HAL_Delay(1000);
  /* USER CODE BEGIN 3 */

  }
  /* USER CODE END 3 */

}

static void acquisition_start(void)
{
	HAL_DAC_Start_DMA(&hdac, DAC_CHANNEL_1, (uint32_t*)&wave_buff, WAVE_BUFF_LEN, DAC_ALIGN_12B_R);
	HAL_ADC_Start_IT(&hadc2);
	HAL_ADC_Start_DMA(&hadc1, (uint32_t*)&mic_buff, WAVE_BUFF_LEN);
	HAL_TIM_Base_Start(&htim2);
	HAL_TIM_Base_Start_IT(&htim3);
}

/**
  * @brief  This function configures the TIM6 as a time base source. 
  *         The time source is configured  to have 1ms time base with a dedicated 
  *         Tick interrupt priority. 
  * @note   This function is called  automatically at the beginning of program after
  *         reset by HAL_Init() or at any time when clock is configured, by HAL_RCC_ClockConfig(). 
  * @param  TickPriority: Tick interrupt priority.
  * @retval HAL status
  */
HAL_StatusTypeDef HAL_InitTick (uint32_t TickPriority)
{
  RCC_ClkInitTypeDef    sClokConfig;
  uint32_t              uwTimclock, uwAPB1Prescaler = 0;
  uint32_t              uwPrescalerValue = 0;
  uint32_t              pFLatency;
  
    /*Configure the TIM6 IRQ priority */
  HAL_NVIC_SetPriority(TIM6_DAC_IRQn, TickPriority ,0); 
  
  /* Get clock configuration */
  HAL_RCC_GetClockConfig(&sClokConfig, &pFLatency);
  
  /* Get APB1 prescaler */
  uwAPB1Prescaler = sClokConfig.APB1CLKDivider;
  
  /* Compute TIM6 clock */
  if (uwAPB1Prescaler == 0) 
  {
    uwTimclock = HAL_RCC_GetPCLK1Freq();
  }
  else
  {
    uwTimclock = 2*HAL_RCC_GetPCLK1Freq();
  }

  /* Compute the prescaler value to have TIM6 counter clock equal to 1MHz */
  uwPrescalerValue = (uint32_t) ((uwTimclock / 1000000) - 1);
  
  /* Initialize TIM6 */
  TimHandle.Instance = TIM6;
    
  /* Initialize TIMx peripheral as follow:
       + Period = [(TIM6CLK/1000) - 1]. to have a (1/1000) s time base.
       + Prescaler = (uwTimclock/1000000 - 1) to have a 1MHz counter clock.
       + ClockDivision = 0
       + Counter direction = Up
  */
  TimHandle.Init.Period = (1000000 / 1000) - 1;
  TimHandle.Init.Prescaler = uwPrescalerValue;
  TimHandle.Init.ClockDivision = 0;
  TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
  if(HAL_TIM_Base_Init(&TimHandle) != HAL_OK)
  {
    /* Initialization Error */
    ErrorHandler();
  }
  
  /* Start the TIM time Base generation in interrupt mode */
  if(HAL_TIM_Base_Start_IT(&TimHandle) != HAL_OK)
  {
    /* Starting Error */
    ErrorHandler();
  }
  
   /* Return function status */
  return HAL_OK;
}

/**
  * @brief  This function configures the TIM6 as a time base source.
  *         The time source is configured  to have 1ms time base with a dedicated
  *         Tick interrupt priority.
  * @note   This function is called  automatically at the beginning of program after
  *         reset by HAL_Init() or at any time when clock is configured, by HAL_RCC_ClockConfig().
  * @param  TickPriority Tick interrupt priority.
  * @retval HAL status
  */
HAL_StatusTypeDef HAL_InitTick (uint32_t TickPriority)
{
  RCC_ClkInitTypeDef    clkconfig;
  uint32_t              uwTimclock, uwAPB1Prescaler = 0U;
  uint32_t              uwPrescalerValue = 0U;
  uint32_t              pFLatency;

  /*Configure the TIM6 IRQ priority */
  HAL_NVIC_SetPriority(TIM6_DAC_IRQn, TickPriority ,0U);

  /* Enable the TIM6 global Interrupt */
  HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);

  /* Enable TIM6 clock */
  __HAL_RCC_TIM6_CLK_ENABLE();

  /* Get clock configuration */
  HAL_RCC_GetClockConfig(&clkconfig, &pFLatency);

  /* Get APB1 prescaler */
  uwAPB1Prescaler = clkconfig.APB1CLKDivider;

  /* Compute TIM6 clock */
  if (uwAPB1Prescaler == RCC_HCLK_DIV1)
  {
    uwTimclock = HAL_RCC_GetPCLK1Freq();
  }
  else
  {
    uwTimclock = 2*HAL_RCC_GetPCLK1Freq();
  }

  /* Compute the prescaler value to have TIM6 counter clock equal to 1MHz */
  uwPrescalerValue = (uint32_t) ((uwTimclock / 1000000U) - 1U);

  /* Initialize TIM6 */
  TimHandle.Instance = TIM6;

  /* Initialize TIMx peripheral as follow:
   + Period = [(TIM6CLK/1000) - 1]. to have a (1/1000) s time base.
   + Prescaler = (uwTimclock/1000000 - 1) to have a 1MHz counter clock.
   + ClockDivision = 0
   + Counter direction = Up
   */
  TimHandle.Init.Period = (1000000U / 1000U) - 1U;
  TimHandle.Init.Prescaler = uwPrescalerValue;
  TimHandle.Init.ClockDivision = 0;
  TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
  TimHandle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if(HAL_TIM_Base_Init(&TimHandle) == HAL_OK)
  {
    /* Start the TIM time Base generation in interrupt mode */
    return HAL_TIM_Base_Start_IT(&TimHandle);
  }

  /* Return function status */
  return HAL_ERROR;
}

int main(void)
{

	/* USER CODE BEGIN 1 */

	/* USER CODE END 1 */

	/* MCU Configuration----------------------------------------------------------*/

	/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
	HAL_Init();

	/* Configure the system clock */
	SystemClock_Config();

	/* Initialize all configured peripherals */
	MX_GPIO_Init();
	MX_DMA_Init();
	MX_I2C3_Init();
	MX_TIM1_Init();
	MX_TIM2_Init();
	MX_TIM3_Init();
	MX_TIM4_Init();
	MX_TIM5_Init();
	MX_USART1_UART_Init();
	MX_USART2_UART_Init();
	MX_TIM11_Init();

	/* USER CODE BEGIN 2 */
	HAL_TIM_Base_Start_IT(&htim11); // 100 msec timer
	initSonar( MAX_SONAR);
	initSerOutput();
	/* USER CODE END 2 */

	/* Infinite loop */
	/* USER CODE BEGIN WHILE */
	while (1)
	{
		/* USER CODE END WHILE */

		/* USER CODE BEGIN 3 */
		// >>>>> Sonar reading
		triggerSonar( COUPLE_0_2);
		HAL_Delay(49);
		triggerSonar( COUPLE_1_3);
		HAL_Delay(49);
		// <<<<< Sonar reading

		// >>>>> Serial Output
		convertMeasures();
		sendMeasures();
		// <<<<< Serial Output

	}
	/* USER CODE END 3 */

}

void Timer2_Cmd( uint32_t cmd )
{
  if (cmd == ENABLE) {
    HAL_TIM_Base_Start_IT(hTimer2.handle);
  }
  else {
    HAL_TIM_Base_Stop_IT(hTimer2.handle);
  }
}

/**
  * @brief  定时器初始化函数
  * @param  None
  * @retval None
  */
BOOL
xMBPortTimersInit( USHORT usTim1Timerout50us )
{
    /*##-1- Configure the TIM peripheral #######################################*/
    /* -----------------------------------------------------------------------
      In this example TIM3 input clock (TIM3CLK)  is set to APB1 clock (PCLK1),
      since APB1 prescaler is equal to 1.
        TIM3CLK = PCLK1
        PCLK1 = HCLK
        => TIM3CLK = HCLK = SystemCoreClock
      To get TIM3 counter clock at 10 KHz, the Prescaler is computed as following:
      Prescaler = (TIM3CLK / TIM3 counter clock) - 1
      Prescaler = (SystemCoreClock /10 KHz) - 1

      Note:
       SystemCoreClock variable holds HCLK frequency and is defined in system_stm32l1xx.c file.
       Each time the core clock (HCLK) changes, user had to update SystemCoreClock
       variable value. Otherwise, any configuration based on this variable will be incorrect.
       This variable is updated in three ways:
        1) by calling CMSIS function SystemCoreClockUpdate()
        2) by calling HAL API function HAL_RCC_GetSysClockFreq()
        3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency
    ----------------------------------------------------------------------- */

    /* Compute the prescaler value to have TIMx counter clock equal to 10000 Hz */
    uwPrescalerValue = (uint32_t)(SystemCoreClock / 20000) - 1;

    /* Set TIMx instance */
    TimHandle.Instance = TIMx;

    /* Initialize TIMx peripheral as follows:
         + Period = 10000 - 1
         + Prescaler = (SystemCoreClock/10000) - 1
         + ClockDivision = 0
         + Counter direction = Up
    */
    TimHandle.Init.Period            = (usTim1Timerout50us * 20) - 1;
    TimHandle.Init.Prescaler         = uwPrescalerValue;
    TimHandle.Init.ClockDivision     = 0;
    TimHandle.Init.CounterMode       = TIM_COUNTERMODE_UP;

    if (HAL_TIM_Base_Init(&TimHandle) != HAL_OK)
    {
        /* Initialization Error */
        Error_Handler();
    }

    /*##-2- Start the TIM Base generation in interrupt mode ####################*/
    /* Start Channel1 */
    if (HAL_TIM_Base_Start_IT(&TimHandle) != HAL_OK)
    {
        /* Starting Error */
        Error_Handler();
    }
	
	return TRUE;
}

int main(void) {
  HAL_Init();

  Nucleo_BSP_Init();
  MX_TIM3_Init();

  HAL_TIM_Base_Start_IT(&htim3);

  while (1);
}

void hubInit(void) {
	initIO();
	calculatePeriod();
	hubLUTInit();
	initRows();
	hubOrientation = HUB_ROTATE_0;
	hubSetOrient(hubOrientation);
	clearScreen();
	//fillScreen(COLOR_RGB(255, 255, 255));
	hubTestBmp();
	screenRedraw();
	
	// 100 Hz * 16 Phases
  HUB_TIMER_CLK_ENABLE();
  TIM_ClockConfigTypeDef sClockSourceConfig;

  hubtim.Instance = HUB_TIMER;
  hubtim.Init.Prescaler = HUB_PRESCALER - 1;
  hubtim.Init.CounterMode = TIM_COUNTERMODE_UP;
  hubtim.Init.Period = period0 - 1;
  hubtim.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  HAL_TIM_Base_Init(&hubtim);

  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  HAL_TIM_ConfigClockSource(&hubtim, &sClockSourceConfig);

	HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4);
  HAL_NVIC_SetPriority(TIM1_BRK_TIM9_IRQn, 0, 0);
  HAL_NVIC_EnableIRQ(TIM1_BRK_TIM9_IRQn);
	
	HAL_TIM_Base_Start_IT(&hubtim);
	
	// Timer 2 for NOE
	HUB_TIMER2_CLK_ENABLE();

  TIM_OC_InitTypeDef sConfigOC;

  hubtim2.Instance = HUB_TIMER2;
  hubtim2.Init.Prescaler = 0;
  hubtim2.Init.CounterMode = TIM_COUNTERMODE_UP;
  hubtim2.Init.Period = 0xffff;
  hubtim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  HAL_TIM_Base_Init(&hubtim2);

  HAL_TIM_OC_Init(&hubtim2);

  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = period2Min;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  HAL_TIM_OC_ConfigChannel(&hubtim2, &sConfigOC, NOE_TIM_CHANNEL);

	HAL_TIM_PWM_Start(&hubtim2, NOE_TIM_CHANNEL); 

}

/**
  * @brief  Period elapsed callback in non blocking mode
  * @param  htim : TIM handle
  * @retval None
  */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
    __HAL_TIM_SET_COUNTER(htim, 0);

    switch (KB_state) {
    case BTN_Down:
        if ((len_KB_str == 0) || (pKB_str == NULL)) {
            HAL_TIM_Base_Stop_IT(htim);
            return;
        }
        KB_state = BTN_Up;

        char2KBID(*pKB_str);
        SEGGER_RTT_printf(0, "USB send char %c \r\n" , *pKB_str);
        pKB_str++;
        len_KB_str--;
        USBD_HID_SendReport(&USBD_Device, KB_USBBuf, HID_KB_SIZE);

        __HAL_TIM_SET_COUNTER(&TimHandle, 0);
        __HAL_TIM_SET_AUTORELOAD(&TimHandle, 200-1); // 200ms
        HAL_TIM_Base_Start_IT(&TimHandle);
        break;

    case BTN_Up:
        memset(KB_USBBuf, 0, 9);
        KB_USBBuf[0] = 1;
        USBD_HID_SendReport(&USBD_Device, KB_USBBuf, HID_KB_SIZE);
        if (len_KB_str == 0) {
            pKB_str = NULL;
            HAL_TIM_Base_Stop_IT(htim);
        } else {
            KB_state = BTN_Down;
            __HAL_TIM_SET_COUNTER(&TimHandle, 0);
            __HAL_TIM_SET_AUTORELOAD(&TimHandle, 200-1); // 200ms
            HAL_TIM_Base_Start_IT(&TimHandle);
        }
        break;
    default:
        pKB_str = NULL;
        HAL_TIM_Base_Stop_IT(htim);
        break;
    }
}

void CAM_init()
{
	/*
	 * Initialize hardware
	 */
	CAM_GPIO_init();
	CAM_TIM_init(SLOW);
	CAM_ADC_init();

	/*
	 * Initial GPIO pins status
	 */
	RESET_high();
	START_low();
	LOAD_low();
	SIN_low();

	/*
	 * Configure NVIC (interrupts)
	 */
	HAL_NVIC_SetPriority(ADC_IRQn, 0, 0);
	HAL_NVIC_EnableIRQ(ADC_IRQn);

	HAL_NVIC_SetPriority(TIM2_IRQn, 0, 1);
	HAL_NVIC_EnableIRQ(TIM2_IRQn);

	HAL_NVIC_SetPriority(EXTI1_IRQn, 3, 0);
	HAL_NVIC_EnableIRQ(EXTI1_IRQn);

	/*
	 * Initialize library global variables
	 */
	imageCursor = 0;

	flag_captureGoing = 0;
	flag_captureEnd = 0;

	clockPhase = 0;

	acquiring = 0;

	skip = PIXSKIP;

	/*
	 * Start ADC
	 */
	HAL_ADC_Start_IT(&AdcHandle);					// Providing ConvCplt Callback

	/*
	 * Start TIM Base and TIM PWM
	 */
	HAL_TIM_Base_Start_IT(&htim);					// Providing PeriodElapsed Callback
	HAL_TIM_PWM_Start_IT(&htim, TIM_CHANNEL_2);	// Providing PulseFinished Callback
}

void TimInit (void)
{
  uint32_t __prescaler = (uint32_t) ((SystemCoreClock /2) / 1000) - 1;
  
  TimHandle.Instance = TIM1; 
  TimHandle.Init.Period = 50 - 1;
  TimHandle.Init.Prescaler = __prescaler;
  TimHandle.Init.ClockDivision = 0;
  TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
  if(HAL_TIM_Base_Init(&TimHandle) != HAL_OK)
  {
    Error_Handler();
  }
  
	
  if(HAL_TIM_Base_Start_IT(&TimHandle) != HAL_OK)
  {
    Error_Handler();
  }
	
	__prescaler = (uint32_t) ((SystemCoreClock /2) / 1000) - 1;
  
  Tim2_Handle.Instance = TIM2;
  Tim2_Handle.Init.Period = 1000 - 1;
  Tim2_Handle.Init.Prescaler = __prescaler;
  Tim2_Handle.Init.ClockDivision = 10;
  Tim2_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
  if(HAL_TIM_Base_Init(&Tim2_Handle) != HAL_OK)
  {
    Error_Handler();
  }

  if(HAL_TIM_Base_Start_IT(&Tim2_Handle) != HAL_OK)
  {
    Error_Handler();
  }
	HAL_NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 0 ,1);
  HAL_NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);
	HAL_NVIC_SetPriority(TIM2_IRQn, 0 ,1);
  HAL_NVIC_EnableIRQ(TIM2_IRQn);
}