Adds square autonomous test routine where robot drives in square.
Adds rotate autonomous test routine where robot spins at increasing larger angles.
Fixes turn command by adding power output floor which Kp control is applied on top of.
Add check to turn command to verify angular velocity is near-zero before terminating.
Comments out code for subsystems which do not have valid hardware which was creating a lot of error messages.
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)
[ ] 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?
Tested on robot
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
Description
Adds square autonomous test routine where robot drives in square. Adds rotate autonomous test routine where robot spins at increasing larger angles. Fixes turn command by adding power output floor which Kp control is applied on top of. Add check to turn command to verify angular velocity is near-zero before terminating. Comments out code for subsystems which do not have valid hardware which was creating a lot of error messages.
Type of change
How Has This Been Tested?
Tested on robot
Author Checklist: