Got serial port working (USART2)