/// <summary> /// Reads the data from the LIDAR, formats it and triggers an event /// </summary> /// <returns></returns> private async Task Read() { if (readings == null) { readings = new List <Hit>(); } try { byte[] readout = await ReadData(5, readingToken.Token); Hit currentReading = FormatData(readout); // Trigger event and clear current readings on new scan if (currentReading.NewScan) { OnScanCompleted(readings.ToArray()); readings.Clear(); } // Only add reading if no error if (!currentReading.Error) { readings.Add(currentReading); } else { Debug.WriteLine("Read Error -> " + currentReading.ToString()); await ClearReadBuffer(); } } catch (Exception ex) { Debug.WriteLine(ex.Message); } }
public void ProcessReadings(ref Hit[] readings, long timestamp) { // Only process one reading at a time lock (processingLock) { // This group of readings are older than the last group processed, so skip if (timestamp < LastProcessingTime) { return; } //Debug.WriteLine($"Processing {readings.Length} points, {timestamp}..."); var newRec = LidarRecommendation.Move; double adjustAngle = 0; var leftPoints = new List <Hit>(); var rightPoints = new List <Hit>(); // Do processing foreach (var reading in readings) { // Skip bad reading if (reading.Quality == 0) { continue; } // Convert to Cartesian coordiantes var point = reading.CartesianPoint; // Check if we need to stop because we're about to hit something if (IsInStopZone(reading.Angle, reading.Distance)) { // Set recommendation to stop so the robot doesn't keep trying to move newRec = LidarRecommendation.Stop; // Update the last processing time LastProcessingTime = timestamp; OnAnalysisChanged(newRec, adjustAngle); return; } // Check if we need to adjust trajectory if (IsInAdjustZone(reading.Angle, reading.Distance)) { if (point.X < 0) { leftPoints.Add(reading); } else { rightPoints.Add(reading); } } } // Check for obstacle if (leftPoints.Count > MIN_ADJUST_POINT_COUNT || rightPoints.Count > MIN_ADJUST_POINT_COUNT) { // Find the point that is closest to the robot var leftMinPoint = new Hit() { Distance = int.MaxValue }; var rightMinPoint = new Hit() { Distance = int.MaxValue }; foreach (var point in leftPoints) { if (point.Distance < leftMinPoint.Distance) { leftMinPoint = point; } } foreach (var point in rightPoints) { if (point.Distance < rightMinPoint.Distance) { rightMinPoint = point; } } // Check to see which side is closer to object if (leftMinPoint.Distance < rightMinPoint.Distance) { newRec = LidarRecommendation.AdjustObstacle; adjustAngle = 90; } else if (leftMinPoint.Distance > rightMinPoint.Distance) { newRec = LidarRecommendation.AdjustObstacle; adjustAngle = -90; } else { newRec = LidarRecommendation.Stop; } } // Update the last processing time LastProcessingTime = timestamp; OnAnalysisChanged(newRec, adjustAngle); } }