Implement FRC testmode

[m10653]

Implement the test mode detailed here within WIPlib docs.

https://docs.wpilib.org/en/stable/docs/software/vscode-overview/using-test-mode.html

Our test mode should validate all critical components on the robot as a sell test. Verifying sensors and everything are plugged in the right way are not backwards etc.

These tests should be designed to be run on our robot cart/stand.

An example implementation would be displaying on smart dashboard "Motor Front right moving forward" waiting for confirmation from a button press and then testing Front left etc. While this is going on basic assertion checks should be done to make sure there is sensible data coming from the encoders. IE the encoder is positivity increasing when moving forward and backward etc.

Below is a list of things that should be tested (this list is not exhaustive)

• Validate Every motor is in the correct location and direction (Test one at a time)
• Validate Motors are in correct Master Slave configuration (Where applicable)
• Validate Encoders are functional and move in the correct directions.
• Test all actuators can be controlled (test for missing connections, direction if applicable)
• Test Gyroscope orientation is correct and functional.
• Warn about low battery voltage (Sanity check to make sure we use fresh batteries before matches)
• Open test all controls (Verify controller works and mappings are correct)
• Check that autonomous selector works
• Test simple autonomous if possible.