First version of algorithm

Collision avoiding is working