private async void ConfigureFootSwitches()
{
try
{
_gpioController = GpioController.GetDefault();
}
catch (Exception e)
{
await _display.WriteAsync(e.Message);
return;
}
if (_gpioController != null)
{
_legGpioPins[0] = _gpioController.OpenPin(26);
_legGpioPins[1] = _gpioController.OpenPin(19);
_legGpioPins[2] = _gpioController.OpenPin(13);
_legGpioPins[3] = _gpioController.OpenPin(16);
_legGpioPins[4] = _gpioController.OpenPin(20);
_legGpioPins[5] = _gpioController.OpenPin(21);
foreach (var legGpioPin in _legGpioPins)
{
legGpioPin.DebounceTimeout = new TimeSpan(0, 0, 0, 0, 1);
legGpioPin.SetDriveMode(GpioPinDriveMode.InputPullUp);
}
}
else
{
await _display.WriteAsync("No Gpio?");
}
}
internal async Task InitializeAsync()
{
await Task.Run(() =>
{
var args = new SocketAsyncEventArgs
{
UserToken = _socket,
RemoteEndPoint = _endPoint
};
args.Completed += async(sender, eventArgs) =>
{
if (((Socket)sender).Connected)
{
_isConnected = true;
await _display.WriteAsync("NTRIP Connected");
await Task.Delay(500);
}
else
{
_isConnected = false;
await _display.WriteAsync("NTRIP Connection failed");
}
};
_socket.ConnectAsync(args);
}).ConfigureAwait(false);
}
internal async Task InitializeAsync()
{
if (_gps == null)
{
return;
}
_waypoints = await GpsExtensions.LoadWaypoints();
await _display.WriteAsync($"{_waypoints.Count} waypoints");
}
/// <summary>
/// This starts waiting for messages from the IoT Hub.
/// </summary>
/// <returns></returns>
internal async Task StartAsync()
{
while (true)
{
try
{
var receivedMessage = await _deviceClient.ReceiveAsync(new TimeSpan(int.MaxValue));
if (receivedMessage == null)
{
continue;
}
foreach (var prop in receivedMessage.Properties)
{
await _display.WriteAsync($"{prop.Key} {prop.Value}");
}
await _deviceClient.CompleteAsync(receivedMessage);
var messageData = Encoding.ASCII.GetString(receivedMessage.GetBytes());
IotEvent?.Invoke(null, new IotEventArgs {
EventData = receivedMessage.Properties, MessageData = messageData
});
}
catch
{
//Write out to the display perhaps
}
}
}
private async void DisplayTimerCallback(object state)
{
if (_selectedIkFunction == SelectedIkFunction.DisplayPing)
{
await _display.WriteAsync($"{_leftInches} {_centerInches} {_rightInches}", 2);
}
if (_selectedIkFunction == SelectedIkFunction.DisplayYPR)
{
await _display.WriteAsync($"{_yaw} {_pitch} {_roll}", 2);
}
if (_selectedIkFunction == SelectedIkFunction.DisplayAccel)
{
await _display.WriteAsync($"{_accelY}", 2);
}
}
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;
}
}
internal async Task DisplayCoordinates()
{
await _display.WriteAsync(CurrentLatLon.Lat.ToString(CultureInfo.InvariantCulture), 1);
await _display.WriteAsync(CurrentLatLon.Lon.ToString(CultureInfo.InvariantCulture), 2);
}