/**
* @brief Search for color particles. Supports IMAQ_IMAGE_HSL and IMAQ_IMAGE_RGB.
* @param mode Color mode, either IMAQ_HSL or IMAQ_RGB
* @param plane1Range The range for the first plane (hue or red)
* @param plane2Range The range for the second plane (saturation or green)
* @param plane3Range The range for the third plane (luminance or blue)
* @param hitReport Contains array of numberHits largest targets
* @param rect Rectangle to confine search to
* @param numberHitsRequested maximum number to return
* @return 0 = error
*/
int FindColorHits(ColorMode mode, const Range* plane1Range, const Range* plane2Range,
const Range* plane3Range, ImageHits* hitReport, Rect rect,
int numberHitsRequested)
{
char funcName[]="FindColorHits";
int errorCode = 0;
int success = 0;
/* fixed limit to number of hits processed
* larger # takes more processing time but can eliminate failure due to
* interference for instance, a red or green area in the stands or
* closer targets of the wrong alliance
*/
if (numberHitsRequested > FRC_MAX_HITS) numberHitsRequested = FRC_MAX_HITS;
/* create an image object */
Image* cameraImage = frcCreateImage(IMAQ_IMAGE_HSL);
if (!cameraImage) { return success; }
/* get image from camera - if the camera has not finished initializing,
* this will fail */
success = GetImage(cameraImage,NULL);
if (!success){
DPRINTF(LOG_INFO, "No camera Image available Error = %i %s",
errorCode, GetVisionErrorText(errorCode));
frcDispose(cameraImage);
imaqSetError(errorCode, funcName); //reset error code for the caller
return success;
}
/* Color threshold the image */
success = frcColorThreshold(cameraImage, cameraImage, mode, plane1Range, plane2Range, plane3Range);
if ( !success ) {
errorCode = GetLastVisionError();
DPRINTF (LOG_DEBUG, "Error = %i %s ", errorCode, GetVisionErrorText(errorCode));
frcDispose(funcName,cameraImage,NULL);
return success;
}
/* get largest particles that meet criteria */
/* GetLargestParticles fills out only the indices in the hitReport */
success = GetLargestParticles(cameraImage, hitReport, rect, numberHitsRequested);
if ( !success ) {
errorCode = GetLastVisionError();
DPRINTF (LOG_DEBUG, "Error after GetLargestParticles=%i %s ", errorCode, GetVisionErrorText(errorCode));
frcDispose(funcName,cameraImage,NULL);
imaqSetError(ERR_COLOR_NOT_FOUND, funcName);
return success;
}
if (hitReport->numberOfHits==0) {
imaqSetError(ERR_COLOR_NOT_FOUND, funcName);
frcDispose(funcName,cameraImage,NULL);
return 0;
}
//DPRINTF(LOG_INFO, "number hits found = %i\n", hitReport->numberOfHits);
/* Particles were found --
* Fill in report information for the largest particles
*/
double imageTime = GetTime();
for (int i=0; i<hitReport->numberOfHits; ++i) {
hitReport->pars[i].imageTimestamp = imageTime;
success = frcParticleAnalysis(cameraImage, hitReport->indices[i], &hitReport->pars[i]);
}
/* clean up */
frcDispose(funcName,cameraImage,NULL);
return success;
}
/**
* @brief Search for a color. Supports IMAQ_IMAGE_HSL and IMAQ_IMAGE_RGB.
* @param mode Color mode, either IMAQ_HSL or IMAQ_RGB
* @param plane1Range The range for the first plane (hue or red)
* @param plane2Range The range for the second plane (saturation or green)
* @param plane3Range The range for the third plane (luminance or blue)
* @param trackReport Values for tracking: center of particle, particle size, etc
* @param colorReport Color charactaristics of the particle
* @param rect Rectangle to confine search to
* @return 0 = error
*/
int FindColor(ColorMode mode, const Range* plane1Range, const Range* plane2Range,
const Range* plane3Range, ParticleAnalysisReport *trackReport,
ColorReport *colorReport, Rect rect)
{
int errorCode = 0;
int success = 0;
/* create an image object */
Image* cameraImage = frcCreateImage(IMAQ_IMAGE_HSL);
if (!cameraImage) { return success; }
/* get image from camera - if the camera has not finished initializing,
* this will fail
*/
double imageTime;
success = GetImage(cameraImage, &imageTime);
if (!success){
DPRINTF(LOG_INFO, "No camera Image available Error = %i %s",
errorCode, GetVisionErrorText(errorCode));
frcDispose(cameraImage);
imaqSetError(errorCode, __FUNCTION__); //reset error code for the caller
return success;
}
/* save a copy of the image to another image for color thresholding later */
Image* histImage = frcCreateImage(IMAQ_IMAGE_HSL);
if (!histImage) { frcDispose(cameraImage); return success; }
success = frcCopyImage(histImage,cameraImage);
if ( !success ) {
errorCode = GetLastVisionError();
frcDispose(__FUNCTION__,cameraImage,histImage,NULL);
return success;
}
/* Color threshold the image */
success = frcColorThreshold(cameraImage, cameraImage, mode, plane1Range, plane2Range, plane3Range);
if ( !success ) {
errorCode = GetLastVisionError();
DPRINTF (LOG_DEBUG, "Error = %i %s ", errorCode, GetVisionErrorText(errorCode));
frcDispose(__FUNCTION__,cameraImage,histImage,NULL);
return success;
}
int largestParticleIndex = 0;
success = GetLargestParticle(cameraImage, &largestParticleIndex, rect );
if ( !success ) {
errorCode = GetLastVisionError();
DPRINTF (LOG_DEBUG, "Error after GetLargestParticle = %i %s ", errorCode, GetVisionErrorText(errorCode));
frcDispose(__FUNCTION__,cameraImage,histImage,NULL);
imaqSetError(ERR_COLOR_NOT_FOUND, __FUNCTION__);
return success;
}
DPRINTF(LOG_INFO, "largestParticleIndex = %i\n", largestParticleIndex);
/* Particles were found */
/*
* Fill in report information for largest particle found
*/
success = frcParticleAnalysis(cameraImage, largestParticleIndex, trackReport);
trackReport->imageTimestamp = imageTime;
/* clean up */
if (!success) {frcDispose(__FUNCTION__,cameraImage,histImage,NULL); return success;}
/* particle color statistics */
/* only if a color report requested */
if (colorReport != NULL)
{
/* first filter out the other particles */
ParticleFilterCriteria2 criteria;
ParticleFilterOptions* options = NULL;
Rect rect;
int numParticles;
success = frcParticleFilter(cameraImage, cameraImage, &criteria, 1, options,
rect, &numParticles);
if ( !success ) {
DPRINTF(LOG_INFO, "frcParticleFilter errorCode %i", GetLastVisionError());
}
/* histogram the original image using the thresholded image as a mask */
int numClasses = 10; //how many classes?
ColorHistogramReport* chrep = imaqColorHistogram2(histImage, numClasses, IMAQ_HSL,
NULL, cameraImage);
if (chrep == NULL) {
DPRINTF(LOG_INFO, "NULL Color Histogram");
errorCode = GetLastVisionError();
} else {
colorReport->particleHueMax = chrep->plane1.max;
colorReport->particleHueMin = chrep->plane1.min;
colorReport->particleHueMean = chrep->plane1.mean;
colorReport->particleSatMax = chrep->plane2.max;
colorReport->particleSatMin = chrep->plane2.min;
colorReport->particleSatMean = chrep->plane2.mean;
colorReport->particleLumMax = chrep->plane3.max;
colorReport->particleLumMin = chrep->plane3.min;
colorReport->particleLumMean = chrep->plane3.mean;
colorReport->numberParticlesFound = numParticles;
frcDispose(chrep);
}
}
/* clean up */
frcDispose(__FUNCTION__,cameraImage,histImage,NULL);
return success;
}
// Main function of the task
int CameraServo::Main(int a2, int a3, int a4, int a5,
int a6, int a7, int a8, int a9, int a10)
{
Proxy *proxy; // Handle to proxy
Robot *lHandle; // Local handle
CameraServoLog sl; // log
#if 0
unsigned timer = 0; // Timer to only do certain things so often
int largestParticleIndex = 0;
#endif
// Let the world know we're in
DPRINTF(LOG_DEBUG,"In the 166 Template task\n");
// Wait for Robot go-ahead (e.g. entering Autonomous or Tele-operated mode)
WaitForGoAhead();
// Register our logger
lHandle = Robot::getInstance();
lHandle->RegisterLogger(&sl);
// Register the proxy
proxy = Proxy::getInstance();
float CamJoystickX = 0.0;
float CamJoystickY = 0.0;
float CamX = 0.5;
float CamY = 0.5;
#define DEADBAND (0.2)
#define CAMMOVE (.5 / (1000/CAMERA_SERVO_CYCLE_TIME))
// General main loop (while in Autonomous or Tele mode)
while (true) {
if(!proxy->get("Joy3BT")) {
CamJoystickX = proxy->get("Joy3X");
CamJoystickY = proxy->get("Joy3Y");
if(CamJoystickX >= DEADBAND) {
CamX += CAMMOVE;
} else if(CamJoystickX <= -DEADBAND) {
CamX -= CAMMOVE;
}
if(CamJoystickY >= DEADBAND) {
CamY -= CAMMOVE;
} else if(CamJoystickY <= -DEADBAND) {
CamY += CAMMOVE;
}
if(CamX > 1.0) {
CamX = 1.0;
} else if(CamX < .0) {
CamX = .0;
}
if(CamY > 1.0) {
CamY = 1.0;
} else if(CamY < .0) {
CamY = .0;
}
} else {
CamX = CamY = 0.5;
}
#if 0
if ((++timer) % (500/CAMERA_SERVO_CYCLE_TIME) == 0 && camera.IsFreshImage()) {
timer = 0;
camera.GetImage(srcimage);
frcColorThreshold(destimage, srcimage, IMAQ_HSL, &Hue_Range, &Sat_Range, &Lum_Range);
GetLargestParticle(destimage,&largestParticleIndex);
if(largestParticleIndex != 0) {
frcParticleAnalysis(destimage, largestParticleIndex, &Particle_Report);
printf("\tLargest Area: %f\n", Particle_Report.particleArea);
} else {
printf("No particle found...\n");
}
}
#endif
cameraX.Set(CamX);
cameraY.Set(CamY);
// Logging any values
sl.PutOne();
// Wait for our next lap
WaitForNextLoop();
}
return (0);
};
