private void ComputeGoal()
{
robotState.goalBearingDegrees = currentSensorsData.CompassHeadingDegrees;
robotState.goalDistanceMeters = 2.0d;
robotState.computeGoalGeoPosition(robotPose);
robotPose.reset();
robotPose.direction = new Direction()
{
heading = currentSensorsData.CompassHeadingDegrees, bearingRelative = 0.0d, distanceToGoalMeters = 2.0d
};
driveController.OdometryReset();
}