/// <summary>
/// Create a 3X3 grid of paper colors using a source image and the
/// contours for each paper color.
/// </summary>
/// <param name="contourImage">Source image.</param>
/// <param name="yellowContours">Yellow paper contours.</param>
/// <param name="greenContours">Green paper contours.</param>
/// <returns></returns>
public Paper[,] detectColumnPaperColors(Image<Bgr, byte> contourImage, List<Contour<Point>> yellowContours, List<Contour<Point>> greenContours)
{
Paper[,] papers = new Paper[3,3];
for (int i = 0; i < papers.GetLength(0); i++)
{
for(int j = 0; j < papers.GetLength(1); j++)
{
papers[i, j] = new Paper();
papers[i, j].Color = PaperColor.UNKNOWN;
}
}
detectColumns(contourImage, papers, yellowContours, PaperColor.YELLOW);
detectColumns(contourImage, papers, greenContours, PaperColor.GREEN);
return papers;
}
/// <summary>
/// Adds paper colors from a contour into the grid.
/// </summary>
/// <param name="contourImage">Source image.</param>
/// <param name="papers">Grid to add paper colors to.</param>
/// <param name="paperContours">Contours to add to grid.</param>
/// <param name="paperColor">Color of paper to add to grid.</param>
private void detectColumns(Image<Bgr, byte> contourImage, Paper[,] papers, List<Contour<Point>> paperContours, PaperColor paperColor)
{
foreach (Contour<Point> paperContour in paperContours)
{
int x;
int xMidPoint = paperContour.BoundingRectangle.X + paperContour.BoundingRectangle.Width / 2;
if (xMidPoint < contourImage.Width * LEFT_WIDTH_THRESHOLD)
{
x = 0;
}
else if (xMidPoint > contourImage.Width * RIGHT_WIDTH_THRESHOLD)
{
x = 2;
}
else
{
x = 1;
}
int y;
int yMidPoint = paperContour.BoundingRectangle.Y + paperContour.BoundingRectangle.Height / 2;
if (yMidPoint < contourImage.Height * TOP_HEIGHT_THRESHOLD)
{
y = 0;
} else if(yMidPoint > contourImage.Height * BOTTOM_HEIGHT_THRESHOLD)
{
y = 2;
}
else
{
y = 1;
}
Paper currentPaper = papers[y, x];
currentPaper.Color = paperColor;
currentPaper.XMidPoint = xMidPoint;
currentPaper.YMidPoint = yMidPoint;
currentPaper.ParentImageWidth = contourImage.Width;
currentPaper.ParentImageHeight = contourImage.Height;
}
}
/// <summary>
/// Do nothing.
/// See <see cref="State.WAIT_FOR_RUN"/>.
/// </summary>
/// <param name="papers">Current Paper Grid</param>
/// <returns>Next State</returns>
private State waitForRun(Paper[,] papers)
{
Stop();
this.paperColumns = new List<PaperColor[]>();
return State.WAIT_FOR_RUN;
}
/// <summary>
/// Steer to move to the next column.
/// See <see cref="State.STEER_TO_MOVE"/>.
/// </summary>
/// <remarks>This always applies a static
/// steering correction when moving left because the robot would
/// not turn adequately under testing. This should be revisited
/// for specific applications.</remarks>
/// <param name="papers">Current Paper Grid</param>
/// <returns><see cref="State.STEER_ANGLE_CORRECTION"/></returns>
private State steerToMove(Paper[,] papers)
{
if (this.direction == Direction.LEFT)
{
this.motor.turn90DegreesLeft();
this.motor.turn3DegreesLeft();
}
else
{
this.motor.turn90DegreesRight();
}
return State.STEER_ANGLE_CORRECTION;
}
/// <summary>
/// Steer to view the papers in a column.
/// See <see cref="State.STEER_TO_DETECT"/>.
/// </summary>
/// <param name="papers">Current Paper Grid</param>
/// <returns><see cref="State.COLUMN_ANGLE_CORRECTION"/></returns>
private State steerToDetect(Paper[,] papers)
{
if(this.direction == Direction.LEFT)
{
this.motor.turn90DegreesRight();
}
else
{
this.motor.turn90DegreesLeft();
}
return State.COLUMN_ANGLE_CORRECTION;
}
/// <summary>
/// Perform a corrective turn, so the robot drives perpendicular
/// to the paper.
/// See <see cref="State.STEER_ANGLE_CORRECTION"/>.
/// </summary>
/// <param name="papers">Current Paper Grid</param>
/// <returns><see cref="State.STEER_ANGLE_CORRECTION"/> if a
/// correction is made. <see cref="State.MOVE_FORWARD"/> if
/// no correction was made.</returns>
private State steerAngleCorrection(Paper[,] papers)
{
if (this.direction == Direction.LEFT)
{
if (papers[2, 2].Color != PaperColor.UNKNOWN)
{
this.motor.turn3DegreesLeft();
return State.STEER_ANGLE_CORRECTION;
}
}
else
{
if (papers[2, 0].Color != PaperColor.UNKNOWN)
{
this.motor.turn3DegreesRight();
return State.STEER_ANGLE_CORRECTION;
}
}
return State.MOVE_FORWARD;
}
/// <summary>
/// Steer 720 Degrees to indicate that the digit has been
/// detected.
/// See <see cref="State.WAIT_FOR_RUN"/>.
/// </summary>
/// <param name="papers">Current Paper Grid</param>
/// <returns></returns>
private State steer720Degrees(Paper[,] papers)
{
// 8 90 degree turns is 720 degrees
for(int i = 0; i < 8; i++)
{
this.motor.turn90DegreesRight();
}
return State.WAIT_FOR_RUN;
}
/// <summary>
/// Drives forward slightly if the robot did not travel
/// an adequate distance.
/// See <see cref="State.MOVE_FORWARD_CORRECTION"/>.
/// </summary>
/// <param name="papers">Current Paper Grid</param>
/// <returns><see cref="State.MOVE_FORWARD_CORRECTION"/>
/// if a correction is made, otherwise
/// <see cref="State.STEER_TO_DETECT"/></returns>
private State moveForwardCorrection(Paper[,] papers)
{
/*
Paper paper;
if (this.direction == Direction.LEFT)
{
paper = papers[1, 2];
}
else
{
paper = papers[1, 0];
}
if (paper.Color != PaperColor.UNKNOWN)
{
if (paper.YMidPoint < paper.ParentImageHeight * FORWARD_CORRECTION_VERTICAL_THRESHOLD)
{
this.motor.driveForwardCorrection();
return State.MOVE_FORWARD_CORRECTION;
}
}
*/
return State.STEER_TO_DETECT;
}
/// <summary>
/// Drives forward to the next column.
/// See <see cref="State.MOVE_FORWARD"/>.
/// </summary>
/// <remarks>A static correction is always added when the robot is
/// moving right to compensate for uneven motor speeds.</remarks>
/// <param name="papers">Current Paper Grid</param>
/// <returns><see cref="State.MOVE_FORWARD_CORRECTION"/></returns>
private State moveForward(Paper[,] papers)
{
this.motor.driveForward();
if (this.direction == Direction.RIGHT)
{
this.motor.driveForwardCorrection();
this.motor.driveForwardCorrection();
this.motor.driveForwardCorrection();
}
return State.MOVE_FORWARD_CORRECTION;
}
/// <summary>
/// Detects and stores the center columns in the current paper grid.
/// See <see cref="State.COLUMN_DETECTION"/>.
/// </summary>
/// <param name="papers">Current Paper Grid</param>
/// <returns><see cref="State.STEER_TO_MOVE"/> if there are more
/// columns. <see cref="State.CALCULATE_DIGIT"/> if all columns
/// have now been checked.</returns>
private State columnDetection(Paper[,] papers)
{
PaperColor[] paperColorColumn = new PaperColor[3];
// Top Column
paperColorColumn[0] = papers[0, 1].Color;
// Middle Column
paperColorColumn[1] = papers[1, 1].Color;
// Bottom Column
paperColorColumn[2] = papers[2, 1].Color;
this.paperColumns.Add(paperColorColumn);
if (this.paperColumns.Count < COLUMN_COUNT)
{
return State.STEER_TO_MOVE;
}
else
{
return State.CALCULATE_DIGIT;
}
}
/// <summary>
/// Checks of the column is centered in the robot's field of
/// vision and makes corrective turns if it isn't.
/// See <see cref="State.COLUMN_ANGLE_CORRECTION"/>.
/// </summary>
/// <param name="papers">Current Paper Grid</param>
/// <returns><see cref="State.COLUMN_DETECTION"/> if the column
/// is centered, otherwise <see cref="State.COLUMN_DETECTION"/>.</returns>
private State columnAngleCorrection(Paper[,] papers)
{
Paper topCenterCellPaper = papers[0,1];
if(topCenterCellPaper.XMidPoint < topCenterCellPaper.ParentImageWidth * COLUMN_ANGLE_LEFT_CORRECTION_THRESHOLD)
{
this.motor.turn3DegreesLeft();
}
else if (topCenterCellPaper.XMidPoint > topCenterCellPaper.ParentImageWidth * COLUMN_ANGLE_RIGHT_CORRECTION_THRESHOLD)
{
this.motor.turn3DegreesRight();
} else
{
return State.COLUMN_DETECTION;
}
return State.COLUMN_ANGLE_CORRECTION;
}
/// <summary>
/// Check what side of the robot the columns are on and
/// sets the direction used throughout the state machine.
/// See <see cref="State.CHECK_DIRECTION"/>.
/// </summary>
/// <param name="papers">Current Paper Grid</param>
/// <returns><see cref="State.COLUMN_ANGLE_CORRECTION"/></returns>
private State checkDirection(Paper[,] papers)
{
int leftPaperCount = 0;
int rightPaperCount = 0;
for(int i = 0; i < papers.GetLength(0); i++)
{
for(int j = 0; j < papers.GetLength(1); j++)
{
if(papers[i,j].Color != PaperColor.UNKNOWN)
{
if(j == 0)
{
leftPaperCount++;
} else if(j == 2)
{
rightPaperCount++;
}
}
}
if (leftPaperCount > rightPaperCount)
{
this.direction = Direction.LEFT;
}
else
{
this.direction = Direction.RIGHT;
}
}
return State.COLUMN_ANGLE_CORRECTION;
}
/// <summary>
/// Calculates the digit represented by all of the
/// paper color columns. The detected digit is sent
/// to the UI using
/// <see cref="IDigitDetectionCallback.DigitDetected(int, PaperColor[])"/>.
/// </summary>
/// <param name="papers">Current Paper Grid</param>
/// <returns><see cref="State.STEER_720_DEGREES"/></returns>
private State calculateDigit(Paper[,] papers)
{
// The columns are read top to bottom; however, the digit needs to
// be bottom to top, which translate into left to right when the
// digit is compared.
for (int i = 0; i < this.paperColumns.Count; i++)
{
PaperColor temp = this.paperColumns[i][0];
this.paperColumns[i][0] = this.paperColumns[i][2];
this.paperColumns[i][2] = temp;
}
if (this.direction == Direction.LEFT)
{
this.paperColumns.Reverse();
}
// Convert paper color columns to an array
PaperColor[] paperColors = new PaperColor[this.paperColumns.Count * 3];
for (int i = 0; i < paperColumns.Count; i++)
{
for(int j = 0; j < 3; j++)
{
paperColors[i * papers.GetLength(0) + j] = this.paperColumns[i][j];
}
}
int matchNumber = 0;
int matchCount = 0;
// Find the digit that is the closest match. The idea behind
// this algorithm is that the closest match is the digit with
// the most papers in common with the digit represented by
// the paper viewed by the robot. In the event that the robot
// is unable to read a sheet of paper, this algorithm assumes
// that it would have been a match.
for(int i = 0; i < this.digitsToMatch.GetLength(0); i++)
{
int currentMatchCount = 0;
for(int j = 0; j < this.digitsToMatch.GetLength(1); j++)
{
if(paperColors[j] == PaperColor.UNKNOWN)
{
currentMatchCount++;
} else if(paperColors[j] == this.digitsToMatch[i, j])
{
currentMatchCount++;
}
}
if(currentMatchCount > matchCount)
{
matchNumber = i;
matchCount = currentMatchCount;
}
}
this.callback.DigitDetected(matchNumber, paperColors);
return State.STEER_720_DEGREES;
}
/// <summary>
/// Run the state machine for the current grid of papers detected.
/// </summary>
/// <param name="papers">Current grid of papers detected.</param>
public void processDigitDetection(Paper[,] papers)
{
if(this.stop)
{
this.state = State.WAIT_FOR_RUN;
}
if (this.initialFrameCount < INITIAL_FRAMES_IGNORED)
{
this.initialFrameCount++;
}
else
{
this.initialFrameCount = 0;
switch (this.state)
{
case State.WAIT_FOR_RUN:
this.state = this.waitForRun(papers);
break;
case State.CHECK_DIRECTION:
this.state = this.checkDirection(papers);
break;
case State.COLUMN_ANGLE_CORRECTION:
this.state = this.columnAngleCorrection(papers);
break;
case State.COLUMN_DETECTION:
this.state = this.columnDetection(papers);
break;
case State.STEER_TO_MOVE:
this.state = this.steerToMove(papers);
break;
case State.STEER_ANGLE_CORRECTION:
this.state = this.steerAngleCorrection(papers);
break;
case State.MOVE_FORWARD:
this.state = this.moveForward(papers);
break;
case State.MOVE_FORWARD_CORRECTION:
this.state = this.moveForwardCorrection(papers);
break;
case State.STEER_TO_DETECT:
this.state = this.steerToDetect(papers);
break;
case State.CALCULATE_DIGIT:
this.state = this.calculateDigit(papers);
break;
case State.STEER_720_DEGREES:
this.state = this.steer720Degrees(papers);
break;
default:
this.state = this.waitForRun(papers);
break;
}
}
}
/// <summary>
/// Display the current grid of paper colors on the UI.
/// </summary>
/// <param name="papers">Current paper grid.</param>
private void displayPapers(Paper[,] papers)
{
if (this.InvokeRequired)
{
DisplayPaperCallback callback = new DisplayPaperCallback(displayPapers);
try
{
this.Invoke(callback, new object[] { papers });
}
catch (ObjectDisposedException)
{
// No Op
}
}
else
{
this.y0x0ColTxt.Text = PaperColorUtils.PaperColorToString(papers[0, 0].Color);
this.y0x1ColTxt.Text = PaperColorUtils.PaperColorToString(papers[0, 1].Color);
this.y0x2ColTxt.Text = PaperColorUtils.PaperColorToString(papers[0, 2].Color);
this.y1x0ColTxt.Text = PaperColorUtils.PaperColorToString(papers[1, 0].Color);
this.y1x1ColTxt.Text = PaperColorUtils.PaperColorToString(papers[1, 1].Color);
this.y1x2ColTxt.Text = PaperColorUtils.PaperColorToString(papers[1, 2].Color);
this.y2x0ColTxt.Text = PaperColorUtils.PaperColorToString(papers[2, 0].Color);
this.y2x1ColTxt.Text = PaperColorUtils.PaperColorToString(papers[2, 1].Color);
this.y2x2ColTxt.Text = PaperColorUtils.PaperColorToString(papers[2, 2].Color);
}
}
