fixed a couple issues in OurRobot.replanIfNeeded()