Exemplo n.º 1
0
        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();
        }