Semi-accurate speed

Implemented speed control in Leg trajectory functions and did a little
more cleanup.