Rework TC Mux

[RCMast3r]

• [ ] only have one controller operating at once
• [ ] only have one controller output be gotten out of the tc mux and make it a functional thing instead of a member variable
• [ ] make a clear safety manager for the values gotten from the torque controllers
• [ ] remove sub-ticking of random logic checking and ensure that there doesnt exist nested tick stuff as this can introduce conflicts for stuff
• [ ] create state estimation system that receives input from all sensors and then creates a shared system state that is given to the controllers.

[RCMast3r]

we can just make the torque controllers each different "systems" with each inheriting from a base class that just has the output function and an input that contains a super-state of the car that has every variable that is needed by all the controllers that gets fed by the sensors of the car. this state estimator should handle all of the filtering and sensor data unification for ease of distributing to the controllers. It can just be given access to the CAN interface and the other sensor interfaces.

[walkermburns]

This is probably out of scope for FS Invitational, but we can try. TC MUX and State Estimator should be seperate issues and are each a pretty big project. TC MUX rework is probably higher priority

[walkermburns]

Taking this out of the Formula South Milestone, we are just going to have to work with what we got for now.

[walkermburns]

Ideas for simplification:

• Replace templated TC base class
• Make all controllers inherit the simplified base class
• Use an array of controller references rather than array of writeouts - (may not work depending on power limiting implementation)
• 'active', 'sleeping', and 'switching' flags to tune controller behavior based on currently selected controller in TCMux
• Possibly unify the input struct to allow for ticking all controllers as the base class (only one or two tick functions)
• Re-evaluate switching protection to work for both speed and torque-setpoint based controllers and make sure that the driver will be able to safely panic-switch to mode-0 without unintended consequences from switching through other controllers
• Investigate possibilities with torque/power limit in TC MUX. May be able to place these in the base TC class to allow certain controllers to ignore torque limit (launch controllers)
• Either remove ticks from within controllers and only tick from TC MUX, or remove ticking from TC MUX and improve inner controller ticking with active/sleeping tick rates