Small tweaks to nav, nav controller and objectDisplacement. Everything
implemented works weel. Still need localizaiton and further bluetooth
testing