Refactored the control_system and the simulator stack

All the modules inside these two stacks have been refactored.
ohhk... https://web.duke.edu/robotics/html/auv_software_propulsion.html read this