Optimise entry points

[BogdanTheGeek]

When routing pockets, start the next pocket as close to the end of the previous one as possible.
Would also be nice optimise route commands in drill.py (use plunge and retract to detect slots)