private async void GetDesiredTurnDirection()
{
if (this.nextWayPoint == null)
{
return;
}
this.IncrementCoordinates();
GeoCoordinate currentPosition = this.currentPosition;
var targetHeading = WayPointHelper.DegreeBearing(currentPosition, this.nextWayPoint);
// calculate where we need to turn to head to destination
var headingError = targetHeading -
this.compass.GetHeadingInDegrees(this.compass.ReadRaw());
// adjust for compass wrap
if (headingError < -180)
{
headingError += 360;
}
if (headingError > 180)
{
headingError -= 360;
}
// calculate which way to turn to intercept the targetHeading
if (Math.Abs(headingError) <= this.HEADING_TOLERANCE_DEGREES)
{
// if within tolerance, don't turn
this.turnDirection = Commands.DrivingDirection.Forward;
}
else if (headingError < 0)
{
this.turnDirection = Commands.DrivingDirection.Left;
}
else if (headingError > 0)
{
this.turnDirection = Commands.DrivingDirection.Right;
}
else
{
this.turnDirection = Commands.DrivingDirection.Forward;
}
this.dummyIndex++;
NotifyUIEventArgs notifyEventArgs = new NotifyUIEventArgs()
{
NotificationType = NotificationType.Console,
Data = "Turn Direction - " + this.turnDirection
};
this.NotifyUIEvent(notifyEventArgs);
}
public static string MapDrivingDirectionToCommand(Commands.DrivingDirection drivingDirection)
{
switch (drivingDirection)
{
default:
case Commands.DrivingDirection.Forward:
return(Commands.DriveForward);
case Commands.DrivingDirection.Reverse:
return(Commands.DriveReverse);
case Commands.DrivingDirection.Left:
return(Commands.DriveLeft);
case Commands.DrivingDirection.Right:
return(Commands.DriveRight);
case Commands.DrivingDirection.Stop:
return(Commands.DriveStop);
}
}
