Simplified and fixed lead screw code
Merge origin/master