Basic robot code to do initial hardware test