double VisionSubsystemV2::scoreAspectRatio(BinaryImage *image, ParticleAnalysisReport *report, bool outer) {
double rectLong,
rectShort,
idealAspectRatio,
aspectRatio;
if (outer) {
idealAspectRatio = (62/29);
} else {
idealAspectRatio = (62/20);
}
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (scoreAspectRatio) Using the aspect ratio of %f to score with\n", idealAspectRatio);
#endif
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &rectLong);
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &rectShort);
if (report->boundingRect.width > report->boundingRect.height) {
aspectRatio = 100*(1 - fabs((1 - ((rectLong / rectShort) / idealAspectRatio))));
} else {
aspectRatio = 100*(1 - fabs((1 - ((rectShort / rectLong) / idealAspectRatio))));
}
double score = (max(0.0, min(aspectRatio, 1000.00)));
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (scoreAspectRatio) The score is %f\n", score);
#endif
return score;
}
/**
* NOTE: This is included temporarily until the updated WPILib
* is released with the proper headers and immplmentation for
* this method. It will be in TrackAPI.cpp.
*
* @brief Find the largest particle that meets a criteria
* @param binaryImage Image to inspect
* @param rect area to search
* @return 0 = error
*/
bool TempInArea(Image* binaryImage, int particleIndex, Rect rect)
{
char funcName[]="InArea";
double position;
imaqMeasureParticle(binaryImage, particleIndex, 0,
IMAQ_MT_BOUNDING_RECT_LEFT, &position);
if ( position < (rect.left ) ) return false; // outside left of rectangle?
imaqMeasureParticle(binaryImage, particleIndex, 0,
IMAQ_MT_BOUNDING_RECT_TOP, &position);
if ( position < (rect.top ) ) return false; // outside top of rectangle ?
imaqMeasureParticle(binaryImage, particleIndex, 0,
IMAQ_MT_BOUNDING_RECT_RIGHT, &position);
if (position > (rect.left + rect.width) ) return false; // outside right of rectangle ?
imaqMeasureParticle(binaryImage, particleIndex, 0,
IMAQ_MT_BOUNDING_RECT_BOTTOM, &position);
if (position > (rect.top + rect.height) ) return false; // outside bottom of rectangle ?
DPRINTF(LOG_INFO, "particle %i is in (%i %i) height %i width %i\n",
particleIndex, rect.left, rect.top, rect.height, rect.width);
return true;
}
double VisionControl::AspectRatio(BinaryImage *image, ParticleAnalysisReport *report)
{
double rectLong, rectShort, aspectRatio;
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &rectLong);
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &rectShort);
aspectRatio = rectLong/rectShort;
return aspectRatio;
}
/**
* @brief Conduct measurements for a single particle in an images.
* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL.
*
* @param image image with the particle to analyze. This function modifies the source image.
* If you need the original image, create a copy of the image using frcCopy() before using this function.
* @param particleNumber The number of the particle to get information on
* @param par on return, a particle analysis report containing information about the particle. This structure must be created by the caller.
* @return On success: 1. On failure: 0. To get extended error information, call GetLastError().
*/
int frcParticleAnalysis(Image* image, int particleNumber, ParticleAnalysisReport* par)
{
int success = 0;
/* image information */
int height, width;
if ( ! imaqGetImageSize(image, &width, &height) ) { return success; }
par->imageWidth = width;
par->imageHeight = height;
par->particleIndex = particleNumber;
/* center of mass point of the largest particle */
double returnDouble;
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_CENTER_OF_MASS_X, &returnDouble);
if ( !success ) { return success; }
par->center_mass_x = (int)returnDouble; // pixel
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_CENTER_OF_MASS_Y, &returnDouble);
if ( !success ) { return success; }
par->center_mass_y = (int)returnDouble; // pixel
/* particle size statistics */
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_AREA, &returnDouble);
if ( !success ) { return success; }
par->particleArea = returnDouble;
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_BOUNDING_RECT_TOP, &returnDouble);
if ( !success ) { return success; }
par->boundingRect.top = (int)returnDouble;
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_BOUNDING_RECT_LEFT, &returnDouble);
if ( !success ) { return success; }
par->boundingRect.left = (int)returnDouble;
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_BOUNDING_RECT_HEIGHT, &returnDouble);
if ( !success ) { return success; }
par->boundingRect.height = (int)returnDouble;
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &returnDouble);
if ( !success ) { return success; }
par->boundingRect.width = (int)returnDouble;
/* particle quality statistics */
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_AREA_BY_IMAGE_AREA, &returnDouble);
if ( !success ) { return success; }
par->particleToImagePercent = returnDouble;
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_AREA_BY_PARTICLE_AND_HOLES_AREA, &returnDouble);
if ( !success ) { return success; }
par->particleQuality = returnDouble;
/* normalized position (-1 to 1) */
par->center_mass_x_normalized = RangeToNormalized(par->center_mass_x, width);
par->center_mass_y_normalized = RangeToNormalized(par->center_mass_y, height);
return success;
}
double VisionSubsystem::scoreAspectRatio(BinaryImage *image, ParticleAnalysisReport *report, bool outer) {
double rectLong, rectShort, idealAspectRatio, aspectRatio;
idealAspectRatio = outer ? (62/29) : (62/20);
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &rectLong);
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &rectShort);
if(report->boundingRect.width > report->boundingRect.height) {
aspectRatio = 100*(1-fabs((1-((rectLong/rectShort)/idealAspectRatio))));
} else {
aspectRatio = 100*(1-fabs((1-((rectShort/rectLong)/idealAspectRatio))));
}
return (max(0.0, min(aspectRatio, 1000.00)));
}
/**
* Measure a single parameter for an image.
* Get the measurement for a single parameter about an image by calling the imaqMeasureParticle
* function for the selected parameter.
* @param particleNumber which particle in the set of particles
* @param whatToMeasure the imaq MeasurementType (what to measure)
* @param result the value of the measurement
* @returns true on failure, false on success
*/
bool BinaryImage::ParticleMeasurement(int particleNumber, MeasurementType whatToMeasure, double *result)
{
int success;
success = imaqMeasureParticle(m_imaqImage, particleNumber, 0, whatToMeasure, result);
wpi_setImaqErrorWithContext(success, "Error measuring particle");
return !StatusIsFatal();
}
int GetLargestParticle(Image* binaryImage, int* largestParticleIndex, Rect rect)
{
*largestParticleIndex = 0; // points to caller-provided variable
/* determine number of particles in thresholded image */
int numParticles;
int success = frcCountParticles(binaryImage, &numParticles);
if ( !success ) { return success; }
/* if no particles found we can quit here */
if (numParticles == 0) { return 0; } // unsuccessful if zero particles found
// find the largest particle
double largestParticleArea = 0;
double particleArea;
for (int i = 0; i < numParticles; ++i) {
success = imaqMeasureParticle(binaryImage, i, 0, IMAQ_MT_AREA, &particleArea);
if ( !success ) { return success; }
if (particleArea > largestParticleArea) {
// see if is in the right area
if ( InArea(binaryImage, i, rect) ) {
largestParticleArea = particleArea;
*largestParticleIndex = i; // return index to caller
}
}
}
return success;
}
/**
* Computes a score (0-100) comparing the aspect ratio to the ideal aspect ratio for the target. This method uses
* the equivalent rectangle sides to determine aspect ratio as it performs better as the target gets skewed by moving
* to the left or right. The equivalent rectangle is the rectangle with sides x and y where particle area= x*y
* and particle perimeter= 2x+2y
*
* @param image The image containing the particle to score, needed to perform additional measurements
* @param report The Particle Analysis Report for the particle, used for the width, height, and particle number
* @param outer Indicates whether the particle aspect ratio should be compared to the ratio for the inner target or the outer
* @return The aspect ratio score (0-100)
*/
double TargetReport::scoreAspectRatio(BinaryImage *image, ParticleAnalysisReport *report, bool outer){
double rectLong, rectShort, idealAspectRatio, aspectRatio;
idealAspectRatio = outer ? (62/29) : (62/20); //Dimensions of goal opening + 4 inches on all 4 sides for reflective tape
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &rectLong);
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &rectShort);
//Divide width by height to measure aspect ratio
if(report->boundingRect.width > report->boundingRect.height){
//particle is wider than it is tall, divide long by short
aspectRatio = 100*(1-fabs((1-((rectLong/rectShort)/idealAspectRatio))));
} else {
//particle is taller than it is wide, divide short by long
aspectRatio = 100*(1-fabs((1-((rectShort/rectLong)/idealAspectRatio))));
}
return (max(0, min(aspectRatio, 100))); //force to be in range 0-100
}
/**
* Computes a score (0-100) comparing the aspect ratio to the ideal aspect ratio for the target. This method uses
* the equivalent rectangle sides to determine aspect ratio as it performs better as the target gets skewed by moving
* to the left or right. The equivalent rectangle is the rectangle with sides x and y where particle area= x*y
* and particle perimeter= 2x+2y
*
* @param image The image containing the particle to score, needed to perform additional measurements
* @param report The Particle Analysis Report for the particle, used for the width, height, and particle number
* @param outer Indicates whether the particle aspect ratio should be compared to the ratio for the inner target or the outer
* @return The aspect ratio score (0-100)
*/
inline double scoreAspectRatio(BinaryImage *image, ParticleAnalysisReport *report, bool vertical){
double rectLong, rectShort, idealAspectRatio, aspectRatio;
idealAspectRatio = vertical ? (4.0/32) : (23.5/4); //Vertical reflector 4" wide x 32" tall, horizontal 23.5" wide x 4" tall
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &rectLong);
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &rectShort);
//Divide width by height to measure aspect ratio
if(report->boundingRect.width > report->boundingRect.height){
//particle is wider than it is tall, divide long by short
aspectRatio = ratioToScore(((rectLong/rectShort)/idealAspectRatio));
} else {
//particle is taller than it is wide, divide short by long
aspectRatio = ratioToScore(((rectShort/rectLong)/idealAspectRatio));
}
return aspectRatio; //force to be in range 0-100
}
/**
* @brief Find the largest particles that meet a criteria
* @param binaryImage Image to inspect
* @param hitReport structure containing arry of hits - first hit is largest
* @param rect area to search
* @param numParticles Number of particles in array
* @return 0 = error
*/
int GetLargestParticles(Image* binaryImage, ImageHits *hitReport, Rect rect,
int numberHitsRequested)
{
//char funcName[]="GetLargestParticles";
HitList *hitsInArea = new HitList(); // list to contain all particles within area sorted by size
int i;
/* determine number of particles in thresholded image */
int numParticles = -1;
int success = frcCountParticles(binaryImage, &numParticles);
if ( !success ) { return success; }
//DPRINTF(LOG_DEBUG, "particles requested = %i ; particles found in image = %i", numberHitsRequested, numParticles);
/* if no particles found we can quit here */
if (numParticles <= 0) { return 1; } // successful, but zero particles found
/* get areas of each particle and insert into list */
double particleArea;
int count = 0;
for (i = 0; i < numParticles; ++i) {
success = imaqMeasureParticle(binaryImage, i, 0, IMAQ_MT_AREA, &particleArea);
if ( !success ) { return success; }
//DPRINTF (LOG_INFO, "size of particle %i = %g", i, particleArea);
// see if is in the right area and large enough to be considered a possible target
//TODO: Call InArea & delete TempInArea when new WPILib is released
//if (InArea(binaryImage, i, rect)) {
//if ( InArea(binaryImage, i, rect) && (particleArea >= FRC_MINIMUM_PIXELS_FOR_TARGET) ) {
if ( TempInArea(binaryImage, i, rect) && (particleArea >= FRC_MINIMUM_PIXELS_FOR_TARGET) ) {
hitsInArea->AddNode(i,particleArea);
count++;
}
}
// see what's in the list
hitsInArea->debugDump();
// fill in return structure - number of hits
if (numParticles < numberHitsRequested) {
hitReport->numberOfHits = numParticles;
} else {
hitReport->numberOfHits = numberHitsRequested;
}
// fill in return structure - indices & areas of largest hits
HitNode *hitPtr = hitsInArea->head;
for (i = 0; i < hitReport->numberOfHits; ++i) {
if (hitPtr == NULL) {
break;
}
hitReport->indices[i] = hitPtr->nodeIndex;
hitReport->areas[i] = hitPtr->nodeArea;
//DPRINTF (LOG_INFO, "put index %i in hitReport %i", hitPtr->nodeIndex, hitReport->indices[i]);
hitPtr = hitPtr->next;
}
// dispose of HitList
delete hitsInArea;
return 1; // success
}
/**
* Measure a single parameter for an image.
* Get the measurement for a single parameter about an image by calling the imaqMeasureParticle
* function for the selected parameter.
* @param image the image to measure
* @param particleNumber which particle in the set of particles
* @param whatToMeasure the imaq MeasurementType (what to measure)
* @returns the value of the measurement
*/
double BinaryImage::ParticleMeasurement(int particleNumber, MeasurementType whatToMeasure)
{
double returnDouble;
int success;
success = imaqMeasureParticle(m_imaqImage, particleNumber, 0, whatToMeasure, &returnDouble);
wpi_imaqAssert(success, "Error measuring particle");
if (!success) return 0.0;
else return returnDouble;
}
double VisionSubsystemV2::computeTargetDistance(BinaryImage *image, ParticleAnalysisReport *report, VisionSubsystemV2::Target target) {
double rectShort, height, targetHeight = -1;
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeDistance) Image Hight: %d, Image Width: %d\n", image->GetHeight(), image->GetWidth());
#endif
if(target == High) {
targetHeight = (20.0 / 12.0);
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeDistance) Using High target height %f\n", targetHeight);
#endif
} else if(target == Middle) {
targetHeight = (29.0 / 12.0);
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeDistance) Using Middle target height %f\n", targetHeight);
#endif
} else if(target == Low) {
targetHeight = (32.0 / 12.0);
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeDistance) Using Low target height %f\n", targetHeight);
#endif
}
//imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &rectShort);
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE_FERET, &rectShort);
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeDistance) imaq rectangle short side is %f\n", rectShort);
#endif
double repoertedOfBoundingRect = report->boundingRect.height;
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeDistance) Bounding rectangle heigth %f\n", repoertedOfBoundingRect);
#endif
height = min(rectShort, repoertedOfBoundingRect);
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeDistance) The Height being used is %f\n", height);
#endif
double tangent = VisionSubsystemV2::tanDegress(VIEW_ANGLE_V2 / 2);
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeDistance) tanget of %f is %f\n", (VIEW_ANGLE_V2 / 2), tangent);
#endif
//double distance = ((((targetHeight / Y_IMAGE_RES) / height ) / 2) / tangent);
double FOV = (targetHeight * Y_IMAGE_RES) / height;
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeDistance) Field of view is %f\n", FOV);
#endif
double distance = (FOV / 2)/ tangent;
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeDistance) Distance is %f\n", distance);
#endif
return distance;
}
/**
* Computes the estimated distance to a target using the height of the particle in the image. For more information and graphics
* showing the math behind this approach see the Vision Processing section of the ScreenStepsLive documentation.
*
* @param image The image to use for measuring the particle estimated rectangle
* @param report The Particle Analysis Report for the particle
* @param outer True if the particle should be treated as an outer target, false to treat it as a center target
* @return The estimated distance to the target in Inches.
*/
double TargetReport::computeDistance (BinaryImage *image, ParticleAnalysisReport *report, bool outer) {
double rectShort, height;
int targetHeight;
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &rectShort);
//using the smaller of the estimated rectangle short side and the bounding rectangle height results in better performance
//on skewed rectangles
height = min(report->boundingRect.height, rectShort);
targetHeight = outer ? 29 : 21;
return X_IMAGE_RES * targetHeight / (height * 12 * 2 * tan(VIEW_ANGLE*PI/(180*2)));
}
/**
* Computes the estimated distance to a target using the height of the particle in the image. For more information and graphics
* showing the math behind this approach see the Vision Processing section of the ScreenStepsLive documentation.
*
* @param image The image to use for measuring the particle estimated rectangle
* @param report The Particle Analysis Report for the particle
* @return The estimated distance to the target in feet.
*/
inline double computeDistance (BinaryImage *image, ParticleAnalysisReport *report) {
double rectLong, height;
int targetHeight;
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &rectLong);
//using the smaller of the estimated rectangle long side and the bounding rectangle height results in better performance
//on skewed rectangles
height = min(report->boundingRect.height, rectLong);
targetHeight = 32;
return Y_IMAGE_RES * targetHeight / (height * 12 * 2 * tan(VIEW_ANGLE*PI/(180*2)));
}
double VisionSubsystemV2::computeTargetAzimuth(BinaryImage *image, ParticleAnalysisReport *report, Target target, double targetDistance) {
double targetWidthPixelImaq;
double targetWidthFeet = -1;
if(target == High) {
targetWidthFeet = (62.0 / 12.0);
} else if(target == Middle) {
targetWidthFeet = (62.0 / 12.0);
} else if(target == Low) {
targetWidthFeet = (37.0 / 12.0);
}
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeTargetAzimuth) The target width in feet is %f\n", targetWidthFeet);
#endif
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &targetWidthPixelImaq);
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeTargetAzimuth) The imaq pixel width is %f\n", targetWidthPixelImaq);
#endif
double targetWidthPixelBoundingRect = report->boundingRect.width;
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeTargetAzimuth) The Bounding rectangle width is %f\n", targetWidthPixelBoundingRect);
#endif
double targetWidthPixel = min(targetWidthPixelBoundingRect, targetWidthPixelBoundingRect);
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeTargetAzimuth) The target width being used is %f\n", targetWidthPixel);
#endif
double fov = (targetWidthFeet * image->GetWidth()) / targetWidthPixel;
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeTargetAzimuth) The horizontal field of view is %f\n", fov);
#endif
double prePreTan = ((report->center_mass_x - (image->GetWidth() / 2)) * (fov / 2)) / image->GetWidth();
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeTargetAzimuth) the distance from teh center in feet is %f\n", prePreTan);
#endif
double preTan = prePreTan / targetDistance;
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeTargetAzimuth) The value before tan is %f\n", preTan);
#endif
double azimuth = atan(preTan) * (180.0 / PI_V2);
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (computeTargetAzimuth) the azimuth is %f\n", azimuth);
#endif
return azimuth;
}
/****** AUTO FUNCTIONS END *******/
void Autonomous()
{
int counter=0;
int autonomousEngagement = 0;
DriverStationLCD *screen = DriverStationLCD::GetInstance();
compressor.Start(); //starts compressor class
rightArmSolenoid.Set(DoubleSolenoid::kReverse); //brings the arms down
leftArmSolenoid.Set(DoubleSolenoid::kReverse);
/*** ENSURES THE CATAPULT IS LOADED AND LOADS IF UNLOADED ***/
if (leftLimitSwitch.Get() == 1 && rightLimitSwitch.Get() == 1)
{
winchMotor.Set(0.1); // Gears need to be moving slowly to allow the dog gear to engage properly
dogSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to engage the dog gear
Wait(0.2); // Giving the pistons time to engage properly
winchMotor.Set(0); // Now that the dog gear is engaged, the gears do not have to move
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to engage the ratchet
Wait(0.2); // Giving the pistons time to engage properly
}
while (leftLimitSwitch.Get() == 1 && rightLimitSwitch.Get() == 1) // If Limit Switch Buttons are not pressed
{
winchMotor.Set(1); //Now starts the winch motor to load the catapult
}
// If the Catapult Left & Limit Switches are (0,0), (0,1), (1,0)
{
winchMotor.Set(0); // Stops the Winch Motor since one or more buttons are pressed
if ((dogSolenoid.Get() == DoubleSolenoid::kReverse) && (ratchetSolenoid.Get() == DoubleSolenoid::kForward)) // If the Dog Gear is disengaged but the ratchet is engaged
{
winchMotor.Set(0.05); // Gears need to be moving slowly to allow the dog gear to engage properly. Might want to test this since the catapult's already loaded.
dogSolenoid.Set(DoubleSolenoid::kForward); // Engages the dog gear so both dog gear and ratchet are engaged before shooting for safety
Wait(0.1); // Giving the pistons time to engage properly
winchMotor.Set(0); // Now that the dog gear is engaged, the gears do not have to move
}
else if ((dogSolenoid.Get() == DoubleSolenoid::kForward) && (ratchetSolenoid.Get() == DoubleSolenoid::kReverse)) // If the dog gear is engaged but the ratchet is disengaged
{
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Engages the ratchet so that both dog gear and ratchet are engaged before shooting for safety
Wait(0.1); // Giving the pistons time to engage properly
}
}
/*** DONE LOADING THE CATAPULT ***/
float pLower = 5; // min height of rectangle for comparison
float pUpper = 15; // max height of rectangle for comparison
int criteriaCount = 1; // number of elements to include/exclude at a time
int rejectMatches = 1; // when set to true, particles that do not meet the criteria are discarded
int connectivity = 1; // declares connectivity value as 1; so corners are not ignored
int filterFunction; // removes small blobs
int borderSetting; // variable to store border settings, limit for rectangle
int borderSize = 1; // border for the camera frame (if you don't put this, DriverStation gets mad at you)
ParticleFilterCriteria2 particleCriteria;
ParticleFilterOptions2 particleFilterOptions;
int numParticles;
particleCriteria.parameter = IMAQ_MT_BOUNDING_RECT_HEIGHT; //The Morphological measurement we use
particleCriteria.lower = pLower; // The lower bound of the criteria range
particleCriteria.upper = pUpper; // The upper bound of the criteria range
particleCriteria.calibrated = FALSE; // We aren't calibrating to real world measurements. We don't need this.
particleCriteria.exclude = TRUE; // Remove all particles that aren't in specific pLower and pUpper range
particleFilterOptions.rejectMatches = rejectMatches; // Set to 1 above, so images that do not meet the criteria are discarded
particleFilterOptions.rejectBorder = 0; // Set to 0 over here so border images are not discarded
particleFilterOptions.connectivity8 = connectivity; // Sets the image image to 8 bit
while ((IsAutonomous()))
{
if (logitech.GetRawButton(4))
{
autonomousEngagement = 1;
}
if (autonomousEngagement == 0) // If real autonomous is not engaged start
{
if (logitech.GetRawButton(1))
{
driveForward();
}
if (logitech.GetRawButton(9))
{
dogSolenoid.Set(DoubleSolenoid::kForward); // Brings the pneumatic piston backward to raise the retrieval arm
winchMotor.Set(0.1);
Wait(0.3);
ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to lower the retrieval arm
while(leftLimitSwitch.Get()==1 && rightLimitSwitch.Get()==1)
{
winchMotor.Set(1);
}
}
if (logitech.GetRawButton(2))
{
autonomousCatapultRelease();
}
if (logitech.GetRawButton(3))
{
stopDriving();
}
if (logitech.GetRawButton(5))
{
turnLeft();
}
if (logitech.GetRawButton(7))
{
turnLeftMore();
}
if (logitech.GetRawButton(6))
{
turnRight();
}
if (logitech.GetRawButton(8))
{
turnRightMore();
}
}// If real autonomous is not engaged end
HSLImage* imgpointer; // declares an image container as an HSL (hue-saturation-luminence) image
imgpointer = camera.GetImage(); //tells camera to capture image
ringLight.Set(Relay::kForward); //turns ringlight on
BinaryImage* binIMG = NULL; // declares a container to hold a binary image
binIMG = imgpointer -> ThresholdHSL(0, 255, 0, 255, 235, 255); // thresholds HSL image and places in the binary image container
delete imgpointer; // deletes the HSL image to free up memory on the cRIO
Image* modifiedImage = imaqCreateImage(IMAQ_IMAGE_U8, 0); //create a binary 8-bit format shell for the image
filterFunction = imaqParticleFilter4(modifiedImage, binIMG -> GetImaqImage(), &particleCriteria, criteriaCount, &particleFilterOptions, NULL, &numParticles); //The Particle Filter Function we use. (The ones before it are outdated)
borderSetting = imaqSetBorderSize(modifiedImage, borderSize); // Sets a border size so DriverStation is happy
delete binIMG; //Deletes the Binary image
int functionCountParticles; // stores number of particles
int particleAmount; // stores the number of particles for the measure particle function
functionCountParticles = imaqCountParticles(modifiedImage, TRUE, &particleAmount); // Counts the number of particles
int functionOne; // The first measuring particle function (specifically for particle #1)
int functionTwo; // The second measuring particle function (specifically for particle #2)
double particleOneOrientation; // TRULY ARBITRARY name of the first particle it find
double particleTwoOrientation; // TRULY ARBITRARY name of the second particle it finds
functionOne = imaqMeasureParticle(modifiedImage, 0, FALSE, IMAQ_MT_ORIENTATION, &particleOneOrientation); // Measures orientation of particle 1
functionTwo = imaqMeasureParticle(modifiedImage, 1, FALSE, IMAQ_MT_ORIENTATION, &particleTwoOrientation); // Measures orientation of particle 2
screen->PrintfLine(DriverStationLCD::kUser_Line2,"P1: %f", particleOneOrientation); // Prints particle 1's orientation
screen->PrintfLine(DriverStationLCD::kUser_Line3,"P2: %f", particleTwoOrientation); // Prints particle 2's orientation
imaqDispose(modifiedImage); // Deletes the filtered image
/**LEFT POSITION**/
if ((leftPositionSwitch.Get() == 1) && (rightPositionSwitch.Get() == 0)) // Left switch set on, switch set off
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Left Position:F"); // Left position and facing forward
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10))
// The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Left Position Hot!");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRight();
//driveForward();
Wait(3);
stopDriving();
//autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Left Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRight();
driveForward();
Wait(4);
stopDriving();
//autonomousCatapultRelease();
}
}
/**CENTER POSITION**/
else if ((leftPositionSwitch.Get() == 0) && (rightPositionSwitch.Get() == 0)) // Left switch off and right switch off
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position:R"); // Middle position and facing
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10)) // The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeftMore();
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
}
/** RIGHT POSITION**/
else if ((leftPositionSwitch.Get() == 1) && (rightPositionSwitch.Get() == 1))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position:R"); // Middle position and facing
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10)) // The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeftMore();
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
}
else if (((leftPositionSwitch.Get()) == 1) && ((rightPositionSwitch.Get()) == 0)) // Left switch off and switch on
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Right Position"); // position and facing forward
if ((particleOneOrientation > 0 && particleOneOrientation < 10) || ((particleTwoOrientation > 0) && (particleTwoOrientation < 10))) // The target should be hot. Now it goes to the other goal.
/* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeft();
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line4, "Right Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
autonomousCatapultRelease();
}
}
counter++;
screen -> PrintfLine(DriverStationLCD::kUser_Line5,"R: %f L: %f)", rightFront.Get(), leftFront.Get());
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Counter %d", counter);
screen->UpdateLCD();
}
compressor.Stop();
}
bool Target::FindParticles()
{
printf("Target::FindParticles\n");
long then = (long) GetFPGATime();
// extract the blue plane
if (!imaqExtractColorPlanes(m_cameraImage.GetImaqImage(), IMAQ_RGB,
NULL, NULL, m_monoImage.GetImaqImage()))
{
printf("%s: imaqExtractColorPlanes FAILED\n", __FUNCTION__);
return false;
}
// select interesting particles
if (!imaqThreshold(m_threshold.GetImaqImage(), m_monoImage.GetImaqImage(),
/*rangeMin*/ THRESHOLD, /*rangeMax*/ 255, /*useNewValue*/ 1, /*newValue */ 1))
{
printf("%s: imaqThreshold FAILED\n", __FUNCTION__);
return false;
}
if (!imaqConvexHull(m_convexHull.GetImaqImage(), m_threshold.GetImaqImage(),
/*connectivity8*/ 1))
{
printf("%s: imaqConvexHull FAILED\n", __FUNCTION__);
return false;
}
if (!imaqSizeFilter(m_filtered.GetImaqImage(), m_convexHull.GetImaqImage(),
/*connectivity8*/ 1, /*erosions*/ 2, /*keepSize*/ IMAQ_KEEP_LARGE,
/*structuringElement*/ NULL))
{
printf("%s: imaqSizeFilter FAILED\n", __FUNCTION__);
return false;
}
int particleCount = 0;
if (!imaqCountParticles(m_filtered.GetImaqImage(), 1, &particleCount))
{
printf("%s: imaqCountParticles FAILED\n", __FUNCTION__);
return false;
}
// select the four largest particles (insertion sort)
// for now, keep track of only the particle number (index) and size
memset((void *)m_particles, 0, sizeof m_particles);
for (int i = 0; i < particleCount; i++) {
double size;
if (!imaqMeasureParticle(m_filtered.GetImaqImage(), i, FALSE,
IMAQ_MT_PARTICLE_AND_HOLES_AREA, &size))
{
printf("%s: imaqMeasureParticle %d FAILED\n", __FUNCTION__, i);
break;
}
for (int j = 0; j < 4; j++) {
if (size > m_particles[j].size) {
for (int k = 3; k > j; k--) {
m_particles[k].index = m_particles[k-1].index;
m_particles[k].size = m_particles[k-1].size;
}
m_particles[j].index = i;
m_particles[j].size = size;
break;
}
}
}
// fill in the rest of the measured data
for (m_numParticles = 0;
m_numParticles < 4 && m_particles[m_numParticles].size > 0;
m_numParticles++)
{
Particle* p = &m_particles[m_numParticles];
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_CENTER_OF_MASS_X, &(p->xCenter));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_CENTER_OF_MASS_Y, &(p->yCenter));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_BOUNDING_RECT_LEFT, &(p->leftBound));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_BOUNDING_RECT_RIGHT, &(p->rightBound));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_BOUNDING_RECT_TOP, &(p->topBound));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_BOUNDING_RECT_BOTTOM, &(p->bottomBound));
// calculate height/width from bounding box
p->height = p->bottomBound - p->topBound;
p->width = p->rightBound - p->leftBound;
}
long now = (long) GetFPGATime();
printf("%s: particle detection took %ld microseconds\n", __FUNCTION__, (now - then));
printf("%s: found %d particles\n", __FUNCTION__, particleCount);
printf("%s: returning %d particles\n", __FUNCTION__, m_numParticles);
for (int i = 0; i < m_numParticles; i++) {
Particle *p = &m_particles[i];
printf(" particle %d index %d top %g bottom %g left %g right %g size %g x %g y %g\n",
i, p->index, p->topBound, p->bottomBound, p->leftBound, p->rightBound,
p->size, p->xCenter, p->yCenter);
}
return true;
}
void Autonomous() override {
while (IsAutonomous() && IsEnabled())
{
//read file in from disk. For this example to run you need to copy image20.jpg from the SampleImages folder to the
//directory shown below using FTP or SFTP: http://wpilib.screenstepslive.com/s/4485/m/24166/l/282299-roborio-ftp
imaqError = imaqReadFile(frame, "//NewDir//image2.jpg", NULL, NULL);
//Update threshold values from SmartDashboard. For performance reasons it is recommended to remove this after calibration is finished.
TOTE_HUE_RANGE.minValue = SmartDashboard::GetNumber("Tote hue min", TOTE_HUE_RANGE.minValue);
TOTE_HUE_RANGE.maxValue = SmartDashboard::GetNumber("Tote hue max", TOTE_HUE_RANGE.maxValue);
TOTE_SAT_RANGE.minValue = SmartDashboard::GetNumber("Tote sat min", TOTE_SAT_RANGE.minValue);
TOTE_SAT_RANGE.maxValue = SmartDashboard::GetNumber("Tote sat max", TOTE_SAT_RANGE.maxValue);
TOTE_VAL_RANGE.minValue = SmartDashboard::GetNumber("Tote val min", TOTE_VAL_RANGE.minValue);
TOTE_VAL_RANGE.maxValue = SmartDashboard::GetNumber("Tote val max", TOTE_VAL_RANGE.maxValue);
//Threshold the image looking for yellow (tote color)
imaqError = imaqColorThreshold(binaryFrame, frame, 255, IMAQ_HSV, &TOTE_HUE_RANGE, &TOTE_SAT_RANGE, &TOTE_VAL_RANGE);
//Send particle count to dashboard
int numParticles = 0;
imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
SmartDashboard::PutNumber("Masked particles", numParticles);
//Send masked image to dashboard to assist in tweaking mask.
SendToDashboard(binaryFrame, imaqError);
//filter out small particles
float areaMin = SmartDashboard::GetNumber("Area min %", AREA_MINIMUM);
criteria[0] = {IMAQ_MT_AREA_BY_IMAGE_AREA, areaMin, 100, false, false};
imaqError = imaqParticleFilter4(binaryFrame, binaryFrame, criteria, 1, &filterOptions, NULL, NULL);
//Send particle count after filtering to dashboard
imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
SmartDashboard::PutNumber("Filtered particles", numParticles);
if(numParticles > 0)
{
//Measure particles and sort by particle size
std::vector<ParticleReport> particles;
for(int particleIndex = 0; particleIndex < numParticles; particleIndex++)
{
ParticleReport par;
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA_BY_IMAGE_AREA, &(par.PercentAreaToImageArea));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA, &(par.Area));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_CONVEX_HULL_AREA, &(par.ConvexHullArea));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_TOP, &(par.BoundingRectTop));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_LEFT, &(par.BoundingRectLeft));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_BOTTOM, &(par.BoundingRectBottom));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_RIGHT, &(par.BoundingRectRight));
particles.push_back(par);
}
sort(particles.begin(), particles.end(), CompareParticleSizes);
//This example only scores the largest particle. Extending to score all particles and choosing the desired one is left as an exercise
//for the reader. Note that the long and short side scores expect a single tote and will not work for a stack of 2 or more totes.
//Modification of the code to accommodate 2 or more stacked totes is left as an exercise for the reader.
scores.Trapezoid = TrapezoidScore(particles.at(0));
SmartDashboard::PutNumber("Trapezoid", scores.Trapezoid);
scores.LongAspect = LongSideScore(particles.at(0));
SmartDashboard::PutNumber("Long Aspect", scores.LongAspect);
scores.ShortAspect = ShortSideScore(particles.at(0));
SmartDashboard::PutNumber("Short Aspect", scores.ShortAspect);
scores.AreaToConvexHullArea = ConvexHullAreaScore(particles.at(0));
SmartDashboard::PutNumber("Convex Hull Area", scores.AreaToConvexHullArea);
bool isTote = scores.Trapezoid > SCORE_MIN && (scores.LongAspect > SCORE_MIN || scores.ShortAspect > SCORE_MIN) && scores.AreaToConvexHullArea > SCORE_MIN;
bool isLong = scores.LongAspect > scores.ShortAspect;
//Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote
SmartDashboard::PutBoolean("IsTote", isTote);
SmartDashboard::PutNumber("Distance", computeDistance(binaryFrame, particles.at(0), isLong));
} else {
SmartDashboard::PutBoolean("IsTote", false);
}
Wait(0.005); // wait for a motor update time
}
}
Vision_Out Run(Vision_In in)
{
Vision_Out out;
//read file in from disk. For this example to run you need to copy image.jpg from the SampleImages folder to the
//directory shown below using FTP or SFTP: http://wpilib.screenstepslive.com/s/4485/m/24166/l/282299-roborio-ftp
//imaqError = imaqReadFile(frame, "//home//lvuser//SampleImages//image.jpg", NULL, NULL);
imaqError = camera->GetImage(frame);
//Update threshold values from SmartDashboard. For performance reasons it is recommended to remove this after calibration is finished.
/*
RING_HUE_RANGE.minValue = SmartDashboard::GetNumber("Tote hue min", RING_HUE_RANGE.minValue);
RING_HUE_RANGE.maxValue = SmartDashboard::GetNumber("Tote hue max", RING_HUE_RANGE.maxValue);
RING_SAT_RANGE.minValue = SmartDashboard::GetNumber("Tote sat min", RING_SAT_RANGE.minValue);
RING_SAT_RANGE.maxValue = SmartDashboard::GetNumber("Tote sat max", RING_SAT_RANGE.maxValue);
RING_VAL_RANGE.minValue = SmartDashboard::GetNumber("Tote val min", RING_VAL_RANGE.minValue);
RING_VAL_RANGE.maxValue = SmartDashboard::GetNumber("Tote val max", RING_VAL_RANGE.maxValue);
*/
if(in.shouldProcess)
{
//Threshold the image looking for ring light color
imaqError = imaqColorThreshold(binaryFrame, frame, 255, IMAQ_HSV, &RING_HUE_RANGE, &RING_SAT_RANGE, &RING_VAL_RANGE);
//Send particle count to dashboard
int numParticles = 0;
imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
SmartDashboard::PutNumber("Masked particles", numParticles);
//Send masked image to dashboard to assist in tweaking mask.
SendToDashboard(binaryFrame, imaqError);
//filter out small particles
float areaMin = SmartDashboard::GetNumber("Area min %", AREA_MINIMUM);
criteria[0] = {IMAQ_MT_AREA_BY_IMAGE_AREA, areaMin, 100, false, false};
imaqError = imaqParticleFilter4(binaryFrame, binaryFrame, criteria, 1, &filterOptions, NULL, NULL);
//Send particle count after filtering to dashboard
imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
SmartDashboard::PutNumber("Filtered particles", numParticles);
if(numParticles > 0)
{
//Measure particles and sort by particle size
std::vector<ParticleReport> particles;
for(int particleIndex = 0; particleIndex < numParticles; particleIndex++)
{
ParticleReport par;
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA_BY_IMAGE_AREA, &(par.PercentAreaToImageArea));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA, &(par.Area));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_TOP, &(par.BoundingRectTop));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_LEFT, &(par.BoundingRectLeft));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_BOTTOM, &(par.BoundingRectBottom));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_RIGHT, &(par.BoundingRectRight));
particles.push_back(par);
}
sort(particles.begin(), particles.end(), CompareParticleSizes);
//This example only scores the largest particle. Extending to score all particles and choosing the desired one is left as an exercise
//for the reader. Note that this scores and reports information about a single particle (single L shaped target). To get accurate information
//about the location of the tote (not just the distance) you will need to correlate two adjacent targets in order to find the true center of the tote.
scores.Aspect = AspectScore(particles.at(0));
SmartDashboard::PutNumber("Aspect", scores.Aspect);
scores.Area = AreaScore(particles.at(0));
SmartDashboard::PutNumber("Area", scores.Area);
bool isTarget = scores.Area > SCORE_MIN && scores.Aspect > SCORE_MIN;
//Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote
SmartDashboard::PutBoolean("IsTarget", isTarget);
double distance = computeDistance(binaryFrame, particles.at(0));
SmartDashboard::PutNumber("Distance", computeDistance(binaryFrame, particles.at(0)));
out.returnDistanceToTote = distance;
out.returnIsTote = true;
return out;
}
else
{
out.returnIsTote = false;
SmartDashboard::PutBoolean("IsTarget", false);
}
}
return out;
}
inline void processTaskFunc(UINT32 hotGoalPtr...)
{
bool hotGoal = (bool *) hotGoalPtr;
Scores *scores;
TargetReport target;
int verticalTargets[MAX_PARTICLES];
int horizontalTargets[MAX_PARTICLES];
int verticalTargetCount, horizontalTargetCount;
Threshold threshold(0, 255, 0, 255, 220, 255); //HSV threshold criteria, ranges are in that order ie. Hue is 60-100
ParticleFilterCriteria2 criteria[] = {
{IMAQ_MT_AREA, AREA_MINIMUM, 65535, false, false}
}; //Particle filter criteria, used to filter out small particles
//AxisCamera &camera = AxisCamera::GetInstance(); //To use the Axis camera uncomment this line
/**
* Do the image capture with the camera and apply the algorithm described above. This
* sample will either get images from the camera or from an image file stored in the top
* level directory in the flash memory on the cRIO. The file name in this case is "testImage.jpg"
*/
ColorImage *image;
image = new RGBImage("/testImage.jpg"); // get the sample image from the cRIO flash
//image = camera.GetImage(); //To get the images from the camera comment the line above and uncomment this one
BinaryImage *thresholdImage = image->ThresholdHSV(threshold); // get just the green target pixels
//thresholdImage->Write("/threshold.bmp");
BinaryImage *filteredImage = thresholdImage->ParticleFilter(criteria, 1); //Remove small particles
//filteredImage->Write("Filtered.bmp");
vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports(); //get a particle analysis report for each particle
verticalTargetCount = horizontalTargetCount = 0;
//Iterate through each particle, scoring it and determining whether it is a target or not
if(reports->size() > 0)
{
scores = new Scores[reports->size()];
for (unsigned int i = 0; i < MAX_PARTICLES && i < reports->size(); i++) {
ParticleAnalysisReport *report = &(reports->at(i));
//Score each particle on rectangularity and aspect ratio
scores[i].rectangularity = scoreRectangularity(report);
scores[i].aspectRatioVertical = scoreAspectRatio(filteredImage, report, true);
scores[i].aspectRatioHorizontal = scoreAspectRatio(filteredImage, report, false);
//Check if the particle is a horizontal target, if not, check if it's a vertical target
if(scoreCompare(scores[i], false))
{
printf("particle: %d is a Horizontal Target centerX: %d centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
horizontalTargets[horizontalTargetCount++] = i; //Add particle to target array and increment count
} else if (scoreCompare(scores[i], true)) {
printf("particle: %d is a Vertical Target centerX: %d centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
verticalTargets[verticalTargetCount++] = i; //Add particle to target array and increment count
} else {
printf("particle: %d is not a Target centerX: %d centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
}
printf("Scores rect: %f ARvert: %f \n", scores[i].rectangularity, scores[i].aspectRatioVertical);
printf("ARhoriz: %f \n", scores[i].aspectRatioHorizontal);
}
//Zero out scores and set verticalIndex to first target in case there are no horizontal targets
target.totalScore = target.leftScore = target.rightScore = target.tapeWidthScore = target.verticalScore = 0;
target.verticalIndex = verticalTargets[0];
for (int i = 0; i < verticalTargetCount; i++)
{
ParticleAnalysisReport *verticalReport = &(reports->at(verticalTargets[i]));
for (int j = 0; j < horizontalTargetCount; j++)
{
ParticleAnalysisReport *horizontalReport = &(reports->at(horizontalTargets[j]));
double horizWidth, horizHeight, vertWidth, leftScore, rightScore, tapeWidthScore, verticalScore, total;
//Measure equivalent rectangle sides for use in score calculation
imaqMeasureParticle(filteredImage->GetImaqImage(), horizontalReport->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &horizWidth);
imaqMeasureParticle(filteredImage->GetImaqImage(), verticalReport->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &vertWidth);
imaqMeasureParticle(filteredImage->GetImaqImage(), horizontalReport->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &horizHeight);
//Determine if the horizontal target is in the expected location to the left of the vertical target
leftScore = ratioToScore(1.2*(verticalReport->boundingRect.left - horizontalReport->center_mass_x)/horizWidth);
//Determine if the horizontal target is in the expected location to the right of the vertical target
rightScore = ratioToScore(1.2*(horizontalReport->center_mass_x - verticalReport->boundingRect.left - verticalReport->boundingRect.width)/horizWidth);
//Determine if the width of the tape on the two targets appears to be the same
tapeWidthScore = ratioToScore(vertWidth/horizHeight);
//Determine if the vertical location of the horizontal target appears to be correct
verticalScore = ratioToScore(1-(verticalReport->boundingRect.top - horizontalReport->center_mass_y)/(4*horizHeight));
total = leftScore > rightScore ? leftScore:rightScore;
total += tapeWidthScore + verticalScore;
//If the target is the best detected so far store the information about it
if(total > target.totalScore)
{
target.horizontalIndex = horizontalTargets[j];
target.verticalIndex = verticalTargets[i];
target.totalScore = total;
target.leftScore = leftScore;
target.rightScore = rightScore;
target.tapeWidthScore = tapeWidthScore;
target.verticalScore = verticalScore;
}
}
//Determine if the best target is a Hot target
target.Hot = hotOrNot(target);
}
if(verticalTargetCount > 0)
{
//Information about the target is contained in the "target" structure
//To get measurement information such as sizes or locations use the
//horizontal or vertical index to get the particle report as shown below
ParticleAnalysisReport *distanceReport = &(reports->at(target.verticalIndex));
double distance = computeDistance(filteredImage, distanceReport);
if(target.Hot)
{
printf("Hot target located \n");
printf("Distance: %f \n", distance);
hotGoal = true;
} else {
printf("No hot target present \n");
printf("Distance: %f \n", distance);
hotGoal = false;
}
}
}
// be sure to delete images after using them
delete filteredImage;
delete thresholdImage;
delete image;
//delete allocated reports and Scores objects also
delete scores;
delete reports;
}
////////////////////////////////////////////////////////////////////////////////
//
// Function Name: IVA_Particle
//
// Description : Computes the number of particles detected in a binary image and
// a 2D array of requested measurements about the particle.
//
// Parameters : image - Input image
// connectivity - Set this parameter to 1 to use
// connectivity-8 to determine
// whether particles are touching.
// Set this parameter to 0 to use
// connectivity-4 to determine
// whether particles are touching.
// pixelMeasurements - Array of measuremnets parameters
// numPixelMeasurements - Number of elements in the array
// calibratedMeasurements - Array of measuremnets parameters
// numCalibratedMeasurements - Number of elements in the array
// ivaData - Internal Data structure
// stepIndex - Step index (index at which to store
// the results in the resuts array)
//
// Return Value : success
//
////////////////////////////////////////////////////////////////////////////////
static int IVA_Particle(Image* image,
int connectivity,
int pPixelMeasurements[],
int numPixelMeasurements,
int pCalibratedMeasurements[],
int numCalibratedMeasurements,
IVA_Data* ivaData,
int stepIndex)
{
int success = 1;
int numParticles;
double* pixelMeasurements = NULL;
double* calibratedMeasurements = NULL;
unsigned int visionInfo;
IVA_Result* particleResults;
int i;
int j;
double centerOfMassX;
double centerOfMassY;
//-------------------------------------------------------------------//
// Particle Analysis //
//-------------------------------------------------------------------//
// Counts the number of particles in the image.
VisionErrChk(imaqCountParticles(image, connectivity, &numParticles));
// Allocate the arrays for the measurements.
pixelMeasurements = (double*)malloc(numParticles * numPixelMeasurements * sizeof(double));
calibratedMeasurements = (double*)malloc(numParticles * numCalibratedMeasurements * sizeof(double));
// Delete all the results of this step (from a previous iteration)
IVA_DisposeStepResults(ivaData, stepIndex);
// Check if the image is calibrated.
VisionErrChk(imaqGetVisionInfoTypes(image, &visionInfo));
// If the image is calibrated, we also need to log the calibrated position (x and y)
ivaData->stepResults[stepIndex].numResults = (visionInfo & IMAQ_VISIONINFO_CALIBRATION ?
numParticles * 4 + 1 : numParticles * 2 + 1);
ivaData->stepResults[stepIndex].results = malloc (sizeof(IVA_Result) * ivaData->stepResults[stepIndex].numResults);
particleResults = ivaData->stepResults[stepIndex].results;
#if defined (IVA_STORE_RESULT_NAMES)
sprintf(particleResults->resultName, "Object #");
#endif
particleResults->type = IVA_NUMERIC;
particleResults->resultVal.numVal = numParticles;
particleResults++;
for (i = 0 ; i < numParticles ; i++)
{
// Computes the requested pixel measurements about the particle.
for (j = 0 ; j < numPixelMeasurements ; j++)
{
VisionErrChk(imaqMeasureParticle(image, i, FALSE, pPixelMeasurements[j], &pixelMeasurements[i*numPixelMeasurements + j]));
}
// Computes the requested calibrated measurements about the particle.
for (j = 0 ; j < numCalibratedMeasurements ; j++)
{
VisionErrChk(imaqMeasureParticle(image, i, TRUE, pCalibratedMeasurements[j], &calibratedMeasurements[i*numCalibratedMeasurements + j]));
}
#if defined (IVA_STORE_RESULT_NAMES)
sprintf(particleResults->resultName, "Particle %d.X Position (Pix.)", i + 1);
#endif
particleResults->type = IVA_NUMERIC;
VisionErrChk(imaqMeasureParticle(image, i, FALSE, IMAQ_MT_CENTER_OF_MASS_X, ¢erOfMassX));
particleResults->resultVal.numVal = centerOfMassX;
particleResults++;
#if defined (IVA_STORE_RESULT_NAMES)
sprintf(particleResults->resultName, "Particle %d.Y Position (Pix.)", i + 1);
#endif
particleResults->type = IVA_NUMERIC;
VisionErrChk(imaqMeasureParticle(image, i, FALSE, IMAQ_MT_CENTER_OF_MASS_Y, ¢erOfMassY));
particleResults->resultVal.numVal = centerOfMassY;
particleResults++;
if (visionInfo & IMAQ_VISIONINFO_CALIBRATION)
{
#if defined (IVA_STORE_RESULT_NAMES)
sprintf(particleResults->resultName, "Particle %d.X Position (Calibrated)", i + 1);
#endif
particleResults->type = IVA_NUMERIC;
VisionErrChk(imaqMeasureParticle(image, i, TRUE, IMAQ_MT_CENTER_OF_MASS_X, ¢erOfMassX));
particleResults->resultVal.numVal = centerOfMassX;
particleResults++;
#if defined (IVA_STORE_RESULT_NAMES)
sprintf(particleResults->resultName, "Particle %d.Y Position (Calibrated)", i + 1);
#endif
particleResults->type = IVA_NUMERIC;
VisionErrChk(imaqMeasureParticle(image, i, TRUE, IMAQ_MT_CENTER_OF_MASS_Y, ¢erOfMassY));
particleResults->resultVal.numVal = centerOfMassY;
particleResults++;
}
}
Error:
free(pixelMeasurements);
free(calibratedMeasurements);
return success;
}
void Autonomous()
{
DriverStationLCD *screen = DriverStationLCD::GetInstance();
while ((IsAutonomous()))
{
HSLImage* imgpointer; // declares an image container as an HSL (hue-saturation-luminence) image
imgpointer = camera.GetImage(); //tells camera to capture image
backpack.Set(Relay::kForward); //turns ringlight on
BinaryImage* binIMG = NULL; // declares a container to hold a binary image
binIMG = imgpointer -> ThresholdHSL(0, 255, 0, 255, 235, 255); // thresholds HSL image and places in the binary image container
delete imgpointer; // deletes the HSL image to free up memory on the cRIO
Image* Kirby = imaqCreateImage(IMAQ_IMAGE_U8, 0); //create 8 bit image
Image* KirbyTwo = imaqCreateImage(IMAQ_IMAGE_U8, 0); // creates the second 8-bit image that we can use separately for counting particles.
// (The first image gets eaten by the measureparticle function)
float pLower = 175; // min height of rectangle for comparison
float pUpper = 500; // max height of rectangle for comparison
int criteriaCount = 1; // number of elements to include/exclude at a time
int rejectMatches = 1; // when set to true, particles that do not meet the criteria are discarded
int connectivity = 1; // declares connectivity value as 1; so corners are not ignored
int Polturgust3000; // removes small blobs
int borderSetting; // variable to store border settings, limit for rectangle
int cloningDevice; // we create another image because the ParticleMeasuring steals the image from particlecounter
int borderSize = 1; // border for the camera frame (if you don't put this, DriverStation gets mad at you)
ParticleFilterCriteria2 particleCriteria;
ParticleFilterOptions2 particleFilterOptions;
int numParticles;
particleCriteria.parameter = IMAQ_MT_AREA; //The Morphological measurement we use
particleCriteria.lower = pLower; // The lower bound of the criteria range
particleCriteria.upper = pUpper; // The upper bound of the criteria range
particleCriteria.calibrated = FALSE; // We aren't calibrating to real world measurements. We don't need this.
particleCriteria.exclude = TRUE; // Remove all particles that aren't in specific pLower and pUpper range
particleFilterOptions.rejectMatches = rejectMatches; // Set to 1 above, so images that do not meet the criteria are discarded
particleFilterOptions.rejectBorder = 0; // Set to 0 over here so border images are not discarded
particleFilterOptions.connectivity8 = connectivity; // Sets the image image to 8 bit
Polturgust3000 = imaqParticleFilter4(Kirby, binIMG -> GetImaqImage(), &particleCriteria, criteriaCount, &particleFilterOptions, NULL, &numParticles); //The Particle Filter Function we use. (The ones before it are outdated)
borderSetting = imaqSetBorderSize(Kirby, borderSize); // Sets a border size
cloningDevice = imaqDuplicate(KirbyTwo, Kirby); //Officially creating a duplicate of the first image to count the number of particles.
delete binIMG; //Deletes the Binary image
int ParticleCounter; // stores number of particles
int* countparticles; // stores the number of particles for the measure particle function
ParticleCounter = imaqCountParticles(Kirby, TRUE, countparticles); // Counts the number of particles to be sent off later to the MeasureParticle function. Then it gets eaten by the measureparticle function
int TinyRuler; // TRULY ARBITRARY name of the first measuring particle function (specifically for particle #1)
int BabyYardstick; // TRULY ARBITRARY Name of the second measuring particle function (specifically for particle #2)
double* unowidth; // TRULY ARBITRARY name of the first particle it find
double* doswidth; // TRULY ARBITRARY name of the second particle it finds
TinyRuler = imaqMeasureParticle(Kirby, 0, FALSE, IMAQ_MT_BOUNDING_RECT_WIDTH, unowidth); // Function of measuring rectangle width is applied to particle 1 (unowidth)
BabyYardstick = imaqMeasureParticle(Kirby, 1, FALSE, IMAQ_MT_BOUNDING_RECT_WIDTH, doswidth); // Function of measuring width is applied to particle 2 (doswidth)
screen->PrintfLine(DriverStationLCD::kUser_Line3,"W1: %f",*unowidth); // Prints the applied information to particle 1. (Rectangle width)
screen->PrintfLine(DriverStationLCD::kUser_Line4,"W2: %f",*doswidth);
imaqDispose(Kirby);
imaqDispose(KirbyTwo);
if (((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 1))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Left Position");
if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
// Even this needs to be tested
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Left Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRight();
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Left Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
}
//both on
else if (((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 1))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position");
if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeftMore();
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
else if (((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 0))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position");
if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRightMore();
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Middle Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
shootCatapult();
driveForward();
Wait(3);
stopDriving();
}
}
//Left button on && right off
else if (((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 0))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Right Position");
if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Right Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeft();
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Right Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
}
Wait(0.005);
screen -> UpdateLCD();
}
}
}
void Camera::GetInfo() {
Image* binFrame;
binFrame = imaqCreateImage(IMAQ_IMAGE_U8, 0);
Range Hue = {hueMin, hueMax};
Range Sat = {satMin, satMax};
Range Val = {valMin, valMax};
ParticleFilterCriteria2 criteria[1];
//ParticleFilterOptions2 filterOptions = {0, 0, 1, 1};
criteria[0] = {IMAQ_MT_AREA, 0, (float) aireMin, false, true};
int nbParticles(0);
IMAQdxGrab(session, frame, true, NULL);
imaqScale(frame, frame, 2, 2, ScalingMode::IMAQ_SCALE_SMALLER, IMAQ_NO_RECT);
imaqColorThreshold(binFrame, frame, 255, IMAQ_HSV, &Hue, &Sat, &Val);
imaqMorphology(binFrame, binFrame, IMAQ_DILATE, NULL);
//imaqParticleFilter4(binFrame, binFrame, &criteria[0], 1, &filterOptions, NULL, &nbParticles);
imaqCountParticles(binFrame, 0, &nbParticles);
CameraServer::GetInstance()->SetImage(binFrame);
int indexMax(0);
double aireMax(0);
if(nbParticles > 0) {
for(int particleIndex = 0; particleIndex < nbParticles; particleIndex++){
double aire (0);
imaqMeasureParticle(binFrame, particleIndex, 0, IMAQ_MT_AREA, &aire);
if (aire > aireMax){
aireMax = aire;
indexMax = particleIndex;
}
}
double hypothenuse(0);
int hauteurImage(0);
int largeurImage(0);
double centreX(0);
double centreY(0);
double angleX(0);
double angleY(0);
double offset(0);
imaqMeasureParticle(binFrame, indexMax, 0, IMAQ_MT_CENTER_OF_MASS_X, ¢reX);
imaqMeasureParticle(binFrame, indexMax, 0, IMAQ_MT_CENTER_OF_MASS_Y, ¢reY);
imaqGetImageSize(binFrame, &largeurImage, &hauteurImage);
double dHauteurImage(hauteurImage);
double dLargeurImage(largeurImage);
//Normalisation
centreX = ((2 * centreX) / dLargeurImage) - 1;
centreY = ((-2 * centreY) / dHauteurImage) + 1;
angleX = atan(centreX * tan(FOV_X * acos(-1) / 180.0));
angleY = atan(centreY * tan(FOV_Y * acos(-1) / 180.0));
distance = (TARGET_HEIGHT - CAMERA_HEIGHT) / tan(CAMERA_ANGLE * acos(-1) / 180 + angleY);
hypothenuse = sqrt(pow(distance, 2) + pow(TARGET_HEIGHT - CAMERA_HEIGHT, 2));
offset = hypothenuse * tan(angleX);
ecart = offset - CAMERA_OFFSET;
SmartDashboard::PutNumber("Distance Cible", distance);
SmartDashboard::PutNumber("Angle Cible", ecart);
SmartDashboard::PutNumber("Aire Particule", aireMax);
SmartDashboard::PutNumber("Nombre Particules", nbParticles);
SmartDashboard::PutNumber("Hypothenuse", hypothenuse);
SmartDashboard::PutNumber("Largeur image", largeurImage);
}
}
