Implemented debug using UART
Merge branch 'feature/debug' into develop