New driving to point function

Now it's set drive before loop. And stop robot after loop. The robot
stops if he is in radius of 15mm near the goal point.
Add slow possibility