Fixed a lot of bugs in the self driving to X Y pose function. Still need to comment them properly.