Refactored the control_system and the simulator stack

All the modules inside these two stacks have been refactored.
No. i am not at the lab. Everyone from the mech team left the lab soon.