More Porting and tests for simulator

Added a timer for the simulator.
More porting of autonomous code.