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