Improved navigation. Updated firmware to support new motors