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;
}
if (_headPosition.Pitch.HasValue && _headPosition.Yaw.HasValue && _headPosition.Roll.HasValue)
{
_headPositionSemaphore?.Release();
}
}
public async Task MoveHeadAsync(double pitchDegrees, double rollDegrees, double yawDegrees)
{
_misty.RegisterActuatorEvent(ActuatorEventCallback, 0, true, null, "SkillHelperActuatorEventCallback", OnResponse);
// We move head to non-final position first so that we can verify change in position.
double firstPitchDegrees = pitchDegrees + 10;
if (pitchDegrees > 0)
{
firstPitchDegrees = pitchDegrees - 10;
}
_misty.MoveHead(firstPitchDegrees, rollDegrees, yawDegrees, 70, MistyRobotics.Common.Types.AngularUnit.Degrees, OnResponse);
await Task.Delay(3000);
_headPosition = new HeadPosition();
_headPositionSemaphore = new SemaphoreSlim(0);
await _headPositionSemaphore.WaitAsync(5000);
double initPitch = _headPosition.Pitch.HasValue ? _headPosition.Pitch.Value : 100;
LogMessage($"Head position after pre-move: {_headPosition.Pitch:f2}, {_headPosition.Roll:f2}, {_headPosition.Yaw:f2}.");
// Now move head to final position.
_headPosition = new HeadPosition();
int retries = 0;
while ((!_headPosition.Pitch.HasValue || (_headPosition.Pitch.HasValue && initPitch != 100 && Math.Abs(_headPosition.Pitch.Value - initPitch) < 2)) && retries++ < 3)
{
_misty.MoveHead(pitchDegrees, rollDegrees, yawDegrees, 70, MistyRobotics.Common.Types.AngularUnit.Degrees, OnResponse);
await Task.Delay(5000);
if (_abort)
{
break;
}
_headPositionSemaphore = new SemaphoreSlim(0);
await _headPositionSemaphore.WaitAsync(5000);
LogMessage($"Head position after move: {_headPosition.Pitch:f2}, {_headPosition.Roll:f2}, {_headPosition.Yaw:f2}.");
}
_misty.UnregisterEvent("SkillHelperActuatorEventCallback", OnResponse);
}
