Simple worked version with USART1_Handler