Robot is walking! Some code cleanup as well.