Old FSM with soft faults and unit tests

[rebeccamccabe]

This is very old state machine code that is superseded by branch fsm-consolidated.
However, this branch includes two items which are not in the consolidated code:

• Unit tests for the FSM (unclear if this is necessary)
• Soft fault thresholds for motor speed and WEC position/angle (should be added)