Cleaned up visual servo code and added ~ to the parameters so they load from the right place