Added in walking capabilities. Needs a lot of testing on hardware but works in simulation