Merge in more changes

Fix - leg position issue
Make Single Leg mode code optional
Some more Debug Terminal code
Including ability to track servo