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);
}
private async void XboxController_FunctionButtonChanged(int button)
{
if (_functionStopwatch.ElapsedMilliseconds > 250)
{
_functionStopwatch.Restart();
}
else
{
return;
}
switch (button)
{
case 0: //A
_selectedIkFunction--;
if (_selectedIkFunction < 0)
{
_selectedIkFunction = 0;
}
await _display.WriteAsync($"{Enum.GetName(typeof(SelectedIkFunction), _selectedIkFunction)}", 1);
await _ik.RequestSetFunctionAsync(_selectedIkFunction);
break;
case 1: //B
_selectedIkFunction++;
if ((int)_selectedIkFunction > 14)
{
_selectedIkFunction = (SelectedIkFunction)14;
}
await _display.WriteAsync($"{Enum.GetName(typeof(SelectedIkFunction), _selectedIkFunction)}", 1);
await _ik.RequestSetFunctionAsync(_selectedIkFunction);
break;
case 2: //X
if (_selectedGpsFunction == SelectedGpsFunction.GpsDisabled)
{
await _navigator.StartAsync();
await _display.WriteAsync("GPS Nav Enabled", 1);
_selectedGpsFunction = SelectedGpsFunction.GpsEnabled;
}
else
{
await _display.WriteAsync("GPS Nav Disabled", 1);
_navigator.Stop();
_selectedGpsFunction = SelectedGpsFunction.GpsDisabled;
}
break;
case 3: //Y
break;
case 7: //StartAsync button
_isMovementStarted = !_isMovementStarted;
if (_isMovementStarted)
{
await _ik.RequestSetFunctionAsync(SelectedIkFunction.GaitSpeed);
_ik.RequestBodyPosition(_bodyRotX, _bodyRotZ, _bodyPosX, _bodyPosZ, _bodyPosY, _bodyRotY);
_ik.RequestSetGaitOptions(_gaitSpeed, _legLiftHeight);
_ik.RequestSetGaitType(GaitType.TripleTripod16);
_ik.RequestMovement(_gaitSpeed, _travelLengthX, _travelLengthZ, _travelRotationY);
}
else
{
_ik.RequestMovement(_gaitSpeed, 0, 0, 0);
}
SetGaitOptions();
_ik.RequestSetMovement(_isMovementStarted);
break;
case 6: //back button
await Gps.Gps.CurrentLatLon.SaveWaypoint();
break;
default:
await _display.WriteAsync($"Unknown button {button}", 1);
break;
}
}