Finished implementation of simulation and tested

Implementation complete
Tested with swerve algorithm. test results in run.dat and Swervetest.ods
Things to test: sensor and algorithms