HistogramReport* frcHistogram(const Image* image, int numClasses, float min, float max, Rect rect)
{
int success;
int fillValue = 1;
/* create the region of interest */
ROI* pRoi = imaqCreateROI();
success = imaqAddRectContour(pRoi, rect);
if ( !success ) { GetLastVisionError(); return NULL; }
/* make a mask from the ROI */
Image* pMask = frcCreateImage(IMAQ_IMAGE_U8);
success = imaqROIToMask(pMask, pRoi, fillValue, NULL, NULL);
if ( !success ) {
GetLastVisionError();
frcDispose(__FUNCTION__, pRoi, NULL);
return NULL;
}
/* get a histogram report */
HistogramReport* pHr = NULL;
pHr = imaqHistogram(image, numClasses, min, max, pMask);
/* clean up */
frcDispose(__FUNCTION__, pRoi, pMask, NULL);
return pHr;
}
/**
* Get an image from camera and store it on the cRIO
* @param imageName stored on home directory of cRIO ( "/" )
**/
void TakeSnapshot(char* imageName)
{
/* allow writing to vxWorks target */
//Priv_SetWriteFileAllowed(1);
DPRINTF(LOG_DEBUG, "taking a SNAPSHOT ");
Image* cameraImage = frcCreateImage(IMAQ_IMAGE_HSL);
if (!cameraImage) {
DPRINTF (LOG_INFO,"frcCreateImage failed - errorcode %i",GetLastVisionError());
}
if ( !camera166->GetImage(cameraImage) ) {
DPRINTF (LOG_INFO,"\nImage Acquisition from camera failed %i", GetLastVisionError());
} else {
DPRINTF (LOG_DEBUG, "calling frcWriteImage for %s", imageName);
if (!frcWriteImage(cameraImage, imageName) ) {
int errCode = GetLastVisionError();
DPRINTF (LOG_INFO,"frcWriteImage failed - errorcode %i", errCode);
char *errString = GetVisionErrorText(errCode);
DPRINTF (LOG_INFO,"errString= %s", errString);
} else {
DPRINTF (LOG_INFO,"\n>>>>> Saved image to %s", imageName);
// always dispose of image objects when done
frcDispose(cameraImage);
}
}
}
/**
* Constructor for this robot subclass.
* Create an instance of a RobotDrive with left and right motors
* plugged into PWM ports 1 and 2 on the first digital module. The two
* servos are PWM ports 3 and 4.
*/
VisionServoDemo(void)
{
ds = DriverStation::GetInstance();
myRobot = new RobotDrive(1, 2, 0.5); // robot will use PWM 1-2 for drive motors
rightStick = new Joystick(1); // create the joysticks
leftStick = new Joystick(2);
horizontalServo = new Servo(3); // create horizontal servo
verticalServo = new Servo(4); // create vertical servo
servoDeadband = 0.01; // move if > this amount
sinStart = 0.0; // control whether to start panning up or down
m_pDashboard = &(m_ds->GetDashboardPacker());
/* set up debug output:
* DEBUG_OFF, DEBUG_MOSTLY_OFF, DEBUG_SCREEN_ONLY,
* DEBUG_FILE_ONLY, DEBUG_SCREEN_AND_FILE
*/
SetDebugFlag(DEBUG_FILE_ONLY);
/* start the CameraTask */
if (StartCameraTask(20, 0, k160x120, ROT_180) == -1)
{
DPRINTF(LOG_ERROR,
"Failed to spawn camera task; exiting. Error code %s",
GetVisionErrorText(GetLastVisionError()));
}
m_pVideoServer = new PCVideoServer;
/* allow writing to vxWorks target */
Priv_SetWriteFileAllowed(1);
/* stop the watchdog if debugging */
GetWatchdog().SetEnabled(false);
}
/**
* Constructor for this robot subclass.
* Create an instance of a RobotDrive with left and right motors plugged into PWM
* ports 1 and 2 on the first digital module. The two servos are PWM ports 3 and 4.
* Tested with camera settings White Balance: Flurorescent1, Brightness 40, Exposure Auto
*/
TwoColorDemo(void)
{
ds = DriverStation::GetInstance();
myRobot = new RobotDrive(1, 2, 0.5); // robot will use PWM 1-2 for drive motors
rightStick = new Joystick(1); // create the joysticks
leftStick = new Joystick(2);
// remember to use jumpers on the sidecar for the Servo PWMs
horizontalServo = new Servo(9); // create horizontal servo on PWM 9
verticalServo = new Servo(10); // create vertical servo on PWM 10
servoDeadband = 0.01; // move if > this amount
framesPerSecond = 15; // number of camera frames to get per second
sinStart = 0.0; // control where to start the sine wave for pan
memset(&par, 0, sizeof(par)); // initialize particle analysis report
/* image data for tracking - override default parameters if needed */
/* recommend making PINK the first color because GREEN is more
* subsceptible to hue variations due to lighting type so may
* result in false positives */
// PINK
sprintf(td1.name, "PINK");
td1.hue.minValue = 220;
td1.hue.maxValue = 255;
td1.saturation.minValue = 75;
td1.saturation.maxValue = 255;
td1.luminance.minValue = 85;
td1.luminance.maxValue = 255;
// GREEN
sprintf(td2.name, "GREEN");
td2.hue.minValue = 55;
td2.hue.maxValue = 125;
td2.saturation.minValue = 58;
td2.saturation.maxValue = 255;
td2.luminance.minValue = 92;
td2.luminance.maxValue = 255;
/* set up debug output:
* DEBUG_OFF, DEBUG_MOSTLY_OFF, DEBUG_SCREEN_ONLY, DEBUG_FILE_ONLY, DEBUG_SCREEN_AND_FILE
*/
SetDebugFlag(DEBUG_SCREEN_ONLY);
/* start the CameraTask */
if (StartCameraTask(framesPerSecond, 0, k320x240, ROT_0) == -1)
{
DPRINTF(LOG_ERROR,
"Failed to spawn camera task; exiting. Error code %s",
GetVisionErrorText(GetLastVisionError()));
}
/* allow writing to vxWorks target */
Priv_SetWriteFileAllowed(1);
/* stop the watchdog if debugging */
GetWatchdog().SetExpiration(0.5);
GetWatchdog().SetEnabled(false);
}
TargetDetector::TargetDetector(void)
{
ds = DriverStation::GetInstance();
framesPerSecond = 15; // number of camera frames to get per second
cameraRotation = ROT_0; // input parameter for camera orientation
memset(&par,0,sizeof(par)); // initialize particle analysis report
/* image data for tracking - override default parameters if needed */
/* recommend making PINK the first color because GREEN is more
* subsceptible to hue variations due to lighting type so may
* result in false positives */
// PINK
sprintf (td1.name, "PINK");
td1.hue.minValue = 220;
td1.hue.maxValue = 255;
td1.saturation.minValue = 75;
td1.saturation.maxValue = 255;
td1.luminance.minValue = 85;
td1.luminance.maxValue = 255;
// GREEN
sprintf (td2.name, "GREEN");
td2.hue.minValue = 55;
td2.hue.maxValue = 125;
td2.saturation.minValue = 58;
td2.saturation.maxValue = 255;
td2.luminance.minValue = 92;
td2.luminance.maxValue = 255;
/* set up debug output:
* DEBUG_OFF, DEBUG_MOSTLY_OFF, DEBUG_SCREEN_ONLY, DEBUG_FILE_ONLY, DEBUG_SCREEN_AND_FILE
*/
SetDebugFlag(DEBUG_SCREEN_ONLY);
/* start the CameraTask */
if (StartCameraTask(framesPerSecond, 0, k320x240, ROT_180) == -1)
{
DPRINTF( LOG_ERROR,"Failed to spawn camera task; exiting. Error code %s",
GetVisionErrorText(GetLastVisionError()) );
}
Wait(1.0);
StartImageAcquisition();
/* allow writing to vxWorks target */
Priv_SetWriteFileAllowed(1);
savedImageTimestamp = 0.0;
}
/**
* start the CameraTask
**/
void StartCamera()
{
/* read a configuration file and send it to the camera */
char *imageName = "166StartPic.png";
//camera166 = AxisCamera::getInstance();
camera166 = &AxisCamera::GetInstance();
if ( 0 == camera166) {
DPRINTF( LOG_DEBUG,"Failed to spawn camera task; exiting. Error code %s",
GetVisionErrorText(GetLastVisionError()) );
} else {
SetupCamera(resolution, rotation);
TakeSnapshot(imageName);
}
}
/**
* @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;
}
/**
* @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;
}
/**
* @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;
}
