Refactored initial prototype to be more manageable

Fault tolerance in the RT thread was greatly improved.