internal async Task InitializeAsync()
{
if (_gps == null)
{
return;
}
_waypoints = await GpsExtensions.LoadWaypoints();
await _display.WriteAsync($"{_waypoints.Count} waypoints");
}
internal async Task <bool> NavigateToWaypoint(LatLon currentWaypoint, CancellationToken cancelationToken)
{
var distanceHeading = GpsExtensions.GetDistanceAndHeadingToDestination(Gps.Gps.CurrentLatLon.Lat, Gps.Gps.CurrentLatLon.Lon, currentWaypoint.Lat, currentWaypoint.Lon);
var distanceToWaypoint = distanceHeading[0];
var headingToWaypoint = distanceHeading[1];
var travelLengthX = 0D;
var travelLengthZ = 0D;
var travelRotationY = 0D;
var nomGaitSpeed = 50D;
travelLengthZ = -50;
var turnDirection = "None";
while (distanceToWaypoint > 10) //Inches
{
if (cancelationToken.IsCancellationRequested)
{
return(false);
}
await _display.WriteAsync($"WP D/H {distanceToWaypoint}, {headingToWaypoint}", 1);
await _display.WriteAsync($"{turnDirection} {Gps.Gps.CurrentLatLon.Heading}", 2);
if (headingToWaypoint + 5 > 359 && Math.Abs(headingToWaypoint - Gps.Gps.CurrentLatLon.Heading) > 1)
{
var tempHeading = (headingToWaypoint + 5) - 359;
if (Gps.Gps.CurrentLatLon.Heading > tempHeading)
{
turnDirection = "Right";
travelRotationY = -1;
}
else
{
turnDirection = "Left";
travelRotationY = 1;
}
}
else if (headingToWaypoint - 5 < 1 && Math.Abs(headingToWaypoint - Gps.Gps.CurrentLatLon.Heading) > 1)
{
var tempHeading = (headingToWaypoint + 359) - 5;
if (Gps.Gps.CurrentLatLon.Heading < tempHeading)
{
turnDirection = "Right";
travelRotationY = 1;
}
else
{
turnDirection = "Left";
travelRotationY = -1;
}
}
else if (Gps.Gps.CurrentLatLon.Heading > headingToWaypoint - 5 && Gps.Gps.CurrentLatLon.Heading < headingToWaypoint + 5)
{
travelRotationY = 0;
turnDirection = "None";
}
else if (headingToWaypoint > Gps.Gps.CurrentLatLon.Heading + 20)
{
if (Gps.Gps.CurrentLatLon.Heading - headingToWaypoint > 180)
{
turnDirection = "Left+";
travelRotationY = -2;
}
else
{
turnDirection = "Right+";
travelRotationY = 2;
}
}
else if (headingToWaypoint > Gps.Gps.CurrentLatLon.Heading)
{
if (Gps.Gps.CurrentLatLon.Heading - headingToWaypoint > 180)
{
turnDirection = "Left";
travelRotationY = -1;
}
else
{
turnDirection = "Right";
travelRotationY = 1;
}
}
else if (headingToWaypoint < Gps.Gps.CurrentLatLon.Heading - 20) //If it has a long ways to turn, go fast!
{
if (Gps.Gps.CurrentLatLon.Heading - headingToWaypoint < 180)
{
turnDirection = "Left+";
travelRotationY = -2;
}
else
{
turnDirection = "Right+";
travelRotationY = 2; //Turn towards its right
}
}
else if (headingToWaypoint < Gps.Gps.CurrentLatLon.Heading)
{
if (Gps.Gps.CurrentLatLon.Heading - headingToWaypoint < 180)
{
turnDirection = "Left";
travelRotationY = -1;
}
else
{
turnDirection = "Right";
travelRotationY = 1;
}
}
_ikController.RequestMovement(nomGaitSpeed, travelLengthX, travelLengthZ, travelRotationY);
await Task.Delay(50, cancelationToken);
distanceHeading = GpsExtensions.GetDistanceAndHeadingToDestination(Gps.Gps.CurrentLatLon.Lat, Gps.Gps.CurrentLatLon.Lon, currentWaypoint.Lat, currentWaypoint.Lon);
distanceToWaypoint = distanceHeading[0];
headingToWaypoint = distanceHeading[1];
if (cancelationToken.IsCancellationRequested)
{
return(false);
}
}
await _display.WriteAsync($"WP D/H {distanceToWaypoint}, {headingToWaypoint}", 1);
await _display.WriteAsync($"Heading {Gps.Gps.CurrentLatLon.Heading}", 2);
return(true);
}
