////////////////////////////////////////////////////////////////////////////////
//
// Function Name: IVA_DisposeData
//
// Description : Releases the memory allocated in the IVA_Data structure
//
// Parameters : ivaData - Internal data structure
//
// Return Value : success
//
////////////////////////////////////////////////////////////////////////////////
static int IVA_DisposeData(IVA_Data* ivaData)
{
int i;
// Releases the memory allocated for the image buffers.
for (i = 0 ; i < IVA_MAX_BUFFERS ; i++)
imaqDispose(ivaData->buffers[i]);
// Releases the memory allocated for the array of measurements.
for (i = 0 ; i < ivaData->numSteps ; i++)
IVA_DisposeStepResults(ivaData, i);
free(ivaData->stepResults);
// Dispose of coordinate systems
if (ivaData->numCoordSys)
{
free(ivaData->baseCoordinateSystems);
free(ivaData->MeasurementSystems);
}
free(ivaData);
return true;
}
void CameraServer::FreeImageData(std::tuple<uint8_t*, unsigned int, unsigned int, bool> imageData) {
if (std::get<3>(imageData)) imaqDispose(std::get<0>(imageData));
else if (std::get<0>(imageData) != nullptr) {
std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
m_dataPool.push_back(std::get<0>(imageData));
}
}
////////////////////////////////////////////////////////////////////////////////
//
// Function Name: IVA_CLRThreshold
//
// Description : Thresholds a color image.
//
// Parameters : image - Input image
// min1 - Minimum range for the first plane
// max1 - Maximum range for the first plane
// min2 - Minimum range for the second plane
// max2 - Maximum range for the second plane
// min3 - Minimum range for the third plane
// max3 - Maximum range for the third plane
// colorMode - Color space in which to perform the threshold
//
// Return Value : success
//
////////////////////////////////////////////////////////////////////////////////
static int IVA_CLRThreshold(Image* image, int min1, int max1, int min2, int max2, int min3, int max3, int colorMode)
{
int success = 1;
Image* thresholdImage;
Range plane1Range;
Range plane2Range;
Range plane3Range;
//-------------------------------------------------------------------//
// Color Threshold //
//-------------------------------------------------------------------//
// Creates an 8 bit image for the thresholded image.
VisionErrChk(thresholdImage = imaqCreateImage(IMAQ_IMAGE_U8, 7));
// Set the threshold range for the 3 planes.
plane1Range.minValue = min1;
plane1Range.maxValue = max1;
plane2Range.minValue = min2;
plane2Range.maxValue = max2;
plane3Range.minValue = min3;
plane3Range.maxValue = max3;
// Thresholds the color image.
VisionErrChk(imaqColorThreshold(thresholdImage, image, 1, colorMode, &plane1Range, &plane2Range, &plane3Range));
// Copies the threshold image in the souce image.
VisionErrChk(imaqDuplicate(image, thresholdImage));
Error:
imaqDispose(thresholdImage);
return success;
}
double VisionSubsystem::scoreXEdge(BinaryImage *image, ParticleAnalysisReport *report) {
double total = 0;
LinearAverages *averages = imaqLinearAverages2(image->GetImaqImage(), IMAQ_COLUMN_AVERAGES, report->boundingRect);
for(int i=0; i < (averages->columnCount); i++) {
if(xMin[i*(XMINSIZE-1)/averages->columnCount] < averages->columnAverages[i] && averages->columnAverages[i] < xMax[i*(XMAXSIZE-1)/averages->columnCount]) {
total++;
}
}
total = 100*total/(averages->columnCount);
imaqDispose(averages);
return total;
}
int sgl_release_camera_data(CameraSgl *camera)
{
CameraData *data = &camera->data;
if (data->image != NULL) {
imaqDispose (data->image);
data->image = NULL;
}
data->buffer_num = 0;
return 0;
}
double VisionSubsystem::scoreYEdge(BinaryImage *image, ParticleAnalysisReport *report) {
double total = 0;
LinearAverages *averages = imaqLinearAverages2(image->GetImaqImage(), IMAQ_ROW_AVERAGES, report->boundingRect);
for(int i=0; i < (averages->rowCount); i++){
if(yMin[i*(YMINSIZE-1)/averages->rowCount] < averages->rowAverages[i]
&& averages->rowAverages[i] < yMax[i*(YMAXSIZE-1)/averages->rowCount]){
total++;
}
}
total = 100*total/(averages->rowCount); //convert to score 0-100
imaqDispose(averages); //let IMAQ dispose of the averages struct
return total;
}
/**
* @brief Dispose of a list of objects. Supports any object created on the heap.
*
* @param functionName The name of the function
* @param ... A list of pointers to structures that need to be disposed of.
* The last pointer in the list should always be set to NULL.
*
* @return On success: 1. On failure: 0. To get extended error information, call GetLastError().
*/
int frcDispose( const char* functionName, ... ) /* Variable argument list */
{
va_list disposalPtrList; /* Input argument list */
void* disposalPtr; /* For iteration */
int success, returnValue = 1;
va_start( disposalPtrList, functionName ); /* start of variable list */
disposalPtr = va_arg( disposalPtrList, void* );
while( disposalPtr != NULL ) {
success = imaqDispose(disposalPtr);
if (!success) {returnValue = 0;}
disposalPtr = va_arg( disposalPtrList, void* );
}
return returnValue;
}
double VisionSubsystemV2::scoreYEdge(BinaryImage *image, ParticleAnalysisReport *report) {
double total = 0;
LinearAverages *averages = imaqLinearAverages2(image->GetImaqImage(), IMAQ_ROW_AVERAGES, report->boundingRect);
for(int i=0; i < (averages->rowCount); i++){
if(yMinV2[i*(YMINSIZE-1)/averages->rowCount] < averages->rowAverages[i] && averages->rowAverages[i] < yMaxV2[i*(YMAXSIZE-1)/averages->rowCount]){
total++;
}
}
total = 100*total/(averages->rowCount); //convert to score 0-100
imaqDispose(averages); //let IMAQ dispose of the averages struct
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (scoreYEdge) The score of the Y-edge is %f\n", total);
#endif
return total;
}
double VisionSubsystemV2::scoreXEdge(BinaryImage *image, ParticleAnalysisReport *report) {
double total = 0;
LinearAverages *averages = imaqLinearAverages2(image->GetImaqImage(), IMAQ_COLUMN_AVERAGES, report->boundingRect);
for(int i = 0; i < (averages->columnCount); i++) {
if (xMinV2[i*(XMINSIZE - 1) / averages->columnCount] < averages->columnAverages[i] && averages->columnAverages[i] < xMaxV2[i*(XMAXSIZE - 1) / averages->columnCount]) {
total++;
}
}
total = 100*total/(averages->columnCount);
imaqDispose(averages);
#ifndef VISION_DEBUG_PRINTF_ENABLE
printf("[VisionSubsystemV2] (scoreXEdge) The score of the X-edge is %f\n", total);
#endif
return total;
}
/**
* Look for ellipses in an image.
* Given some input parameters, look for any number of ellipses in an image.
* @param ellipseDescriptor Ellipse descriptor
* @param curveOptions Curve options
* @param shapeDetectionOptions Shape detection options
* @param roi Region of Interest
* @returns a vector of EllipseMatch structures (0 length vector on no match)
*/
vector<EllipseMatch> * MonoImage::DetectEllipses(
EllipseDescriptor *ellipseDescriptor, CurveOptions *curveOptions,
ShapeDetectionOptions *shapeDetectionOptions, ROI *roi)
{
int numberOfMatches;
EllipseMatch *e = imaqDetectEllipses(m_imaqImage, ellipseDescriptor,
curveOptions, shapeDetectionOptions, roi, &numberOfMatches);
vector<EllipseMatch> *ellipses = new vector<EllipseMatch>;
if (e == NULL)
{
return ellipses;
}
for (int i = 0; i < numberOfMatches; i++)
{
ellipses->push_back(e[i]);
}
imaqDispose(e);
return ellipses;
}
/**
* Computes a score based on the match between a template profile and the particle profile in the X direction. This method uses the
* the column averages and the profile defined at the top of the sample to look for the solid vertical edges with
* a hollow center.
*
* @param image The image to use, should be the image before the convex hull is performed
* @param report The Particle Analysis Report for the particle
*
* @return The X Edge Score (0-100)
*/
double TargetReport::scoreXEdge(BinaryImage *image, ParticleAnalysisReport *report){
double total = 0;
const double xMax[XMAXSIZE] = {1.0, 1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5,
0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5,
0.5, 0.5, 0.5, 0.5, 1.0, 1.0, 1.0, 1.0};
const double xMin[XMINSIZE] = {0.4, 0.6, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1,
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1,
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.6, 0.0};
LinearAverages *averages = imaqLinearAverages2(image->GetImaqImage(), IMAQ_COLUMN_AVERAGES, report->boundingRect);
for(int i=0; i < (averages->columnCount); i++){
if(xMin[i*(XMINSIZE-1)/averages->columnCount] < averages->columnAverages[i]
&& averages->columnAverages[i] < xMax[i*(XMAXSIZE-1)/averages->columnCount]){
total++;
}
}
total = 100*total/(averages->columnCount); //convert to score 0-100
imaqDispose(averages); //let IMAQ dispose of the averages struct
return total;
}
/**
* @brief Dispose of one object. Supports any object created on the heap.
*
* @param object object to dispose of
* @return On success: 1. On failure: 0. To get extended error information, call GetLastError().
*/
int frcDispose(void* object) { return imaqDispose(object); }
/****** 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();
}
/**
* Frees memory associated with an ImageBase.
* Destructor frees the imaq image allocated with the class.
*/
ImageBase::~ImageBase()
{
if(m_imaqImage)
imaqDispose(m_imaqImage);
}
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 OperatorControl(void)
{
myRobot.SetSafetyEnabled(false);
AxisCamera &camera = AxisCamera::GetInstance("10.28.53.11");
camera.WriteResolution(AxisCamera::kResolution_320x240);
camera.WriteRotation(AxisCamera::kRotation_180);//Flip image upside-down.
camera.WriteCompression(80);//Compresses the image(?)
camera.WriteBrightness(50);//Sets the brightness to 80 on a scale from 0-100
DriverStationLCD *screen = DriverStationLCD::GetInstance();
int count = 0;
while(IsOperatorControl())
{
screen->UpdateLCD();
count++;
HSLImage* imgpointer; //declares a new hue saturation lum image
imgpointer = camera.GetImage(); //grabs an image to initialize that image
//imaqCreateImage(IMAQ_IMAGE_U8,)
//imaqCast(NULL, imgpointer, IMAQ_IMAGE_U8, NULL, -1);
BinaryImage* binImg = NULL; //declares a new binary image
//ThresholdHSL changes our regular image into a binary image.
/*hueLow Low value for hue
hueHigh High value for hue
saturationLow Low value for saturation
saturationHigh High value for saturation
luminenceLow Low value for luminence
luminenceHigh High value for luminence
*/
binImg = imgpointer->ThresholdHSL(1, 255, 1, 255, 230, 255);
//Saves the image to a temp directory
binImg->Write("/tmp/thresh.jpg");
//Writes the binary image to disk
imaqWriteFile(binImg->GetImaqImage(), "/tmp/thresh2.jpg", NULL);
delete imgpointer;
Image* Convex = imaqCreateImage(IMAQ_IMAGE_U8, 0);
int returnvalue;
returnvalue = imaqConvexHull(Convex, binImg->GetImaqImage(), TRUE);
// imaqWriteFile(Convex, "/tmp/convex.jpg", NULL);
delete binImg;
// screen->PrintfLine(DriverStationLCD::kUser_Line4,"convex is %d",returnvalue);
screen->UpdateLCD();
float lookuptable[256];
lookuptable[0] = 0;
lookuptable[1] = 65535;
//Converts image to 16 bit, then back to 8 bit.
Image* cast = imaqCreateImage(IMAQ_IMAGE_U16, 0);
imaqCast(cast, Convex, IMAQ_IMAGE_U16, lookuptable, 0);
imaqDispose(Convex);
Image* bitcast = imaqCreateImage(IMAQ_IMAGE_U8, 0);
imaqCast(bitcast, cast, IMAQ_IMAGE_U8, NULL, 0);
imaqDispose(cast);
// screen->PrintfLine(DriverStationLCD::kUser_Line3,"det %d", imaqGetLastError());//Notifies the user
imaqWriteFile(bitcast, "/tmp/bitcast.jpg", NULL);
//Image* SuperSize = im
//imaqCreateImage(IMAQ_IMAGE_U8, 2240);
//int returnvalue2;
//returnvalue2 = imaqSizeFilter(SuperSize, Convex, TRUE, 1, IMAQ_KEEP_LARGE, NULL);
//screen->UpdateLCD();
//imaqDispose (Convex);
// ROI *roi;
// Rect rectangle;
// rectangle.top = 0;
// rectangle.left = 0;
// rectangle.width = 320;
// rectangle.height = 120;
// imaqAddRectContour(roi,rectangle);
static RectangleDescriptor rectangleDescriptor =
{
30, // minWidth (All values are estimated)
200, // maxWidth
20, // minHeight
200 // maxHeight
};
static CurveOptions curveOptions = //extraction mode specifies what the VI identifies curves in the image. curve options are all the
{
IMAQ_NORMAL_IMAGE, // extractionMode
75, // threshold
IMAQ_NORMAL, // filterSize
25, // minLength
15, // rowStepSize
15, // columnStepSize
10, // maxEndPointGap
FALSE, // onlyClosed
FALSE // subpixelAccuracy
};
static ShapeDetectionOptions shapeOptions =
{
IMAQ_GEOMETRIC_MATCH_ROTATION_INVARIANT, // mode
NULL, // angle ranges
0, // num angle ranges
{75, 125}, // scale range
300 // minMatchScore
};
int matches = 0;
//double highscore = 0;
//int highestindex = -1;
float difference = 0;
float time = 0;
float average = 0;
double y = 0;
//The big important line of code that does important stuff
RectangleMatch* recmatch = imaqDetectRectangles(bitcast, &rectangleDescriptor, &curveOptions, &shapeOptions, NULL, &matches);
// DashboardDataSender *dds;
// dds = new DashboardDataSender;
//screen->PrintfLine(DriverStationLCD::kUser_Line3,"det %d", imaqGetLastError());
//screen->PrintfLine(DriverStationLCD::kUser_Line4,"Matches: %d",matches);//Notifies the user
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Matches: %i",matches);//Notifies the user
screen->UpdateLCD();
/*for(int i = 0; i < matches; i++)
{
//screen->PrintfLine((DriverStationLCD::Line)(i+1),"%i,%i,%i",recmatch[i].height,recmatch[i].width,recmatch[i].score);
if(recmatch[i].score > highscore)
{
highscore = recmatch[i].score;
highestindex = i;
}
screen->PrintfLine(DriverStationLCD::kUser_Line1,"score %i, i %i", recmatch[i].score, i);
screen->UpdateLCD();
}*/
average = (recmatch->corner[1].x + recmatch->corner[3].x)/2;
y = (472/(recmatch->height-4));
screen->PrintfLine(DriverStationLCD::kUser_Line5,"%f",average);
screen->PrintfLine(DriverStationLCD::kUser_Line3," %f",distance(recmatch->height));
//screen->PrintfLine(DriverStationLCD::kUser_Line5,"%f,%f",recmatch->height, y);
screen->PrintfLine(DriverStationLCD::kUser_Line6,"%f,%f",ceil(recmatch->corner[1].x),ceil(recmatch->corner[3].x));
screen->UpdateLCD();
//screen->PrintfLine(DriverStationLCD::kUser_Line6,"%d, %d, %d, %d",recmatch->corner[1].x,recmatch->corner[2].x,recmatch->corner[3].x,recmatch->corner[4].x);
//screen->PrintfLine(DriverStationLCD::kUser_Line5,"%d, %d",recmatch->height,recmatch->width);
if(average == 0)
{
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Image Not Found");
}
else if(average < 145)
{
difference = (160 - average);
time = difference*0.0062;
vic.Set(0.1);
Wait(time);
vic.Set(0.0);
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Less!");
screen->UpdateLCD();
}
else if (average > 175)
{
difference = (average - 160);
time = difference*0.0062;
vic.Set(-0.1);
Wait(time);
vic.Set(0.0);
screen->PrintfLine(DriverStationLCD::kUser_Line1,"More!");
screen->UpdateLCD();
}
else
{
vic.Set(0);
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Perfection!");
screen->UpdateLCD();
}
screen->PrintfLine(DriverStationLCD::kUser_Line2,"Time: %f",time);
screen->PrintfLine(DriverStationLCD::kUser_Line4,"difference %f",difference);
// screen->PrintfLine(DriverStationLCD::kUser_Line2,"Score %i, Index %i", highscore, highestindex);
/* if(matches > 0)
{
int counter = 0;
while (counter < 20)
{
average = (recmatch[highestindex].corner[1].x + recmatch[highestindex].corner[2].x + recmatch[highestindex].corner[3].x +recmatch[highestindex].corner[4].x)/4;
average2 = (recmatch[highestindex].corner[1].y + recmatch[highestindex].corner[2].y + recmatch[highestindex].corner[3].y +recmatch[highestindex].corner[4].y)/4;
// screen->PrintfLine(DriverStationLCD::kUser_Line5,"Center = %f, %f",average, average2);
screen->UpdateLCD();
if (average > 170)
{
vic.Set(-0.1);
//screen->PrintfLine(DriverStationLCD::kUser_Line1,"Left %f",vic.Get());
//screen->UpdateLCD();
}
else if (average < 150)
{
vic.Set(0.1);
//screen->PrintfLine(DriverStationLCD::kUser_Line1,"Right %f",vic.Get());
//screen->UpdateLCD();
}
else
{
counter = 20;
vic.Set(0);
// screen->PrintfLine(DriverStationLCD::kUser_Line6,"Not Mallory! %f",vic.Get());
screen->UpdateLCD();
}
counter++a;
}
*/
// imaqWriteFile(cast, "/tmp/linedetect.jpg", NULL);
// }
/* else
{
vic.Set(0);
// screen->PrintfLine(DriverStationLCD::kUser_Line6,"Not Mallory! %f",vic.Get());
screen->UpdateLCD();
}*/
imaqDispose(bitcast);
imaqDispose(recmatch);
//imaqDispose(roi);
vic.Set(0);
}
}