Experimental 2 tested
From point A to point B work

Add new feature
- Obstacle detect