Add Unit Testing Tasks to Main

[N8BWert]

Description

This PR is relatively large, but all it really does is move all of the individual unit tests for different aspects of the robot into main. This means that a controller can now be used to spot-test specific aspects of the robot without having to reflash the robot. Additionally, this PR also starts using the PIT to decrease the latency of scheduling the motion_control task.

Associated / Resolved Issue

Conglomerate Tests into Main and Use GPT to schedule motion control loop

Key Files to Review

Main

• control/src/main.rs

Review Checklist

• Docstrings: All methods and classes should have the file appropriate docstrings which follow the guidelines in the "Contributing"
  page of our docs.
• Remove extra print statements: Any print statements used for debugging should be removed