//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 } } } }