--Got most of driving logic done, as well as logging of the commands executed.  Need to finish up driving logic, then do sensor reading and sensed location from Kalman