homing refactor, needs testing on robot