Ejemplo n.º 1
0
        // 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();
        }
Ejemplo n.º 2
0
        /// <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;
        }
Ejemplo n.º 3
0
        //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;
            }
        }