Partially finished code for navigation robot V2