Esempio n. 1
0
/**
* @brief Search for a color. Supports IMAQ_IMAGE_HSL. 
* @param color Definition for the hue range 
* @param trackReport Values for tracking: center of particle, particle size, color
* @return 0 = error
*/
int FindColor(FrcHue color, ParticleAnalysisReport* trackReport)
{
	int success = 0;		// return: 0 = error
	
	/* track color */
	// use ACTIVE_LIGHT or WHITE_LIGHT for brightly lit objects
	TrackingThreshold td = GetTrackingData(color, PASSIVE_LIGHT);

	success = FindColor(IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, trackReport); 
	if ( !success )	{ 
		DPRINTF (LOG_INFO, "did not find color - errorCode= %i",GetLastVisionError());	
		return success;
	}

	//PrintReport(par);
	
	/* set an image quality restriction */
	if (trackReport->particleToImagePercent < PARTICLE_TO_IMAGE_PERCENT) {
		imaqSetError(ERR_PARTICLE_TOO_SMALL, __FUNCTION__);
		success = 0;
	}	
	return success;
}
Esempio n. 2
0
    /**
     * unchanged from SimpleDemo:
     *
     * Runs the motors under driver control with either tank or arcade
     * steering selected by a jumper in DS Digin 0.
     *
     * added for vision:
     *
     * Adjusts the servo gimbal based on the color tracked.  Driving the
     * robot or operating an arm based on color input from gimbal-mounted
     * camera is currently left as an exercise for the teams.
     */
    void OperatorControl(void)
    {
        char funcName[] = "OperatorControl";
        DPRINTF(LOG_DEBUG, "OperatorControl");
        //GetWatchdog().Feed();
        TrackingThreshold td = GetTrackingData(GREEN, FLUORESCENT);

        /* for controlling loop execution time */
        float loopTime = 0.05;
        double currentTime = GetTime();
        double lastTime = currentTime;

        double savedImageTimestamp = 0.0;
        bool foundColor = false;
        bool staleImage = false;

        while (IsOperatorControl())
        {
            setServoPositions(rightStick->GetX(), rightStick->GetY());

            /* calculate gimbal position based on color found */
            if (FindColor
                (IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, &par,
                 &cReport))
            {
                foundColor = true;
                if (par.imageTimestamp == savedImageTimestamp)
                {
                    // This image has been processed already,
                    // so don't do anything for this loop
                    staleImage = true;
                }
                else
                {
                    staleImage = false;
                    savedImageTimestamp = par.imageTimestamp;
                    // compute final H & V destination
                    horizontalDestination = par.center_mass_x_normalized;
                    verticalDestination = par.center_mass_y_normalized;
                }
            }
            else
            {
                foundColor = false;
            }

            PrintReport(&cReport);

            if (!staleImage)
            {
                if (foundColor)
                {
                    /* Move the servo a bit each loop toward the
                     * destination.  Alternative ways to task servos are
                     * to move immediately vs.  incrementally toward the
                     * final destination. Incremental method reduces the
                     * need for calibration of the servo movement while
                     * moving toward the target.
                     */
                    ShowActivity
                        ("** %s found: Servo: x: %f  y: %f",
                         td.name, horizontalDestination, verticalDestination);
                }
                else
                {
                    ShowActivity("** %s not found", td.name);
                }
            }

            dashboardData.UpdateAndSend();

            // sleep to keep loop at constant rate
            // elapsed time can vary significantly due to debug printout
            currentTime = GetTime();
            lastTime = currentTime;
            if (loopTime > ElapsedTime(lastTime))
            {
                Wait(loopTime - ElapsedTime(lastTime));
            }
        }

        while (IsOperatorControl())
        {
            // determine if tank or arcade mode; default with no jumper is
            // for tank drive
            if (ds->GetDigitalIn(ARCADE_MODE) == 0)
            {
                // drive with tank style
                myRobot->TankDrive(leftStick, rightStick);
            }
            else
            {
                // drive with arcade style (use right stick)
                myRobot->ArcadeDrive(rightStick);
            }
        }
    } // end operator control
Esempio n. 3
0
    void Autonomous(void)
    {
        char funcName[] = "Autonomous";
        DPRINTF(LOG_DEBUG, "start VisionDemo autonomous");
        //GetWatchdog().Feed();

        // image data for tracking
        ColorMode mode = IMAQ_HSL;      // RGB or HSL
        //      TrackingThreshold td = GetTrackingData(RED, FLUORESCENT);
        TrackingThreshold td = GetTrackingData(GREEN, FLUORESCENT);

        int  panIncrement = 0;  // pan needs a 1-up number for each call

        DPRINTF(LOG_DEBUG, "SERVO - looking for COLOR %s ", td.name);

        /* initialize position and destination variables
         * position settings range from -1 to 1
         * setServoPositions is a wrapper that handles the conversion to
         * range for servo
         */
        horizontalDestination = 0.0; // final destination range -1.0 to +1.0
        verticalDestination = 0.0;

        // current position range -1.0 to +1.0
        horizontalPosition = RangeToNormalized(horizontalServo->Get(), 1);
        verticalPosition = RangeToNormalized(verticalServo->Get(), 1);

        // incremental tasking toward dest (-1.0 to 1.0)
        float incrementH, incrementV;

        // set servos to start at center position
        setServoPositions(horizontalDestination, verticalDestination);

        /* for controlling loop execution time */
        float loopTime = 0.05;
        double currentTime = GetTime();
        double lastTime = currentTime;

        double savedImageTimestamp = 0.0;

        bool foundColor = false;
        bool staleImage = false;

        while (IsAutonomous())
        {
            /* calculate gimbal position based on color found */
            if (FindColor
                (mode, &td.hue, &td.saturation, &td.luminance, &par,
                 &cReport))
            {
                foundColor = true;
                panIncrement = 0;       // reset pan
                if (par.imageTimestamp == savedImageTimestamp)
                {
                    // This image has been processed already,
                    // so don't do anything for this loop
                    staleImage = true;
                }
                else
                {
                    staleImage = false;
                    savedImageTimestamp = par.imageTimestamp;
                    // compute final H & V destination
                    horizontalDestination = par.center_mass_x_normalized;
                    verticalDestination = par.center_mass_y_normalized;
                }

//                ShowActivity("Found color   ");
            }
            else
            {                   // need to pan
                foundColor = false;
//                ShowActivity("No color found");
            }

            PrintReport(&cReport);

            if (foundColor && !staleImage)
            {
                /* Move the servo a bit each loop toward the destination.
                 * Alternative ways to task servos are to move immediately
                 * vs.  incrementally toward the final
                 * destination. Incremental method reduces the need for
                 * calibration of the servo movement while moving toward
                 * the target.
                 */
                incrementH = horizontalDestination - horizontalPosition;
                incrementV = verticalPosition - verticalDestination;
                adjustServoPositions(incrementH, -incrementV);

                ShowActivity
                    ("** %s found: Servo: x: %f  y: %f  increment: %f  y: %f  ",
                     td.name, horizontalDestination, verticalDestination,
                     incrementH, incrementV);
            }
            else if (!staleImage)
            {
                /* pan to find color after a short wait to settle servos
                 * panning must start directly after panInit or timing
                 * will be off
                 */

                // adjust sine wave for panning based on last movement
                // direction
                if (horizontalDestination > 0.0)
                {
                    sinStart = PI / 2.0;
                }
                else
                {
                    sinStart = -PI / 2.0;
                }

                if (panIncrement == 3)
                {
                    panInit();
                }
                else if (panIncrement > 3)
                {
                    panForTarget(horizontalServo, sinStart);
                    /* Vertical action: center the vertical after several
                     * loops searching */
                    if (panIncrement == 20)
                    {
                        verticalServo->Set(0.5);
                    }
                }
                panIncrement++;
            } // end if found color

            dashboardData.UpdateAndSend();
            // sleep to keep loop at constant rate
            // elapsed time can vary significantly due to debug printout
            currentTime = GetTime();
            lastTime = currentTime;
            if (loopTime > ElapsedTime(lastTime))
            {
                Wait(loopTime - ElapsedTime(lastTime));
            }
        } // end while

        myRobot->Drive(0.0, 0.0); // stop robot
        DPRINTF(LOG_DEBUG, "end autonomous");
        ShowActivity
            ("Autonomous end                                            ");

    } // end autonomous