Simulator

[BrightLaw9]

Not to be merged (as it is simulator code), but please give advice on how to improve overall structure of auto

There had to be some work-around -- like manually updating odometry -- will be excluded in real robot

[MichGedd]

@BrightLaw9 Looks good so far. Can you try adding Motion Profiling on the Drive commands? See shoulder GoToPosition command as a reference. You can do PID based on the position or velocity, not sure which one is better / easier