Threshold error

[GageWErv]

Hello- having some problems with the indicated (by color) code: as it is, everything up until it is done.

I took that filter out and deployed anew- several times, and it doesn't even get that far: strange since there is nothing dependent on it.

#include "WPILib.h"
#include "Cmath"
#include "Vision/RGBImage.h"
#include "Vision/BinaryImage.h"

#define BOTTOMTARGET 107.5//not relevant right now
#define TOPTARGET 31.5//not relevant right now
#define CAMERAHEIGHT 65//not relevant right now
#define ANGLEOFLAUNCH 60//not relevant right now
#define CAMANGLE 54//not relevant right now

#define RL 0//RGB Values for threshold
#define RH 145
#define GL 0
#define GH 255
#define BL 184
#define BH 255

#define LCRIT 0//upper and lower for threshold
#define HCRIT 90

static AxisCamera &camera = AxisCamera::GetInstance("teamnumber");

DriverStationLCD* driverOut = DriverStationLCD::GetInstance();

class VisionSample2012 : public SimpleRobot
{
Joystick stick; // only joystick
long int cycle;
signed char distanceTarget, choiceTarget, highParticle, numTargets, lowParticle;
double firstTarget, secondTarget, thirdTarget, approx, centerX, centerY, area, height, distanceYnorm;
bool limitedDistance;
//AxisCamera *camera;

public:
VisionSample2012(void):
stick(1) // as they are declared above.
{
GetWatchdog().Kill();
setCam();
}

void Autonomous(void)
{
GetWatchdog().Kill();
//Threshold threshold(25, 255, 0, 45, 0, 47);
//ParticleFilterCriteria2 criteria[] = {
// {IMAQ_MT_BOUNDING_RECT_WIDTH, 30, 400, false, false},
// {IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, 400, false, false}};
while (IsAutonomous() && IsEnabled())
{
GetWatchdog().Kill();
//ColorImage *image;
//image = new RGBImage; // get the sample image from the cRIO flash
//camera->GetImage("image.jpg");
UpdateCamData();
}
}

void OperatorControl(void)
{
GetWatchdog().Kill();
while (IsOperatorControl())
{
GetWatchdog().Kill();
}
}

void UpdateCamData()
{
ColorImage *image/*(IMAQ_IMAGE_RGB_U64)*/;
image = new RGBImage;
// if (camera.IsFreshImage())//if latest from cam not recieved
// {
camera.GetImage(image);
//&
driverOut->Clear();
driverOut->Printf(DriverStationLCD::kUser_Line2, 1, "sixth");
driverOut->UpdateLCD();

Threshold threshold(RH,RL,GH,GL,BH,BL);
BinaryImage *thresholdImage = image->ThresholdRGB(threshold);
//.
driverOut->Clear();
driverOut->Printf(DriverStationLCD::kUser_Line3, 1, "seventh");
driverOut->UpdateLCD();

imaqDispose(&image);
///////////////////////////////////////////////////////////////////////////


StructuringElement elems;
elems.matrixCols = 3;
elems.matrixRows = 3;
elems.hexa = FALSE;
///////////////////////////////////////////////////////////////////////////

BinaryImage *processedImage;
imaqMorphology(processedImage->GetImaqImage(), thresholdImage->GetImaqImage(), IMAQ_CLOSE, &elems);
imaqDispose(&thresholdImage);
///////////////////////////////////////////////////////////////////////////

BinaryImage *filteredImage;
ParticleFilterCriteria2 Criteria[5] = {IMAQ_MT_AREA_BY_PARTICLE_AND_HOLES_AREA, LCRIT, HCRIT, false, false};
ParticleFilterOptions Options;
Options.rejectMatches = FALSE;
Options.rejectBorder = 0;
Options.connectivity8 = TRUE;
///////////////////////////////////////////////////////////////////////////

imaqParticleFilter3(filteredImage->GetImaqImage(), processedImage->GetImaqImage(), Criteria, 1, &Options, NULL, NULL);//~
free(Criteria);
/////////////////////////////////////////////////////////////////////////

vector<ParticleAnalysisReport> *particles = filteredImage->GetOrderedParticleAnalysisReports();
imaqDispose(filteredImage->GetImaqImage());
imaqDispose(processedImage->GetImaqImage());
/////////////////////////////////////////////////////////////////////////

ParticleAnalysisReport *high;
ParticleAnalysisReport *particle;
ParticleAnalysisReport *low;
/////////////////////////////////////////////////////////////////////////

for (unsigned int i = 1; 5 > i && i <= particles->size(); i++)
{
particle = &(particles)->at(i);
if(particle->center_mass_y_normalized > high->center_mass_y_normalized)
{
high = &(particles)->at(i);//add criteria for too small particles
highParticle = i;
}
delete particle;
}

for (unsigned int i = 1; 5 > i && i <= particles->size(); i++)
{
particle = &(particles)->at(i);
if(particle->center_mass_y_normalized < low->center_mass_y_normalized)
{
low = &(particles)->at(i);//add criteria for too small particles
lowParticle = i;
}
delete particle;
}
///////////////////////////////////////////////////////////////////////////

numTargets = particles->size();//MAKE SURE ALL ARE VALID SIZE RANGE IN ABOVE FILTERS
ParticleAnalysisReport *par = &(particles)->at(highParticle);
numTargets = particles->size();
centerX = par->center_mass_x_normalized;
centerY = par->center_mass_y_normalized;
par = &(particles)->at(lowParticle);
area = par->particleArea;
height = par->imageHeight;
distanceYnorm = par->center_mass_y_normalized;
///////////////////////////////////////////////////////////////////////////
double app;
if (numTargets > 0)
approx = getDistance(app);
else
approx = -1;

delete high;
delete low;
// }
}


double fOfX(double x)//part of accurate distance finding in getDistance()
{
return (atan((TOPTARGET-CAMERAHEIGHT)/(x)) //angle from top triangle (triangle formed by camera target and line at camera's height)
+atan((CAMERAHEIGHT-BOTTOMTARGET)/(x))); //angle from bottom triangle
}

double getDistance(double &approximation)//find distance
{
if (numTargets < 3)//if must use less accurate tracking
{
approximation = 9//half height of target in inches over target to get adjacent
/tan(//tan of this to get opposite over adjacent
CAMANGLE*
((area/24)//to get height in pixels
/height)//above divided to get ratio of size
/2);//to get half of angle and therefore right triangle
}
else
{
double theta=(centerY-distanceYnorm)/240*CAMANGLE;//the distance is turned into an angle (refer to fofx(x))
//107.5 is top target height 31.5 is bottom target height

double dotbinary=54;
approximation=0;
for(int i=0; i<30; i++)//binary approximation-> guesses using 1/2 distances until //tlar(that looks about right) -- function too complex for now
{
dotbinary/=2; //this is the number which modifies the approximation
if(fOfX(approximation+dotbinary)>theta) //if the value to be added overshoots it does not add
approximation+=dotbinary;
}
}
return (approximation);
}

void setCam(void)
{
camera.WriteBrightness(30);
camera.WriteWhiteBalance(AxisCamera::kWhiteBalance _Hold);
camera.WriteResolution(AxisCamera::kResolution_640 x480);
camera.WriteColorLevel(100);
cameraOfSauron.WriteCompression(0);
//lower easier on CRIO and harder on cam
camera.WriteMaxFPS(5);
}

};

START_ROBOT_CLASS(VisionSample2012);

[frdrake]

Something that immediately jumps out to me is that I'm going to assume RH = red high, RL = red low etc.

That's backwards, for the threshold it should be (red low, red high, green low, green high, blue low, blue high).

If your value of RH is higher than RL then I'm not sure what will happen to it. If that doesn't fix it let me know and I'll see if I can find anything else.