internal void RequestSetGaitType(GaitType gaitType)
{
_lastGaitType = _gaitType;
_gaitType = gaitType;
GaitSelect();
}
public Oscillator()
{
gait = GaitType.Tripod;
phase = new float[N_OSCILLATORS];
for (int i = 0; i < N_OSCILLATORS; ++i)
{
phase[i] = Random.Range(0, 2 * Mathf.PI);
}
UpdateGait();
}
private float[] GetGaitPhaseBias(GaitType g)
{
// 7 elements representing links:
// (1,2) (1, 3) (2, 4) (3, 4) (3, 5) (4, 6) (5, 6)
switch (g)
{
case GaitType.Metachronal:
return(new float[] { 1.0f, 1.0f / 3.0f, 1.0f / 3.0f, 1.0f, 1.0f / 3.0f, 1.0f / 3.0f, 1.0f });
case GaitType.Ripple:
return(new float[] { -1.0f, -3.0f / 2.0f, 1.0f / 2.0f, 1.0f, 1.0f / 2.0f, 1.0f / 2.0f, 1.0f });
case GaitType.Tripod:
return(new float[] { 1.0f, 1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f });
default:
Debug.LogWarning("Using default gait");
return(new float[] { 0, 0, 0, 0, 0, 0, 0 });
}
}
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;
}
}
internal void RequestSetGaitType(GaitType gaitType)
{
_inverseKinematics.RequestSetGaitType(gaitType);
}
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;
}
}
internal void RequestSetGaitType(GaitType gaitType)
{
_inverseKinematics.RequestSetGaitType(gaitType);
}
internal void RequestSetGaitType(GaitType gaitType)
{
_lastGaitType = _gaitType;
_gaitType = gaitType;
GaitSelect();
}
