//checking if rear sonar detected passed the door
public void SonarDetector_OnDistanceChanged(object sender, SonarDistanceChangedEventArgs eventArgs)
{
for (int i = 0; i < eventArgs.Sonars.Count; i++)
{
SonarSensorInfo info = eventArgs.Sonars[i];
List <int> currentDistances = this.SonarDetector.currentDistances;
if (info.ID == i && isSonarDetectedDoorList[i] == false && info.ChangedDifference < 0 && currentDistances[i] < 40)
{
if (i == 0 || i == 3)
{
isSonarDetectedDoorList[i] = true;
}
else if (isSonarPassedList[i - 1] == true) //only changed this status when front sensor passed door
{
isSonarDetectedDoorList[i] = true;
}
}
if (info.ID == i && isSonarDetectedDoorList[i] == true && info.ChangedDifference > 0)
{
isSonarDetectedDoorList[i] = false;
if (isSonarPassedList[i] == false && isForward == true) //passed door
{
isSonarPassedList[i] = true;
}
if (isSonarPassedList[i] == true && isForward == false) //back inside
{
isSonarPassedList[i] = false;
}
}
}
if (step == 2)
{
if (IsNavigationSuceessfulPassedTheDoor())
{
NotifyNavigationEnded(true);
//stop subscribe sonar detector
_sonarDetector.OnDistanceBigChangedDetected -= SonarDetector_OnDistanceChanged;
_sonarDetector.OnObstacleTooClosed -= SonarDetector_OnObstacleTooClosed;
}
}
}
public void SonarDetector_OnObstacleTooClosed(object sender, SonarDistanceChangedEventArgs eventArgs)
{
//TODO: avoid to hit any obstacle
List <SonarSensorInfo> sonars = eventArgs.Sonars;
List <int> currentDistances = this.SonarDetector.currentDistances;
for (int i = 0; i < sonars.Count; i++)
{
if (sonars[i].ID == 0 || sonars[i].ID == 3) //front left sensor too close
{
//check other sonar sensor
if (currentDistances[0] < MIN_FRONT_SONAR_DISTANCE && currentDistances[3] < MIN_FRONT_SONAR_DISTANCE + RIGHT_OFFSET) //cant pass
{
NotifyNavigationEnded(false);
//stop subscribe door detector
_doorDetector.OnDoorDetected -= DoorDetector_OnDoorDetected;
}
else if (currentDistances[0] > MIN_FRONT_SONAR_DISTANCE && currentDistances[3] > MIN_FRONT_SONAR_DISTANCE + RIGHT_OFFSET)
{
sonarVector = new Vector(0, 1); // enough space keep forward
}
else if (currentDistances[0] < MIN_FRONT_SONAR_DISTANCE) //too left
{
sonarVector = new Vector(1, 0); //turn right
}
else if (currentDistances[3] < MIN_FRONT_SONAR_DISTANCE + RIGHT_OFFSET)
{
sonarVector = new Vector(-1, 0); //turn left
}
else
{
sonarVector = new Vector(0, 1); // enough space keep forward
}
}
if (sonars[i].ID == 1 || sonars[i].ID == 4) //front left sensor too close
{
//check other sonar sensor
if (currentDistances[1] < MIN_MID_SONAR_DISTANCE && currentDistances[4] < MIN_MID_SONAR_DISTANCE + RIGHT_OFFSET) //cant pass
{
NotifyNavigationEnded(false);
//stop subscribe door detector
_doorDetector.OnDoorDetected -= DoorDetector_OnDoorDetected;
}
else if (currentDistances[1] > MIN_MID_SONAR_DISTANCE && currentDistances[4] > MIN_MID_SONAR_DISTANCE + RIGHT_OFFSET)
{
sonarVector = new Vector(0, 1); // enough space keep forward
}
else if (currentDistances[1] < MIN_MID_SONAR_DISTANCE) //too left
{
if (currentDistances[3] < MIN_FRONT_SONAR_DISTANCE)
{
sonarVector = new Vector(-1, 1);
}
else
{
sonarVector = new Vector(1, 0); //turn right
}
}
else if (currentDistances[4] < MIN_MID_SONAR_DISTANCE + RIGHT_OFFSET)
{
if (currentDistances[0] < MIN_FRONT_SONAR_DISTANCE)
{
sonarVector = new Vector(1, 1);
}
else
{
sonarVector = new Vector(-1, 0); //turn left
}
}
else
{
sonarVector = new Vector(0, 1); // enough space keep forward
}
}
}
}
