Add a small omnidrive test program

This should make the robot drive forward for 2 seconds, left for 2 seconds, backwards for 2 seconds and then right for 2 seconds.