private void Control(Bitmap image)
{
var uBitmap = new Utility.UnsafeBitmap(image);
uBitmap.LockBitmap();
try
{
for (int y = 0; y < uBitmap.Bitmap.Height; y += 2)
{
for (int x = 0; x < uBitmap.Bitmap.Width; x += 2)
{
byte r = uBitmap.GetPixel(x, y).red;
byte b = uBitmap.GetPixel(x, y).blue;
byte g = uBitmap.GetPixel(x, y).green;
if ((r - 100 > g) && (r - 100 > b))
{
var pStart = StartControl(x - 2, y, image);
var pHEnd = VerticalControl(pStart, image);
var pWEnd = GorizontalControl(pStart, image);
if (pHEnd.X != -1)
{
if (Mathematic(pStart, pHEnd, pWEnd, uBitmap.Bitmap.Size.Width, uBitmap.Bitmap.Size.Height))
{
y = uBitmap.Bitmap.Height;
break;
}
}
}
}
}
}
catch { }
uBitmap.UnlockBitmap();
uBitmap.Dispose();
}
private void Control(Bitmap image)
{
Utility.UnsafeBitmap uBitmap = new Utility.UnsafeBitmap(image);
uBitmap.LockBitmap();
Point pStart = new Point(-1, -1);
Point pHEnd = new Point(-1, -1);
Point pWEnd = new Point(-1, -1);
byte r, g, b;
for (int y = 0; y < uBitmap.Bitmap.Height; y+=2)
{
for (int x = 0; x < uBitmap.Bitmap.Width; x+=2)
{
r = uBitmap.GetPixel(x, y).red;
b = uBitmap.GetPixel(x, y).blue;
g = uBitmap.GetPixel(x, y).green;
if ((r - 100 > g) && (r - 100 > b))
{
pStart = StartControl(x - 2, y - 2, image);
pWEnd = GorizontalControl(pStart, image);
pHEnd = VerticalControl(pStart, image);
if (pHEnd.X != -1)
{
if (Mathematic(pStart, pHEnd, pWEnd, uBitmap.Bitmap.Size.Width, uBitmap.Bitmap.Size.Height))
{
y = uBitmap.Bitmap.Height;
break;
}
}
}
}//for (int x = 0; x < uBitmap.Bitmap.Width; x++)
}//for (int y = 0; y < uBitmap.Bitmap.Height; y+=5)
uBitmap.UnlockBitmap();
uBitmap.Dispose();
}
private Point VerticalControl(Point pStart, Bitmap image)
{
Utility.UnsafeBitmap uBitmap = new Utility.UnsafeBitmap(image);
uBitmap.LockBitmap();
Point pEnd = new Point(-1, -1);
byte r, g, b;
for (int y = pStart.Y; y < 30+pStart.Y; y++)
{
r = uBitmap.GetPixel(pStart.X, y).red;
b = uBitmap.GetPixel(pStart.X, y).blue;
g = uBitmap.GetPixel(pStart.X, y).green;
if (((r-100>b)&&(r-100>g))||((r>150)&&(g>120)&&(b>120)))
{
}
else
{
pEnd.X = pStart.X;
pEnd.Y = y;
break;
}
}
uBitmap.UnlockBitmap();
uBitmap.Dispose();
return pEnd;
}
private Point StartControl(int x,int y, Bitmap image)
{
Utility.UnsafeBitmap uBitmap = new Utility.UnsafeBitmap(image);
uBitmap.LockBitmap();
Point pStart = new Point(-1,-1);
byte r, g, b;
for (int Y=y; Y < y+30; Y++)
{
for (int X=x; X < x+ 30; X++)
{
r = uBitmap.GetPixel(X, Y).red;
b = uBitmap.GetPixel(X, Y).blue;
g = uBitmap.GetPixel(X, Y).green;
if ((r - 100 > g) && (r - 100 > b))
{
if (pStart.X == -1) // save start point
{
pStart.X = X;
pStart.Y = Y;
break;
}
}//if ((r - 100 > g) && (r - 100 > b))
}//for (int x = 0; x < uBitmap.Bitmap.Width; x++)
if (pStart.X != -1)
{
break;
}
}//for (int y = 0; y < uBitmap.Bitmap.Height; y+=5)
uBitmap.UnlockBitmap();
uBitmap.Dispose();
return pStart;
}
private Point GorizontalControl(Point pStart,Bitmap image)
{
Point pHEnd = new Point(-1, -1);
Utility.UnsafeBitmap uBitmap = new Utility.UnsafeBitmap(image);
uBitmap.LockBitmap();
byte r, g, b;
for (int x = pStart.X; x < 30+pStart.X; x++)
{
r = uBitmap.GetPixel(x, pStart.Y).red;
b = uBitmap.GetPixel(x, pStart.Y).blue;
g = uBitmap.GetPixel(x, pStart.Y).green;
if (((r - 100 > b) && (r - 100 > g)) || ((r > 200) && (g > 120) && (b > 120)))
{
}
else
{
pHEnd.X = x;
pHEnd.Y = pStart.Y;
break;
}
}//for (int x = 0; x < uBitmap.Bitmap.Width; x++)
uBitmap.UnlockBitmap();
uBitmap.Dispose();
return pHEnd;
}
// Process new frame
public void ProcessFrame(ref Bitmap image)
{
if (_mForm.leftMouseButtonDown == true)
{
//Code to draw a rubberband selection rectangle
//Compute width and height of rectangle
int w = Math.Abs((int)_mForm.x2 - (int)_mForm.x1);
int h = Math.Abs((int)_mForm.y2 - (int)_mForm.y1);
//Decide x and y of rectangle
int x = 0, y = 0;
if (_mForm.x1 < _mForm.x2)
{
x = (int)_mForm.x1;
}
else if ((int)_mForm.x1 > _mForm.x2)
{
x = (int)_mForm.x2;
}
if (_mForm.y1 < _mForm.y2)
{
y = (int)_mForm.y1;
}
else if (_mForm.y1 > _mForm.y2)
{
y = (int)_mForm.y2;
}
//Draw the rectangle
Rectangle rect = new Rectangle(x, y, w, h);
Graphics g = Graphics.FromImage((System.Drawing.Image)image);
g.FillRectangle(new SolidBrush(Color.FromArgb
(60, 184, 184, 0)), rect); //Draws a semi-transparent rectangle using alpha blending
g.Dispose();
return;
}
if (_mForm.videoCropHeight == -1)
{
//Set or reset video cropping so that nothing gets trimmed out
_mForm.videoCropHeight = image.Height;
_mForm.videoCropWidth = image.Width;
}
//Crop the bitmap
Bitmap tmpImage0 = image.Clone(new Rectangle(_mForm.videoCropX,
_mForm.videoCropY, _mForm.videoCropWidth, _mForm.videoCropHeight),
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
//Resize the bitmap
AForge.Imaging.Filters.Resize resize = new Resize(image.Width / _mForm.resizeFactor,
image.Height / _mForm.resizeFactor, InterpolationMethod.NearestNeighbor);
Bitmap tmpImage1 = resize.Apply(tmpImage0);
//Get width and height of Bitmap
imageWidth = tmpImage1.Width;
imageHeight = tmpImage1.Height;
tmpImage0.Dispose();
Utility.UnsafeBitmap uBitmap = new Utility.UnsafeBitmap(tmpImage1);
//Locate the brightest pixel in the bitmap
bool brightnessFound = false;
uBitmap.LockBitmap();
float brightest = 0;
int xPos = 0, yPos = 0, xprev = 0, yprev = 0, currx = 0, curry = 0, dispx = 0, dispy = 0;
for (int y = 0; y < imageHeight; y += 1)
{
for (int x = 0; x < imageWidth; x += 1)
{
byte red, green, blue;
red = uBitmap.GetPixel(x, y).red;
green = uBitmap.GetPixel(x, y).green;
blue = uBitmap.GetPixel(x, y).blue;
if (red > 150 && green < 100 && blue < 100)
{
xPos = x;
yPos = y;
currx = x;
curry = y;
brightnessFound = true;
s_red = red; s_green = green; s_blue = blue;
}
if (green > 170 && red < 150 && blue < 190)
{
xPos = x;
yPos = y;
currx = x;
curry = y;
brightnessFound = true;
s_red = red; s_green = green; s_blue = blue;
}
if (blue > 150 && green < 100 && red < 100)
{
xPos = x;
yPos = y;
currx = x;
curry = y;
brightnessFound = true;
s_red = red; s_green = green; s_blue = blue;
}
/* if (red > 220 && green > 220 && blue > 220)
* {
*
* xPos = x;
* yPos = y;
* currx = x;
* curry = y;
* brightnessFound = true;
* s_red = red; s_green = green; s_blue = blue;
* } */
if (red > 150 && green > 150 && blue < 100)
{
xPos = x;
yPos = y;
currx = x;
curry = y;
brightnessFound = true;
s_red = red; s_green = green; s_blue = blue;
}
// (brightest > _mForm.threshold)
} // x loop
} // y loop
string path = "ByteValues.txt";
// This text is added only once to the file.
if (File.Exists(path))
{
File.Delete(path);
}
if (!File.Exists(path))
{
// Create a file to write to.
using (StreamWriter sw = new StreamWriter(path, true))
{
sw.WriteLine(s_red.ToString() + " " + s_green.ToString() + " " + s_blue.ToString());
sw.Close();
}
}
if (brightnessFound == true)
{
int colour = 0;
if (((s_red > s_blue) && (s_red > s_green) && (Math.Abs(s_red - s_green) > 65) && ((Math.Abs(s_red - s_blue) > 65))))
{
colour = 1;
}
else if ((((s_green > s_red) && (s_green > s_blue) && Math.Abs(s_green - s_red) > 65) && ((Math.Abs(s_green - s_blue) > 65))))
{
colour = 2;
}
else if (((s_blue > s_red) && (s_blue > s_green) && (Math.Abs(s_blue - s_red) > 65) && ((Math.Abs(s_blue - s_green) > 65))))
{
colour = 3;
}
// else if (s_red>220 && s_green>220 && s_blue>220 ) colour = 4;
else if (s_red > 150 && s_green > 150 && s_blue < 100)
{
colour = 5;
}
string path1 = "Colour Detected.txt";
switch (colour)
{
case 1:
if (!File.Exists(path1))
{
// Create a file to write to.
using (StreamWriter sw = new StreamWriter(path1, true))
{
sw.Write("Red Detected ");
sw.Close();
}
}
else
{
using (StreamWriter sw = File.AppendText(path1))
{
sw.Write("Red Detected ");
sw.Close();
}
}
if ((xprev - currx != 0) || (yprev - curry != 0))
{
dispx = xprev - currx;
dispy = yprev - curry;
ControlCursor(xPos, yPos, _mForm.enableClick); //Set cursor position
}
xprev = currx;
yprev = curry;
break;
case 2:
if (!File.Exists(path1))
{
// Create a file to write to.
using (StreamWriter sw = new StreamWriter(path1, true))
{
sw.Write("Green Detected ");
sw.Close();
}
}
else
{
using (StreamWriter sw = File.AppendText(path1))
{
sw.Write("Green Detected ");
sw.Close();
}
}
//Generate a left mouse button click
mouse_event(MOUSEEVENTF_LEFTDOWN | MOUSEEVENTF_LEFTUP, 0, 0, 0, 0);
break;
case 3:
if (!File.Exists(path1))
{
// Create a file to write to.
using (StreamWriter sw = new StreamWriter(path1, true))
{
sw.Write("Blue Detected ");
sw.Close();
}
}
else
{
using (StreamWriter sw = File.AppendText(path1))
{
sw.Write("Blue Detected ");
sw.Close();
}
}
//Generate a left mouse button click
mouse_event(MOUSEEVENTF_RIGHTDOWN | MOUSEEVENTF_RIGHTUP, 0, 0, 0, 0);
break;
/* case 4:
* if (!File.Exists(path1))
* {
* // Create a file to write to.
* using (StreamWriter sw = new StreamWriter(path1, true))
* {
* sw.Write("White Detected ");
* sw.Close();
* }
* }
* else
* {
* using (StreamWriter sw = File.AppendText(path1))
* {
* sw.Write("White Detected ");
* sw.Close();
* }
* }
* break;*/
case 5:
if (!File.Exists(path1))
{
// Create a file to write to.
using (StreamWriter sw = new StreamWriter(path1, true))
{
sw.Write("Yellow Detected ");
sw.Close();
}
}
else
{
using (StreamWriter sw = File.AppendText(path1))
{
sw.Write("Yellow Detected ");
sw.Close();
}
}
break;
}
uBitmap.UnlockBitmap();
//Encircle the brightest pixel
Graphics dc = Graphics.FromImage(tmpImage1);
Pen p = new Pen(Color.LimeGreen, 1);
dc.DrawEllipse(p, xPos - 5, yPos - 5, 10, 10);
//Show the x and y coordinates of the brightest pixel
dc.DrawString("X: " + xPos.ToString() + ", Y: " +
yPos.ToString(), new Font("Verdana", 7, FontStyle.Regular),
Brushes.Yellow, new Point(xPos, yPos + 8));
if (_mForm.controlMouse == true)
{
ControlCursor(xPos, yPos, _mForm.enableClick); //Set cursor position
}
dc.Dispose();
uBitmap.LockBitmap();
}
else
{
if (_mForm.enableClick == true && leftMouseButtonDown == true)
{
//Generate a left mouse button click
mouse_event(MOUSEEVENTF_LEFTDOWN | MOUSEEVENTF_LEFTUP, 0, 0, 0, 0);
leftMouseButtonDown = false;
}
}
uBitmap.UnlockBitmap();
uBitmap.Bitmap.Dispose();
image.Dispose();
image = tmpImage1;
}
private Point StartControl(int x, int y, Bitmap image)
{
var uBitmap = new Utility.UnsafeBitmap(image);
uBitmap.LockBitmap();
var pStart = new Point(-1, -1);
try
{
for (int Y = y; Y < y + 30; Y++)
{
for (int X = x; X < x + 30; X++)
{
byte r = uBitmap.GetPixel(X, Y).red;
byte b = uBitmap.GetPixel(X, Y).blue;
byte g = uBitmap.GetPixel(X, Y).green;
if ((r - 100 > g) && (r - 100 > b))
{
if (pStart.X == -1) // save start point
{
pStart.X = X;
pStart.Y = Y;
break;
}
}
}
if (pStart.X != -1)
{
break;
}
}
}
catch { }
uBitmap.UnlockBitmap();
uBitmap.Dispose();
return pStart;
}
//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;
}
}
// 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();
}
private Point VerticalControl(Point pStart)
{
MainForm f = new MainForm();
Bitmap image = new Bitmap(f.pictureBox1.BackgroundImage);
Utility.UnsafeBitmap uBitmap = new Utility.UnsafeBitmap(image);
uBitmap.LockBitmap();
Point pEnd = new Point(-1, -1);
for (int y = pStart.Y; y < 30 + pStart.Y; y++)
{
int r = uBitmap.GetPixel(pStart.X, y).red;
int b = uBitmap.GetPixel(pStart.X, y).blue;
int g = uBitmap.GetPixel(pStart.X, y).green;
if (((r - 80 > b) && (r - 80 > g)) || ((r > 150) && (g > 120) && (b > 120)))
{
}
else
{
pEnd.X = pStart.X;
pEnd.Y = y;
break;
}
}
uBitmap.UnlockBitmap();
uBitmap.Dispose();
image.Dispose();
return pEnd;
}
private Point StartControl(int x, int y)
{
MainForm f = new MainForm();
Bitmap image = new Bitmap(f.pictureBox1.BackgroundImage);
Utility.UnsafeBitmap uBitmap = new Utility.UnsafeBitmap(image);
image.Save("D:\\saveImage.bmp");
uBitmap.LockBitmap();
Point pStart = new Point(-1, -1);
for (int Y = y; Y < y + 30; Y++)
{
if (y >= 480)
break;
for (int X = x; X < x + 30; X++)
{
if (X >= 640)
break;
int r = uBitmap.GetPixel(X, Y).red;
int b = uBitmap.GetPixel(X, Y).blue;
int g = uBitmap.GetPixel(X, Y).green;
if ((r - 80 > g) && (r - 80 > b))
{
if (pStart.X == -1) // save start point
{
pStart.X = X;
pStart.Y = Y;
break;
}
} //if ((r - 100 > g) && (r - 100 > b))
} //for (int x = 0; x < uBitmap.Bitmap.Width; x++)
if (pStart.X != -1)
{
break;
}
}
uBitmap.UnlockBitmap();
uBitmap.Dispose();
image.Dispose();
//pWEnd = GorizontalControl(pStart, image);
//pHEnd = VerticalControl(pStart, image);
return pStart;
}
private Point GorizontalControl(Point pStart)
{
Point pHEnd = new Point(-1, -1);
MainForm f = new MainForm();
Bitmap image = new Bitmap(f.pictureBox1.BackgroundImage);
Utility.UnsafeBitmap uBitmap = new Utility.UnsafeBitmap(image);
uBitmap.LockBitmap();
for (int x = pStart.X; x < 30 + pStart.X; x++)
{
int r = uBitmap.GetPixel(x, pStart.Y).red;
int b = uBitmap.GetPixel(x, pStart.Y).blue;
int g = uBitmap.GetPixel(x, pStart.Y).green;
if (((r - 80 > b) && (r - 80 > g)) || ((r > 150) && (g > 120) && (b > 120)))
{
}
else
{
pHEnd.X = x;
pHEnd.Y = pStart.Y;
break;
}
}
uBitmap.UnlockBitmap();
uBitmap.Dispose();
image.Dispose();
return pHEnd;
}
private void autoUst()
{
Utility.UnsafeBitmap uBitmap = new Utility.UnsafeBitmap((Bitmap)pictureBox1.BackgroundImage);
uBitmap.LockBitmap();
byte r, g, b;
Point p1 = new Point(-1, -1);
Point p2 = new Point(-1, -1);
Point p3 = new Point(-1, -1);
Point p4 = new Point(-1, -1);
for (int y = 0; y < uBitmap.Bitmap.Size.Height; y+=2)
{
for (int x = 0; x < uBitmap.Bitmap.Size.Width; x+=2)
{
r = uBitmap.GetPixel(x, y).red;
g = uBitmap.GetPixel(x, y).green;
b = uBitmap.GetPixel(x, y).blue;
if ((r - g > 100) && (r - b > 100))
{
ColorControl f = new ColorControl();
if (p1.X == -1)
{
p1 = f.PPoint(x, y, uBitmap.Bitmap);
x +=20;
y += 20;
continue;
}
if ((p1.X != -1) && (p2.X == -1))
{
p2 = f.PPoint(x,y,uBitmap.Bitmap);
x += 20;
y += 20;
continue;
}
if ((p2.X != -1) && (p3.X == -1))
{
p3 = f.PPoint(x, y, uBitmap.Bitmap);
x += 20;
y += 20;
continue;
}
if ((p3.X != -1) && (p4.X == -1))
{
p4 = f.PPoint(x, y, uBitmap.Bitmap);
x += 20;
y += 20;
continue;
}
}
}
}
}
// Process new frame
public void ProcessFrame(ref Bitmap image)
{
if (_mForm.leftMouseButtonDown == true)
{
//Code to draw a rubberband selection rectangle
//Compute width and height of rectangle
int w = Math.Abs((int)_mForm.x2 - (int)_mForm.x1);
int h = Math.Abs((int)_mForm.y2 - (int)_mForm.y1);
//Decide x and y of rectangle
int x = 0, y = 0;
if (_mForm.x1 < _mForm.x2)
{
x = (int)_mForm.x1;
}
else if ((int)_mForm.x1 > _mForm.x2)
{
x = (int)_mForm.x2;
}
if (_mForm.y1 < _mForm.y2)
{
y = (int)_mForm.y1;
}
else if (_mForm.y1 > _mForm.y2)
{
y = (int)_mForm.y2;
}
//Draw the rectangle
Rectangle rect = new Rectangle(x, y, w, h);
Graphics g = Graphics.FromImage((System.Drawing.Image)image);
g.FillRectangle(new SolidBrush(Color.FromArgb
(60, 184, 184, 0)), rect); //Draws a semi-transparent rectangle using alpha blending
g.Dispose();
return;
}
if (_mForm.videoCropHeight == -1)
{
//Set or reset video cropping so that nothing gets trimmed out
_mForm.videoCropHeight = image.Height;
_mForm.videoCropWidth = image.Width;
}
//Crop the bitmap
Bitmap tmpImage0 = image.Clone(new Rectangle(_mForm.videoCropX,
_mForm.videoCropY, _mForm.videoCropWidth, _mForm.videoCropHeight),
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
//Resize the bitmap
AForge.Imaging.Filters.Resize resize = new Resize(image.Width / _mForm.resizeFactor,
image.Height / _mForm.resizeFactor, InterpolationMethod.NearestNeighbor);
Bitmap tmpImage1 = resize.Apply(tmpImage0);
//Get width and height of Bitmap
imageWidth = tmpImage1.Width;
imageHeight = tmpImage1.Height;
tmpImage0.Dispose();
Utility.UnsafeBitmap uBitmap = new Utility.UnsafeBitmap(tmpImage1);
//Locate the brightest pixel in the bitmap
bool brightnessFound = false;
uBitmap.LockBitmap();
float brightest = 0;
int xPos = 0, yPos = 0;
for (int y = 0; y < imageHeight; y += 1)
{
for (int x = 0; x < imageWidth; x += 1)
{
byte red, green, blue;
red = uBitmap.GetPixel(x, y).red;
green = uBitmap.GetPixel(x, y).green;
blue = uBitmap.GetPixel(x, y).blue;
float brightness = (299 * red + 587 * green + 114 * blue) / 1000;
if (brightness > _mForm.threshold)
{
brightest = brightness;
xPos = x;
yPos = y;
brightnessFound = true;
} // (brightest > _mForm.threshold)
} // x loop
} // y loop
if (brightnessFound == true)
{
uBitmap.UnlockBitmap();
//Encircle the brightest pixel
Graphics dc = Graphics.FromImage(tmpImage1);
Pen p = new Pen(Color.LimeGreen, 1);
dc.DrawEllipse(p, xPos - 5, yPos - 5, 10, 10);
//Show the x and y coordinates of the brightest pixel
dc.DrawString("X: " + xPos.ToString() + ", Y: " +
yPos.ToString(), new Font("Verdana", 7, FontStyle.Regular),
Brushes.Yellow, new Point(xPos, yPos + 8));
if (_mForm.controlMouse == true)
{
ControlCursor(xPos, yPos, _mForm.enableClick); //Set cursor position
}
dc.Dispose();
uBitmap.LockBitmap();
}
else
{
if (_mForm.enableClick == true && leftMouseButtonDown == true)
{
//Generate a left mouse button click
mouse_event(MOUSEEVENTF_LEFTDOWN | MOUSEEVENTF_LEFTUP, 0, 0, 0, 0);
leftMouseButtonDown = false;
}
}
uBitmap.UnlockBitmap();
uBitmap.Bitmap.Dispose();
image.Dispose();
image = tmpImage1;
}