// Process new frame
public void ProcessFrame(ref Bitmap image)
{
//Manual Control
if (mForm.ColorTrackingOnFlag == false)
{
switch (mForm.ManualControlFlag)
{
case GO: if (stateCreateMobility != GO)
{
f1.Go_Click();
ResetCounters();
stateCreateMobility = GO;
}
break;
case STOP: if (stateCreateMobility != STOP)
{
f1.Stop_Click();
ResetCounters();
stateCreateMobility = STOP;
};
break;
case RIGHT: if (stateCreateMobility != RIGHT)
{
f1.TurnRight_Click(50);
ResetCounters();
stateCreateMobility = RIGHT;
}
break;
case LEFT : if (stateCreateMobility != LEFT)
{
f1.TurnLeft_Click(50);
ResetCounters();
stateCreateMobility = LEFT;
} break;
case BACK : if (stateCreateMobility != BACK)
{
f1.Back_Click();
ResetCounters();
stateCreateMobility = BACK;
} break;
default: break;
}
targetFlag = false;
return;
}
Stablecount = mForm.StableCounter;
ColorTolerance = mForm.ColorTolerance;
if ((targetFlag == false) || (mForm.MouseClickOnImage == true))
{
getTargetColor(ref image);
//TurnToTarget();
SearchStartPoint.X = 0;
SearchStartPoint.Y = 0;
SearchEndPoint.X = image.Width;
SearchEndPoint.Y = image.Height;
return;
}
//Find out target Color
Utility.UnsafeBitmap uBitmap = new Utility.UnsafeBitmap(image);
bool tragetColorFound = false;
int xPos = 0, yPos = 0;
targetSum.X = 0;
targetSum.Y = 0;
targetPixel = 0;
targetCentroid.X=targetCentroid.Y=0;
minTargetPoint.X = image.Width;
minTargetPoint.Y = image.Height;
maxTargetPoint.X = maxTargetPoint.Y = 0;
uBitmap.LockBitmap();
//Console.WriteLine("Search Window :("+SearchStartPoint.X+","+SearchStartPoint.Y+"),("+SearchEndPoint.X+","+SearchEndPoint.Y+")");
for (int y = SearchStartPoint.Y; y < SearchEndPoint.Y; y += 2)
{
for (int x = SearchStartPoint.X ; x < SearchEndPoint.X; x += 2)
{
//byte red, green, blue;
realImage.red = uBitmap.GetPixel(x, y).red;
realImage.green = uBitmap.GetPixel(x, y).green;
realImage.blue = uBitmap.GetPixel(x, y).blue;
//float brightness = (299 * red + 587 * green + 114 * blue) / 1000;
//float brightness = realImage.red;
//Change the Color Space from RGB to HSV
realImageHSV=ConvertRGB2HSV(realImage.red, realImage.green, realImage.blue);
//Console.WriteLine("Real Image (R,G,B) = ("+realImage.red+","+realImage.green+","+realImage.blue+")");
//Use sin(hue) to compensate the period of hue is pi.
if (((realImageHSV.h >= (targetImageHSV.h - ColorTolerance)) && (realImageHSV.h <= (targetImageHSV.h + ColorTolerance)))
&& ((realImageHSV.s >= (targetImageHSV.s - SaturationTolerance)) && (realImageHSV.s <= (targetImageHSV.s + SaturationTolerance)))
)
{
//if ((realImageHSV.h >= (targetImageHSV.h - ColorTolerance)) && (realImageHSV.h <= (targetImageHSV.h + ColorTolerance)))
//{
/*if (((realImage.red >= (targetImage.red-ColorTolerance))&&(realImage.red <= (targetImage.red+ColorTolerance)))
&& ((realImage.green >= (targetImage.green-ColorTolerance))&&(realImage.green <= (targetImage.green+ColorTolerance)))
&& ((realImage.blue >= (targetImage.blue-ColorTolerance)) && (realImage.blue <= (targetImage.blue+ColorTolerance))))
{*/
//brightest = brightness;
xPos = x;
yPos = y;
targetSum.X += xPos;
targetSum.Y += yPos;
targetPixel ++;
if (xPos > maxTargetPoint.X) maxTargetPoint.X = xPos;
if (yPos > maxTargetPoint.Y) maxTargetPoint.Y = yPos;
if (xPos < minTargetPoint.X) minTargetPoint.X = xPos;
if (yPos < minTargetPoint.Y) minTargetPoint.Y = yPos;
//tragetColorFound = true;
//realImageHSV = ConvertRGB2HSV(realImage.red, realImage.green, realImage.blue);
//Console.WriteLine("Position : ("+xPos+","+yPos+")");
//Console.WriteLine("Real(r,g,b) = (" + realImage.red + "," + realImage.green + "," + realImage.blue + ")");
//Console.WriteLine("Real(H,S,V) = (" + realImageHSV.h + "," + realImageHSV.s + "," + realImageHSV.v + ")");
}
} // x loop
} // y loop
//Calculate the centroid of target object
if (targetPixel > MIN_TARGET_PIXEL)
{
targetCentroid.X = targetSum.X / targetPixel;
targetCentroid.Y = targetSum.Y / targetPixel;
tragetColorFound = true;
//Console.WriteLine("target Centroid : ("+targetCentroid.X+","+targetCentroid.Y+")");
//Console.WriteLine("target pixel : " + targetPixel);
//Change Search Range for the dynamic search
if (stateCreateMobility==GO)
{
if (minTargetPoint.X > SEARCH_MARGIN)
{
SearchStartPoint.X = minTargetPoint.X - SEARCH_MARGIN;
}
else
{
SearchStartPoint.X = 0;
}
if (minTargetPoint.Y > SEARCH_MARGIN)
{
SearchStartPoint.Y = minTargetPoint.Y - SEARCH_MARGIN;
}
else
{
SearchStartPoint.Y = 0;
}
if ((maxTargetPoint.X + SEARCH_MARGIN) < image.Width)
{
SearchEndPoint.X = maxTargetPoint.X + SEARCH_MARGIN;
}
else
{
SearchEndPoint.X = image.Width;
}
if ((maxTargetPoint.Y + SEARCH_MARGIN) < image.Height)
{
SearchEndPoint.Y = maxTargetPoint.Y + SEARCH_MARGIN;
}
else
{
SearchEndPoint.Y = image.Height;
}
//Console.WriteLine("Search Window :(" + SearchStartPoint.X + "," + SearchStartPoint.Y + "),(" + SearchEndPoint.X + "," + SearchEndPoint.Y + ")");
}
else
{
SearchStartPoint.X = 0;
SearchStartPoint.Y = 0;
SearchEndPoint.X = image.Width;
SearchEndPoint.Y = image.Height;
//Console.WriteLine("Search Window :(" + SearchStartPoint.X + "," + SearchStartPoint.Y + "),(" + SearchEndPoint.X + "," + SearchEndPoint.Y + ")");
}
}
else
{
if (StopCounter >= Stablecount)
{
if (stateCreateMobility != STOP)
{
f1.Stop_Click();
ResetCounters();
stateCreateMobility = STOP;
Console.WriteLine("Stop! Target is gone !!");
}
//Expand Search window
SearchStartPoint.X = 0;
SearchStartPoint.Y = 0;
SearchEndPoint.X = image.Width;
SearchEndPoint.Y = image.Height;
//Console.WriteLine("Search Window :(" + SearchStartPoint.X + "," + SearchStartPoint.Y + "),(" + SearchEndPoint.X + "," + SearchEndPoint.Y + ")");
}
else
{
TurnRightCounter = 0;
TurnLeftCounter = 0;
GoCounter = 0;
BumperStopCounter = 0;
StopCounter = 0;
LostCounter++;
}
return;
}
//Control iRobot if target color is found
if (tragetColorFound == true)
{
//Draw cross line on target color
uBitmap.UnlockBitmap();
Graphics dc = Graphics.FromImage(image);
Pen p = new Pen(Color.Yellow, 2);
//dc.DrawLine(p, 0, targetCentroid.Y, image.Width, targetCentroid.Y); // Draw yellow horizontal line
//dc.DrawLine(p, targetCentroid.X, 0, targetCentroid.X, image.Height); // Draw yellow vertical line
dc.DrawLine(p, minTargetPoint.X, minTargetPoint.Y, maxTargetPoint.X, minTargetPoint.Y); // Draw rectanlge of target objects
dc.DrawLine(p, minTargetPoint.X, minTargetPoint.Y, minTargetPoint.X, maxTargetPoint.Y);
dc.DrawLine(p, maxTargetPoint.X, minTargetPoint.Y, maxTargetPoint.X, maxTargetPoint.Y);
dc.DrawLine(p, minTargetPoint.X, maxTargetPoint.Y, maxTargetPoint.X, maxTargetPoint.Y);
dc.Dispose();
uBitmap.LockBitmap();
//Restric the distance from object
if (targetPixel >=(image.Width*image.Height/32))
{
if (stateCreateMobility != STOP)
{
f1.Stop_Click();
ResetCounters();
stateCreateMobility = STOP;
Console.WriteLine("Stop by Adjacency");
}
return;
}
// Control iRobot
if (targetCentroid.X < ((image.Width / 2) - centralRange))
{
//Service for iRobot @ Oct 30, 2008
//move turn left
if (TurnLeftCounter >= Stablecount)
{
if (stateCreateMobility != LEFT)
{
f1.TurnLeft_Click(50);
ResetCounters();
stateCreateMobility = LEFT;
Console.WriteLine("Trun Left");
}
}
else
{
TurnLeftCounter++;
TurnRightCounter = 0;
GoCounter = 0;
StopCounter = 0;
BumperStopCounter = 0;
LostCounter = 0;
}
}
else if (targetCentroid.X > ((image.Width / 2) + centralRange))
{
//Service for iRobot @ Oct 30, 2008
//move turn right
if (TurnRightCounter >= Stablecount)
{
if (stateCreateMobility != RIGHT)
{
f1.TurnRight_Click(50);
ResetCounters();
stateCreateMobility = RIGHT;
Console.WriteLine("Trun Right");
}
}
else
{
TurnRightCounter++;
TurnLeftCounter = 0;
GoCounter = 0;
StopCounter = 0;
BumperStopCounter = 0;
LostCounter = 0;
}
}
else // In case of find out the red stuff within central range
{
if ((f1.ReadSensor_Click(7) & 0x03) != 0) //bumper is pressed
{
if (BumperStopCounter >= Stablecount)
{
if (stateCreateMobility != STOP)
{
f1.Stop_Click();
ResetCounters();
stateCreateMobility = STOP;
Console.WriteLine("Stop by bumper");
}
}
else
{
TurnRightCounter = 0;
TurnLeftCounter = 0;
GoCounter = 0;
StopCounter = 0;
LostCounter = 0;
BumperStopCounter++;
}
}
else
{
if (GoCounter >= Stablecount/2)
{
if (stateCreateMobility != GO)
{
f1.Go_Click();
ResetCounters();
stateCreateMobility = GO;
centralRange = 50;
Console.WriteLine("Go");
}
}
else
{
TurnRightCounter = 0;
TurnLeftCounter = 0;
GoCounter ++;
StopCounter = 0;
LostCounter = 0;
BumperStopCounter = 0;
}
}
}
}
else //In case of fail to find out target color stuffs.
{
if (StopCounter >= Stablecount)
{
if (stateCreateMobility != STOP)
{
f1.Stop_Click();
ResetCounters();
stateCreateMobility = STOP;
Console.WriteLine("Stop! Target is gone !!");
}
//Expand Search window
SearchStartPoint.X = 0;
SearchStartPoint.Y = 0;
SearchEndPoint.X = image.Width;
SearchEndPoint.Y = image.Height;
//Console.WriteLine("Search Window :(" + SearchStartPoint.X + "," + SearchStartPoint.Y + "),(" + SearchEndPoint.X + "," + SearchEndPoint.Y + ")");
}
else
{
TurnRightCounter = 0;
TurnLeftCounter = 0;
GoCounter = 0;
BumperStopCounter = 0;
StopCounter = 0;
LostCounter++;
}
}
uBitmap.UnlockBitmap();
uBitmap.Bitmap.Dispose();
}
/// <summary>
/// Convert RGB to HSV
/// </summary>
/// <param name="r">range : 0~255</param>
/// <param name="g">range : 0~255</param>
/// <param name="b">range : 0~255</param>
/// <returns>HSVColorSpace:h(0~360),s(0~1),v(0~1)</returns>
private HSVColorSpace ConvertRGB2HSV(byte r, byte g, byte b)
{
HSVColorSpace result = new HSVColorSpace();
double red=0, green=0, blue=0;
double rgbMin = 0, rgbMax = 0;
//Scale 0~255 to 0~1
red = r / (double)255;
green = g / (double)255;
blue = b / (double)255;
//Find out max and min
rgbMin = ((green) <= (blue) ? ((red) <= (green) ? (red) : (green)) : ((red) <= (blue) ? (red) : (blue)));
rgbMax = ((green) >= (blue) ? ((red) >= (green) ? (red) : (green)) : ((red) >= (blue) ? (red) : (blue)));
result.v = rgbMax;
if (result.v == 0)
{
result.h = result.s = 0;
return result;
}
//Normalize to 1
//red /=result.v;
//green /=result.v;
//blue /=result.v;
//rgbMin = ((green) <= (blue) ? ((red) <= (green) ? (red) : (green)) : ((red) <= (blue) ? (red) : (blue)));
//rgbMax = ((green) >= (blue) ? ((red) >= (green) ? (red) : (green)) : ((red) >= (blue) ? (red) : (blue)));
//Compute Saturation
result.s = (rgbMax - rgbMin)/rgbMax;
if (result.s == 0)
{
result.h = 0;
return result;
}
//Normalize saturatin to 1
//red = (red - rgbMin) / (rgbMax - rgbMin);
//green = (green - rgbMin) / (rgbMax - rgbMin);
//blue = (blue - rgbMin) / (rgbMax - rgbMin);
//rgbMin = ((green) <= (blue) ? ((red) <= (green) ? (red) : (green)) : ((red) <= (blue) ? (red) : (blue)));
//rgbMax = ((green) >= (blue) ? ((red) >= (green) ? (red) : (green)) : ((red) >= (blue) ? (red) : (blue)));
//Calculate Hue
if (rgbMin == rgbMax)
{
result.h = 0;
}
else
{
if (rgbMax == red)
{
result.h = 60 * (green - blue) / (rgbMax - rgbMin);
if (result.h < 0.0)
{
result.h += 360.0;
}
}
else if (rgbMax == green)
{
result.h = 120.0 + 60.0 * (blue - red) / (rgbMax - rgbMin);
}
else if (rgbMax == blue)
{
result.h = 240.0 + 60.0 * (red - green) / (rgbMax - rgbMin);
}
}
return result;
}
//added for iRobot @ 11/17/08 by joseph
//Get Target Color
public void getTargetColor(ref Bitmap image)
{
//Point mousePoint = new Point();
//Read MouseClick
if(mForm.MouseClickOnImage==true){
targetPoint.X = mForm.MouseLocationOnImage.X;
targetPoint.Y = mForm.MouseLocationOnImage.Y;
Console.WriteLine("Mouse(x,y) = (" + targetPoint.X + "," + targetPoint.Y + ")");
//Read target color
Utility.UnsafeBitmap uBitmap = new Utility.UnsafeBitmap(image);
uBitmap.LockBitmap();
targetImage.red = uBitmap.GetPixel(targetPoint.X, targetPoint.Y).red;
targetImage.green = uBitmap.GetPixel(targetPoint.X, targetPoint.Y).green;
targetImage.blue = uBitmap.GetPixel(targetPoint.X, targetPoint.Y).blue;
targetImageHSV = ConvertRGB2HSV(targetImage.red, targetImage.green, targetImage.blue);
Console.WriteLine("Traget(r,g,b) = (" + targetImage.red + "," + targetImage.green+","+targetImage.blue+ ")");
Console.WriteLine("Target(H,S,V) = (" + targetImageHSV.h + "," + targetImageHSV.s + "," + targetImageHSV.v + ")");
targetFlag = true;
mForm.MouseClickOnImage = false;
}
}