public void TOFBRangeCallback(ITimeOfFlightEvent tofEvent)
{
if (TryGetAdjustedDistance(tofEvent, out double distance))
{
_currentObstacleState.BackTOF = distance;
}
}
private void FrontEdgeCallback(ITimeOfFlightEvent edgeEvent)
{
switch (edgeEvent.SensorPosition)
{
case TimeOfFlightPosition.DownwardFrontRight:
_currentObstacleState.FrontRightEdgeTOF = edgeEvent.DistanceInMeters;
break;
case TimeOfFlightPosition.DownwardFrontLeft:
_currentObstacleState.FrontLeftEdgeTOF = edgeEvent.DistanceInMeters;
break;
}
}
private void TofEventCallback(ITimeOfFlightEvent eventResponse)
{
switch (eventResponse.SensorPosition)
{
case MistyRobotics.Common.Types.TimeOfFlightPosition.FrontLeft:
if (eventResponse.Status == 0)
{
_tofValues.FrontLeft = eventResponse.DistanceInMeters;
}
else
{
_tofValues.FrontLeft = 5;
}
break;
case MistyRobotics.Common.Types.TimeOfFlightPosition.FrontCenter:
if (eventResponse.Status == 0)
{
_tofValues.FrontCenter = eventResponse.DistanceInMeters;
}
else
{
_tofValues.FrontCenter = 5;
}
break;
case MistyRobotics.Common.Types.TimeOfFlightPosition.FrontRight:
if (eventResponse.Status == 0)
{
_tofValues.FrontRight = eventResponse.DistanceInMeters;
}
else
{
_tofValues.FrontRight = 5;
}
break;
case MistyRobotics.Common.Types.TimeOfFlightPosition.Back:
if (eventResponse.Status == 0)
{
_tofValues.Back = eventResponse.DistanceInMeters;
}
else
{
_tofValues.Back = 5;
}
break;
}
}
/// <summary>
/// Callback called when there is a new TimrOfFlight event
/// </summary>
/// <param name="tofEvent"></param>
private void TOFRangeCallback(ITimeOfFlightEvent tofEvent)
{
_misty.SkillLogger.Log($"{(tofEvent.Status != 0 ? "WARNING!! " : "")}BackAndForthSkill : TOFRangeCallback {tofEvent.SensorPosition.ToString()} - Status:{tofEvent.Status} - Meters:{tofEvent.DistanceInMeters}");
switch (tofEvent.SensorPosition)
{
case TimeOfFlightPosition.FrontLeft:
case TimeOfFlightPosition.FrontRight:
case TimeOfFlightPosition.FrontCenter:
if (tofEvent.Status == 0)
{
if (tofEvent.DistanceInMeters <= 0.05)
{
_misty.DriveTime(-25, 0, 1000, null);
}
else if (tofEvent.DistanceInMeters <= 0.1)
{
_misty.DriveTime(-15, 0, 1000, null);
}
else if (tofEvent.DistanceInMeters <= 0.15)
{
_misty.DriveTime(-10, 0, 1000, null);
}
_misty.MoveArms(-60, -60, 100, 100, null, AngularUnit.Degrees, null);
}
break;
case TimeOfFlightPosition.Back:
if (tofEvent.Status == 0)
{
if (tofEvent.DistanceInMeters <= 0.05)
{
_misty.DriveTime(25, 0, 1000, null);
}
if (tofEvent.DistanceInMeters <= 0.1)
{
_misty.DriveTime(15, 0, 1000, null);
}
if (tofEvent.DistanceInMeters <= 0.15)
{
_misty.DriveTime(10, 0, 1000, null);
}
_misty.MoveArms(-30, -30, 100, 100, null, AngularUnit.Degrees, null);
}
break;
}
}
/// <summary>
/// Callback called when there is a new TimrOfFlight event
/// </summary>
/// <param name="tofEvent"></param>
private void TOFRangeCallback(ITimeOfFlightEvent tofEvent)
{
_misty.SkillLogger.Log($"HelloLocomotionSkill : TOFRangeCallback {tofEvent.SensorPosition.ToString()} {tofEvent.DistanceInMeters} - goingforward = {_goingForward}");
if (tofEvent.DistanceInMeters <= 0.1 && tofEvent.Status == 0)
{
if (_goingForward &&
(tofEvent.SensorPosition == TimeOfFlightPosition.FrontLeft ||
tofEvent.SensorPosition == TimeOfFlightPosition.FrontRight ||
tofEvent.SensorPosition == TimeOfFlightPosition.FrontCenter))
{
_misty.SkillLogger.Log($"HelloLocomotionSkill : Driving forward attempt while something in front");
_misty.Stop(null);
}
else if (!_goingForward && tofEvent.SensorPosition == TimeOfFlightPosition.Back)
{
_misty.SkillLogger.Log($"HelloLocomotionSkill : Driving backward attempt while something behind");
_misty.Stop(null);
}
}
}
private void setTimeOfFlight(ITimeOfFlightEvent info)
{
switch (info.SensorPosition)
{
case TimeOfFlightPosition.FrontCenter:
tof.SetCenterFront(info.DistanceInMeters, info.Status);
break;
case TimeOfFlightPosition.FrontLeft:
tof.SetLeftFront(info.DistanceInMeters, info.Status);
break;
case TimeOfFlightPosition.FrontRight:
tof.SetRightFront(info.DistanceInMeters, info.Status);
break;
case TimeOfFlightPosition.Back:
tof.SetRearCenter(info.DistanceInMeters, info.Status);
break;
}
}
private bool TryGetAdjustedDistance(ITimeOfFlightEvent tofEvent, out double distance)
{
distance = 0;
// 0 = valid range data
// 101 = sigma fail - lower confidence but most likely good
// 104 = Out of bounds - Distance returned is greater than distance we are confident about, but most likely good
if (tofEvent.Status == 0 || tofEvent.Status == 101 || tofEvent.Status == 104)
{
distance = tofEvent.DistanceInMeters;
}
else if (tofEvent.Status == 102)
{
//102 generally indicates nothing substantial is in front of the robot so the TOF is returning the floor as a close distance
//So ignore the disance returned and just set to 2 meters
distance = 2;
}
else
{
//TOF returning uncertain data or really low confidence in distance, ignore value
return(false);
}
return(true);
}
