[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);