reworked OurRobot.replanIfNeeded() a bit