Improve thruster scaling algorithm

[ShaanGondalia]

Background

The thruster scaling algorithm is quite rudimentary, involving some hacky math that likely isn't optimal. Half of the scaling is done in thruster_manager.py and the other half is done in thruster_controls.py. The current algorithm is something like this:

1. Obtain thruster allocations from solving Tx = p
2. If any thruster allocation > 1, scale down all allocations such that the maximum thruster allocation is 1
   • This scaling is done by dividing all allocations by the largest allocation
3. Round small values (< 0.001, chosen arbitrarily) to 0
4. Multiply all values by (max pid control effort)/(max thruster allocation).
   • This was a very hacky solution to keep thruster allocations "in line" with pid control efforts. For instance, if the largest pid control effort is 0.1, this intuitively means that the robot shouldn't move very quickly in any direction, so we want to cap the thruster allocation to 0.1 (10% of thruster power).
   • This is a gross oversimplification of how the system should actually work, because pid control efforts and thruster allocations don't have such a simple direct relationship.
5. Clamp values to [-1, 1] (this step is redundant but added for safety)
6. Convert allocations to 8-bit integer by multiplying by 127 (and scaling down if we want to limit speed)

We should do more research on how to optimally scale thruster output after the thruster allocation calculation has occurred. The main areas of concern are steps 2 and 4.

Task

• Research ways that thruster allocation is handled by other autonomous robots
• Implement a more robust thruster scaling algorithm