Added vision stuff and nav logic.
Nothing works still.