Epic auto routine

[wubbasauce]

Description

Added an auto routine for Tag id 6. Robot will score preloaded piece, the grab a cube from center of arena and score it. Also fixed some bugs regarding the RotateToPosition command.

Type of change

• [x] New feature (non-breaking change which adds functionality)
• [ ] Breaking change (fix or feature that would cause existing functionality to not work as expected)
• [x] Bugfix (non-breaking change which fixes an issue, add DEF # above in description)
• [ ] Environment change (changes to developer environment or CI configuration)
• [ ] Documentation Update (if none of the other choices apply)

How Has This Been Tested?

Please describe the tests that you ran to verify your changes. Please also list any relevant details for your test configuration. Identify any new potential hazards or risks introduced.

Author Checklist:

• [x] I have performed a self-review of my code
• [x] I have added 1-2 reviewers
• [x] I have @mentioned relevant non-reviewers as an FYI
• [x] I have included a detailed description of my changes and their motivations

[wubbasauce]

@CJc318 This is the auto routine for tag ID 6