Updating more locomotion code to be nice and clean - code in progress, not finished