private void CompassUpdate()
 {
     //RobotState.SetRawHeading(_compass.readRaw());
     //RobotState.SetHeading(_compass.ReadHeading());
     RobotState.SetHeading(_compass.GetHeading());
     Thread.Sleep(_delay);
 }