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); }