internal async Task RequestSetFunctionAsync(SelectedIkFunction selectedIkFunction)
{
_inverseKinematics.RequestSetFunction(selectedIkFunction);
_selectedIkFunction = selectedIkFunction;
if (_selectedIkFunction == SelectedIkFunction.DisplayCoordinate)
{
await _gps.DisplayCoordinates();
}
}
internal IkController(InverseKinematics inverseKinematics,
SparkFunSerial16X2Lcd display,
IoTClient ioTClient,
Gps.Gps gps)
{
_inverseKinematics = inverseKinematics;
_display = display;
_ioTClient = ioTClient;
_gps = gps;
_perimeterInInches = 15;
_selectedIkFunction = SelectedIkFunction.BodyHeight;
_behavior = Behavior.Bounce;
}
internal IkController(InverseKinematics inverseKinematics,
SparkFunSerial16X2Lcd display,
IoTClient ioTClient,
Gps.Gps gps)
{
_inverseKinematics = inverseKinematics;
_display = display;
_ioTClient = ioTClient;
_gps = gps;
_perimeterInInches = 15;
_selectedIkFunction = SelectedIkFunction.BodyHeight;
_behavior = Behavior.Bounce;
}
internal void CalibrateFootHeight()
{
_selectedFunction = SelectedIkFunction.SetFootHeightOffset;
Task.Factory.StartNew(() =>
{
var height = 0;
for (var i = 0; i < 6; i++)
{
while (_legGpioPins[i].Read() != GpioPinValue.Low)
{
height += 2;
RequestLegYHeight(i, height);
}
}
_calibrated = 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;
}
}
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 RequestSetFunctionAsync(SelectedIkFunction selectedIkFunction)
{
_inverseKinematics.RequestSetFunction(selectedIkFunction);
_selectedIkFunction = selectedIkFunction;
if (_selectedIkFunction == SelectedIkFunction.DisplayCoordinate)
await _gps.DisplayCoordinates();
}
internal void CalibrateFootHeight()
{
_selectedFunction = SelectedIkFunction.SetFootHeightOffset;
Task.Factory.StartNew(() =>
{
var height = 0;
for (var i = 0; i < 6; i++)
{
while (_legGpioPins[i].Read() != GpioPinValue.Low)
{
height += 2;
RequestLegYHeight(i, height);
}
}
_calibrated = true;
});
}
internal void RequestSetFunction(SelectedIkFunction selectedIkFunction)
{
_selectedFunction = selectedIkFunction;
}
internal void RequestSetFunction(SelectedIkFunction selectedIkFunction)
{
_selectedFunction = selectedIkFunction;
}
