// Blocks until an intersection is reached and then returns the intersection details private bool[] getNextIntersection(PacketFilter filter) { while(true) { BluetoothPacket p = filter.nextPacket(); if(p == null) return null; if (p.data[1] == 0x44) { dataPoints.Add(p); } else if (p.data[1] == 0x66) // Dead end { bool[] intersectionType = { false, false, false }; short leftWheel = BitConverter.ToInt16(p.data, 8); short rightWheel = BitConverter.ToInt16(p.data, 10); worker.ReportProgress(2, "Dead end reached"); return intersectionType; } else if (p.data[1] == 0x55) { ushort left = BitConverter.ToUInt16(p.data, 2); ushort center = BitConverter.ToUInt16(p.data, 4); ushort right = BitConverter.ToUInt16(p.data, 6); short leftWheel = BitConverter.ToInt16(p.data, 8); short rightWheel = BitConverter.ToInt16(p.data, 10); uint ticks = BitConverter.ToUInt32(p.data, 12); ushort deltaTick = BitConverter.ToUInt16(p.data, 16); short intersectionError = BitConverter.ToInt16(p.data, 18); short intersectionNormError = BitConverter.ToInt16(p.data, 22); double intersectionClassifier = (double)intersectionNormError / (double)deltaTick * intersectionConstant; bool[] intersectionType = { false, false, false }; if (intersectionClassifier > 1.0) // Right { intersectionType[RIGHT] = true; } else if (intersectionClassifier < -1.0) // Left { intersectionType[LEFT] = true; } else { intersectionType[LEFT] = true; intersectionType[RIGHT] = true; } ushort centerThresh = (ushort)((calibration.SensorsHigh[CalibrationControl.CENTER_SENSOR] + calibration.SensorsLow[CalibrationControl.CENTER_SENSOR]) / 2); if (center < centerThresh) { intersectionType[STRAIGHT] = true; } // Diagnostic / monitoring info worker.ReportProgress(2, Environment.NewLine + "==INTERSECTION DETECTED===" + Environment.NewLine + "Left Sensor: " + left + Environment.NewLine + "Right Sensor: " + right + Environment.NewLine + "Center Sensor," + center + Environment.NewLine + "Left Wheel: " + leftWheel + Environment.NewLine + "Right Wheel: " + rightWheel + Environment.NewLine + "Ticks: " + ticks + Environment.NewLine + "Intersection Ticks: " + deltaTick + Environment.NewLine + "Intersection Error: " + intersectionError + Environment.NewLine + "Equalized Intersection Error: " + intersectionNormError + Environment.NewLine + "Classifier Value: " + intersectionClassifier + Environment.NewLine + "Type: " + (intersectionType[LEFT] ? "L" : "") + (intersectionType[STRAIGHT] ? "S" : "") + (intersectionType[RIGHT] ? "R" : "") + Environment.NewLine); return intersectionType; } } }