Fixed jerkiness in rapid moves

Merged code fixes for jerky movements at fast speeds per Repetier