/// <summary>
/// This event handler is called when the robot/user sends a start message
/// The parameters can be set in the Skill Runner (or as json) and used in the skill if desired
/// </summary>
/// <param name="parameters"></param>
public void OnStart(object sender, IDictionary <string, object> parameters)
{
try
{
_misty.SendDebugMessage("MistyMiner skill started", null);
_misty.MoveHead(20, 0, 0, 0, AngularUnit.Position, null);
//_takeImageTimer = new Timer(CheckForOreCallback, null, 0, 5000);
_misty.RegisterCapTouchEvent(CheckForOreCallback, 1000, true, null, null, null);
_misty.RegisterIMUEvent(setYaw, 5, true, null, null, null);
List <ActuatorPositionValidation> HeadPositionValidation = new List <ActuatorPositionValidation>();
HeadPositionValidation.Add(new ActuatorPositionValidation {
Name = ActuatorPositionFilter.SensorName, Comparison = ComparisonOperator.Equal, ComparisonValue = ActuatorPosition.HeadPitch
});
_misty.RegisterActuatorEvent(setHeadPitch, 10, true, HeadPositionValidation, null, null);
List <ActuatorPositionValidation> ArmPositionValidation = new List <ActuatorPositionValidation>();
ArmPositionValidation.Add(new ActuatorPositionValidation {
Name = ActuatorPositionFilter.SensorName, Comparison = ComparisonOperator.Equal, ComparisonValue = ActuatorPosition.RightArm
});
_misty.RegisterActuatorEvent(setArmPitch, 100, true, ArmPositionValidation, null, null);
_misty.RegisterTimeOfFlightEvent(setTimeOfFlight, 0, true, null, null, null);
//Clean up any lingering runs
yaw.YawReached -= HandleYawReached;
yaw.YawReached -= HandleReturnYawReached;
yaw.YawReached -= FinalYaw;
tof.DistanceReached -= HandleOreReached;
tof.DistanceReached -= HandleReturnReached;
arm.ArmPosReached -= ArmDown;
arm.ArmPosReached -= ArmUp;
}
catch (Exception ex)
{
_misty.SkillLogger.Log($"MistyMiner : OnStart: => Exception", ex);
_misty.SendDebugMessage($"MistyMiner : OnStart: => Exception" + ex, null);
}
}
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);
}
/// <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);
}
