fixed the USART issue and some small bugs

*Replaced the infinite loop in USART IRQ Handler