quick dirty code to test features: sonars, motors, encoders (interrupts), Serial comms