Auto end pose angle function

[SuleymanSade]

• [ ] Make a function that has the robot to any arbitrary end pose vs. the April tag.
• [x] Join this with the code with the equations from Preston so we can end up at an arbitrary angle and location relative to the april tag.
• [x] For 11/11 make the calculations for the positive x offset relative to the April tag.
• [ ] Make good documentation with pictures.
• [x] Figure out the heading
• [ ] Figure out ending pose angle relative to the april tag.

[SuleymanSade]

• Robot deltax and y working for negative xPrime

[An-JieYu]

• Relative angle pose is almost working we think the problem is the offset of cameras position relative to the robot's center.