Updated serial code to work on UART2 and move command handling to a main loop task