private void ActuatorEventCallback(IActuatorEvent eventResponse)
{
if (_headPosition == null)
{
_headPosition = new HeadPosition();
}
switch (eventResponse.SensorPosition)
{
case MistyRobotics.Common.Types.ActuatorPosition.HeadPitch:
_headPosition.Pitch = eventResponse.ActuatorValue;
break;
case MistyRobotics.Common.Types.ActuatorPosition.HeadYaw:
_headPosition.Yaw = eventResponse.ActuatorValue;
break;
case MistyRobotics.Common.Types.ActuatorPosition.HeadRoll:
_headPosition.Roll = eventResponse.ActuatorValue;
break;
}
}
/// <summary>
/// Move head with retries if pitch isn't where we want it to be.
/// </summary>
public async Task MoveHeadAsync(double pitchDegrees, double rollDegrees, double yawDegrees)
{
_misty.RegisterActuatorEvent(ActuatorEventCallback, 0, true, null, "SkillHelperActuatorEventCallback", OnResponse);
LogMessage($"Move head: {pitchDegrees:f0}, {rollDegrees:f0}, {yawDegrees:f0}.");
_headPosition = new HeadPosition();
int retries = 0;
while ((!_headPosition.Pitch.HasValue || Math.Abs(_headPosition.Pitch.Value - pitchDegrees) > 3) && retries++ < 3)
{
_misty.MoveHead(pitchDegrees, rollDegrees, yawDegrees, 70, MistyRobotics.Common.Types.AngularUnit.Degrees, OnResponse);
await Task.Delay(3000); // totally arbitrary wait
if (_abort)
{
break;
}
}
LogMessage($"Head position after move: {_headPosition.Pitch:f0}, {_headPosition.Roll:f0}, {_headPosition.Yaw:f0}.");
_misty.UnregisterEvent("SkillHelperActuatorEventCallback", OnResponse);
}
