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