Finalize Controller.cpp and tweak implementation of Test_controller.cpp. Have all simulated systems output to 'roboticslab3.m' for all timeseries.