Uart communication working, now make it work correctly