Added worm drive (continuous servo) code