Driving commands should run in a thread so they don't have to use immediate return. This allows the regulator to correct the encoder turns with gyros. OR think about how to make the command non blocking without immediate return.
Update heading using gyro at the end of moves (this is how it works in the master branch). The problem is that many moves are happening for some reason, so this update at move end gets stuff messed up. We want to figure out some way to do this so that headings and positions are actually accurate. Right now all the jitter in the gyro gets accumulated onto the heading instead of being cleaned up at the end like this would do.
We need: