First working prototype

Should be able to avoid one obstacle