Fixed algorighm and made the homing better.