//update current distance data void updateDistanceData() { for (int i = 0; i < sensorNum; i++) { try { int distance = sensorArray.SonarArrayDevice.getDistanceAt(i); if (distance < 6) { distance = historicalDistances[i]; } currentDistances[i] = distance; //Console.WriteLine("----------------------------------------"); //Console.WriteLine("*** Output current distances ***"); //Console.WriteLine("Sonar sensor " + i.ToString() + ":"); //Console.WriteLine(distance.ToString()); //Console.WriteLine("----------------------------------------"); } catch (Exception e) { } } //check if any sonar too closed to the door for (int i = 0; i < sensorNum; i++) { Console.WriteLine("----------------------------------------"); Console.WriteLine("*** Output current distances ***"); Console.WriteLine("Sonar sensor " + i.ToString() + ":"); Console.WriteLine(currentDistances[i].ToString()); Console.WriteLine("----------------------------------------"); if ((currentDistances[i] <= MIN_DISTANCE) || (currentDistances[i] > MIN_DISTANCE && isSensorDetectedObstacle[i] == true)) { if (currentDistances[i] <= MIN_DISTANCE) { isSensorDetectedObstacle[i] = true; } else { isSensorDetectedObstacle[i] = false; } List <SonarSensorInfo> sensors = new List <SonarSensorInfo>(); sensors.Add(new SonarSensorInfo(i, currentDistances[i], 0)); SonarDistanceChangedEventArgs args = new SonarDistanceChangedEventArgs(sensors); if (OnObstacleTooClosed != null) { OnObstacleTooClosed(this, args); } } } }
//check if any sonar sensor sensed a big change of distance void MonitorSonarSensor() { List <SonarSensorInfo> sensors = GetBigDistanceChangedSensor(); if (sensors.Count > 0) { SonarDistanceChangedEventArgs args = new SonarDistanceChangedEventArgs(sensors); if (OnDistanceBigChangedDetected != null) { OnDistanceBigChangedDetected(this, args); } } }