WIP - Quad stuff

Try to add start walking with leg farthest away
Also debug commands to set gait and how much of the balance to use.