/**
* @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;
}
/**
* 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
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