private static void InputReportReceived(HidDevice sender, HidInputReportReceivedEventArgs args)
{
var dPad = (int)args.Report.GetNumericControl(0x01, 0x39).Value;
var lstickX = args.Report.GetNumericControl(0x01, 0x30).Value - 32768;
var lstickY = args.Report.GetNumericControl(0x01, 0x31).Value - 32768;
var rstickX = args.Report.GetNumericControl(0x01, 0x33).Value - 32768;
var rstickY = args.Report.GetNumericControl(0x01, 0x34).Value - 32768;
var lt = (int)Math.Max(0, args.Report.GetNumericControl(0x01, 0x32).Value - 32768);
var rt = (int)Math.Max(0, -1 * (args.Report.GetNumericControl(0x01, 0x32).Value - 32768));
foreach (var btn in args.Report.ActivatedBooleanControls) //StartAsync = 7, Back = 6
{
var id = (int)(btn.Id - 5);
if (id < 4)
{
FunctionButtonChanged?.Invoke(id);
}
else if (id >= 4 && id < 6)
{
BumperButtonChanged?.Invoke(id);
}
else
{
FunctionButtonChanged?.Invoke(id);
}
}
if (_leftTrigger != lt)
{
LeftTriggerChanged?.Invoke(lt);
_leftTrigger = lt;
}
if (_rightTrigger != rt)
{
RightTriggerChanged?.Invoke(rt);
_rightTrigger = rt;
}
var lStickMagnitude = GetMagnitude(lstickX, lstickY);
var rStickMagnitude = GetMagnitude(rstickX, rstickY);
var vector = new ControllerVector
{
Direction = CoordinatesToDirection(lstickX, lstickY),
Magnitude = lStickMagnitude
};
if (!_leftStickDirectionVector.Equals(vector) && LeftDirectionChanged != null)
{
_leftStickDirectionVector = vector;
LeftDirectionChanged(_leftStickDirectionVector);
}
vector = new ControllerVector
{
Direction = CoordinatesToDirection(rstickX, rstickY),
Magnitude = rStickMagnitude
};
if (!_rightStickDirectionVector.Equals(vector) && RightDirectionChanged != null)
{
_rightStickDirectionVector = vector;
RightDirectionChanged(_rightStickDirectionVector);
}
vector = new ControllerVector
{
Direction = (ControllerDirection)dPad,
Magnitude = 10000
};
if (_dpadDirectionVector.Equals(vector) || DpadDirectionChanged == null)
{
return;
}
_dpadDirectionVector = vector;
DpadDirectionChanged(vector);
}
private static void InputReportReceived(HidDevice sender, HidInputReportReceivedEventArgs args)
{
var dPad = (int)args.Report.GetNumericControl(0x01, 0x39).Value;
var lstickX = args.Report.GetNumericControl(0x01, 0x30).Value - 32768;
var lstickY = args.Report.GetNumericControl(0x01, 0x31).Value - 32768;
var rstickX = args.Report.GetNumericControl(0x01, 0x33).Value - 32768;
var rstickY = args.Report.GetNumericControl(0x01, 0x34).Value - 32768;
var lt = (int)Math.Max(0, args.Report.GetNumericControl(0x01, 0x32).Value - 32768);
var rt = (int)Math.Max(0, -1 * (args.Report.GetNumericControl(0x01, 0x32).Value - 32768));
foreach (var btn in args.Report.ActivatedBooleanControls) //StartAsync = 7, Back = 6
{
var id = (int)(btn.Id - 5);
if (id < 4)
FunctionButtonChanged?.Invoke(id);
else if (id >= 4 && id < 6)
BumperButtonChanged?.Invoke(id);
else
FunctionButtonChanged?.Invoke(id);
}
if (_leftTrigger != lt)
{
LeftTriggerChanged?.Invoke(lt);
_leftTrigger = lt;
}
if (_rightTrigger != rt)
{
RightTriggerChanged?.Invoke(rt);
_rightTrigger = rt;
}
var lStickMagnitude = GetMagnitude(lstickX, lstickY);
var rStickMagnitude = GetMagnitude(rstickX, rstickY);
var vector = new ControllerVector
{
Direction = CoordinatesToDirection(lstickX, lstickY),
Magnitude = lStickMagnitude
};
if (!_leftStickDirectionVector.Equals(vector) && LeftDirectionChanged != null)
{
_leftStickDirectionVector = vector;
LeftDirectionChanged(_leftStickDirectionVector);
}
vector = new ControllerVector
{
Direction = CoordinatesToDirection(rstickX, rstickY),
Magnitude = rStickMagnitude
};
if (!_rightStickDirectionVector.Equals(vector) && RightDirectionChanged != null)
{
_rightStickDirectionVector = vector;
RightDirectionChanged(_rightStickDirectionVector);
}
vector = new ControllerVector
{
Direction = (ControllerDirection)dPad,
Magnitude = 10000
};
if (_dpadDirectionVector.Equals(vector) || DpadDirectionChanged == null)
return;
_dpadDirectionVector = vector;
DpadDirectionChanged(vector);
}
private void XboxController_LeftDirectionChanged(ControllerVector sender)
{
switch (_selectedIkFunction)
{
case SelectedIkFunction.TranslateHorizontal:
SetBodyHorizontalOffset(sender);
break;
case SelectedIkFunction.Translate3D:
SetBodyRot(sender);
break;
default:
SetBodyRot(sender);
break;
}
}
private void SetBodyHorizontalOffset(ControllerVector sender)
{
switch (sender.Direction)
{
case ControllerDirection.Left:
_bodyPosX = sender.Magnitude.Map(0, 10000, 0, 30);
_bodyPosZ = 0;
break;
case ControllerDirection.UpLeft:
_bodyPosX = sender.Magnitude.Map(0, 10000, 0, 30);
_bodyPosZ = -sender.Magnitude.Map(0, 10000, 0, 30);
break;
case ControllerDirection.UpRight:
_bodyPosX = sender.Magnitude.Map(0, 10000, 0, 30);
_bodyPosZ = sender.Magnitude.Map(0, 10000, 0, 30);
break;
case ControllerDirection.Right:
_bodyPosX = -sender.Magnitude.Map(0, 10000, 0, 30);
_bodyPosZ = 0;
break;
case ControllerDirection.Up:
_bodyPosX = sender.Magnitude.Map(0, 10000, 0, 30);
_bodyPosZ = 0;
break;
case ControllerDirection.Down:
_bodyPosX = -sender.Magnitude.Map(0, 10000, 0, 30);
_bodyPosZ = 0;
break;
case ControllerDirection.DownLeft:
_bodyPosZ = -sender.Magnitude.Map(0, 10000, 0, 30);
_bodyPosX = -sender.Magnitude.Map(0, 10000, 0, 30);
break;
case ControllerDirection.DownRight:
_bodyPosZ = sender.Magnitude.Map(0, 10000, 0, 30);
_bodyPosX = -sender.Magnitude.Map(0, 10000, 0, 30);
break;
}
_ik.RequestBodyPosition(_bodyRotX, _bodyRotZ, _bodyPosX, _bodyPosZ, _bodyPosY, _bodyRotY);
}
private void XboxController_RightDirectionChanged(ControllerVector sender)
{
switch (sender.Direction)
{
case ControllerDirection.Left:
_travelRotationY = -sender.Magnitude.Map(0, 10000, 0, TravelRotationYlimit);
_travelLengthZ = 0;
break;
case ControllerDirection.UpLeft:
_travelRotationY = -sender.Magnitude.Map(0, 10000, 0, TravelRotationYlimit);
_travelLengthZ = -sender.Magnitude.Map(0, 10000, 0, TravelLengthZlowerLimit);
break;
case ControllerDirection.DownLeft:
_travelRotationY = -sender.Magnitude.Map(0, 10000, 0, TravelRotationYlimit);
_travelLengthZ = sender.Magnitude.Map(0, 10000, 0, TravelLengthZlowerLimit); //110
break;
case ControllerDirection.Right:
_travelRotationY = sender.Magnitude.Map(0, 10000, 0, TravelRotationYlimit); //3
_travelLengthZ = 0;
break;
case ControllerDirection.UpRight:
_travelRotationY = sender.Magnitude.Map(0, 10000, 0, TravelRotationYlimit);
_travelLengthZ = -sender.Magnitude.Map(0, 10000, 0, TravelLengthZupperLimit); //190
break;
case ControllerDirection.DownRight:
_travelRotationY = sender.Magnitude.Map(0, 10000, 0, TravelRotationYlimit);
_travelLengthZ = sender.Magnitude.Map(0, 10000, 0, TravelLengthZlowerLimit);
break;
case ControllerDirection.Up:
_travelLengthZ = -sender.Magnitude.Map(0, 10000, 0, TravelLengthZupperLimit);
_travelRotationY = 0;
break;
case ControllerDirection.Down:
_travelLengthZ = sender.Magnitude.Map(0, 10000, 0, TravelLengthZupperLimit);
_travelRotationY = 0;
break;
}
_ik.RequestMovement(_gaitSpeed, _travelLengthX, _travelLengthZ, _travelRotationY);
}
private async void XboxController_DpadDirectionChanged(ControllerVector sender)
{
if (_dpadStopwatch.ElapsedMilliseconds > 250)
{
_dpadStopwatch.Restart();
}
else
return;
switch (sender.Direction)
{
case ControllerDirection.Left:
switch (_selectedIkFunction)
{
case SelectedIkFunction.Behavior:
_selectedBehavior--;
if ((int)_selectedBehavior <= 0)
_selectedBehavior = 0;
await _display.WriteAsync($"{Enum.GetName(typeof(Behavior), _selectedBehavior)}", 2);
break;
case SelectedIkFunction.GaitType:
_gaitType--;
if (_gaitType < 0)
_gaitType = (GaitType)4;
await _display.WriteAsync(Enum.GetName(typeof(GaitType), _gaitType), 2);
SetGaitOptions();
break;
case SelectedIkFunction.GaitSpeed:
_gaitSpeed = _gaitSpeed - 2;
if (_gaitSpeed < GaitSpeedMin)
_gaitSpeed = GaitSpeedMin;
_ik.RequestSetGaitOptions(_gaitSpeed, _legLiftHeight);
await _display.WriteAsync($"_gaitSpeed = {_gaitSpeed}", 2);
break;
case SelectedIkFunction.SetFootHeightOffset:
_selectedLeg--;
if (_selectedLeg < 0)
_selectedLeg = 0;
await _display.WriteAsync($"_selectedLeg = {_selectedLeg}", 2);
break;
}
break;
case ControllerDirection.Right:
switch (_selectedIkFunction)
{
case SelectedIkFunction.Behavior:
_selectedBehavior++;
if ((int)_selectedBehavior > 4)
_selectedBehavior = (Behavior)4;
await _display.WriteAsync($"{Enum.GetName(typeof(Behavior), _selectedBehavior)}", 2);
break;
case SelectedIkFunction.GaitType:
_gaitType++;
if ((int)_gaitType > 4)
_gaitType = 0;
await _display.WriteAsync(Enum.GetName(typeof(GaitType), _gaitType), 2);
SetGaitOptions();
break;
case SelectedIkFunction.GaitSpeed:
_gaitSpeed = _gaitSpeed + 2;
if (_gaitSpeed > GaitSpeedMax)
_gaitSpeed = GaitSpeedMax;
_ik.RequestSetGaitOptions(_gaitSpeed, _legLiftHeight);
await _display.WriteAsync($"_gaitSpeed = {_gaitSpeed}", 2);
break;
case SelectedIkFunction.SetFootHeightOffset:
_selectedLeg++;
if (_selectedLeg == 5)
_selectedLeg = 5;
await _display.WriteAsync($"_selectedLeg = {_selectedLeg}", 2);
break;
}
break;
case ControllerDirection.Up:
switch (_selectedIkFunction)
{
case SelectedIkFunction.LegLiftHeight:
_legLiftHeight++;
if (_legLiftHeight > LegLiftHeightUpperLimit)
_legLiftHeight = LegLiftHeightUpperLimit;
await _display.WriteAsync($"Height = {_legLiftHeight}", 2);
_ik.RequestSetGaitOptions(_gaitSpeed, _legLiftHeight);
break;
case SelectedIkFunction.SetFootHeightOffset:
_legPosY = _legPosY + 1;
_ik.RequestLegYHeight(_selectedLeg, _legPosY);
break;
case SelectedIkFunction.PingSetup:
_ik.RequestNewPerimeter(true);
break;
case SelectedIkFunction.BodyHeight:
_bodyPosY = _bodyPosY + 5;
if (_bodyPosY > 110)
_bodyPosY = 110;
_ik.RequestBodyPosition(_bodyRotX, _bodyRotZ, _bodyPosX, _bodyPosZ, _bodyPosY, _bodyRotY);
await _display.WriteAsync($"_bodyPosY = {_bodyPosY}", 2);
break;
case SelectedIkFunction.Posture:
_posture++;
await SetPosture();
break;
case SelectedIkFunction.Behavior:
_ik.RequestBehavior(_selectedBehavior, true);
await _display.WriteAsync($"{nameof(_selectedBehavior)} start");
break;
}
break;
case ControllerDirection.Down:
switch (_selectedIkFunction)
{
case SelectedIkFunction.LegLiftHeight:
_legLiftHeight--;
if (_legLiftHeight < LegLiftHeightLowerLimit)
_legLiftHeight = LegLiftHeightLowerLimit;
await _display.WriteAsync($"Height = {_legLiftHeight}", 2);
_ik.RequestSetGaitOptions(_gaitSpeed, _legLiftHeight);
break;
case SelectedIkFunction.SetFootHeightOffset:
_legPosY = _legPosY - 1;
_ik.RequestLegYHeight(_selectedLeg, _legPosY);
break;
case SelectedIkFunction.PingSetup:
_ik.RequestNewPerimeter(false);
break;
case SelectedIkFunction.BodyHeight:
_bodyPosY = _bodyPosY - 5;
if (_bodyPosY < 10)
_bodyPosY = 10;
_ik.RequestBodyPosition(_bodyRotX, _bodyRotZ, _bodyPosX, _bodyPosZ, _bodyPosY, _bodyRotY);
await _display.WriteAsync($"_bodyPosY = {_bodyPosY}", 2);
break;
case SelectedIkFunction.Posture:
_posture--;
await SetPosture();
break;
case SelectedIkFunction.Behavior:
_ik.RequestBehavior(_selectedBehavior, false);
await _display.WriteAsync($"{nameof(_selectedBehavior)} stop");
break;
}
break;
}
}
