BinaryImage *BinaryImage::ParticleFilter(ParticleFilterCriteria2 *criteria, int criteriaCount)
{
BinaryImage *result = new BinaryImage();
int numParticles;
ParticleFilterOptions2 filterOptions = {0, 0, 0, 1};
int success = imaqParticleFilter4(result->GetImaqImage(), m_imaqImage, criteria, criteriaCount, &filterOptions, NULL, &numParticles);
wpi_setImaqErrorWithContext(success, "Error in particle filter operation");
return result;
}
/****** 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();
}
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 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;
}
