// Sends a rotate command to the robot and returns right away // The robot will go forward until it hits a dead end or until // it's crossed intersectionGoal intersections. This async call // will allow the thread to filter for intersection detected messages. // intersectionGoal = 0 for unlimited private void goStraightAsync(int intersectionGoal) { byte[] filterHead = { 0xEE }; StringBuilder buf = new StringBuilder(); buf.Append("PID_LINE "); buf.Append(intersectionGoal + " "); buf.Append(straightPowerHigh + " "); buf.Append(straightPowerLow + " "); buf.Append(straightLowPowerZone + " "); buf.Append(straightKP + " "); buf.Append(straightKI + " "); buf.Append(straightKD + " "); buf.Append(dampingFactor + " "); buf.Append(iSectDetectDelay); RobotCommand c = new RobotCommand(buf.ToString()); PacketFilter filter = new PacketFilter(link, filterHead, c.Sequence); filter.init(); link.sendCommand(c); this.lastPacketFilter = filter; }
// 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; } } }