More move code development

Fixed bug in lookahead calculations
Implemented slower homing when near endstop