[SoyStuff]

Our vision system seems to be terribly unreliable. It works on and off, but never completely. The objective is to have it aim towards the target and then shoot. When it can fine the target, this works quite well. Unfortunately, it cannot seem to find the target most of the time. It is extremely picky. We use a green ring of LEDs from superbrightleds.com to illuminate the targets. We try to find the highest target that is also roughly rectangular (to check that it is rectangular (or at least a parallelogram), we divide the particle area by the lengthxwidth and make sure that it is close to 1.) Here is our function (called aimNFire). Please note that we drive back until the target is around 33 pixels wide because that works out to around 12 feet, which is the only point where we have data on how the shooter needs to be oriented.

team1257LCD->Printf(DriverStationLCD::kUser_Line2, 1, "Aim n' Fire!");
	team1257LCD->UpdateLCD();
	
	//Threshold threshold(0, 120,  100, 255, 125, 255);
	//Threshold threshold(19, 87, 42, 84, 03, 255);
	//Threshold threshold(0, 75,  100, 255, 0, 60);
	//Threshold threshold(0, 80,  0, 255, 55, 220); // tolerant threshold
	Threshold threshold(0, 80,  0, 220, 55, 220); // competition threshold
	//Threshold threshold(0, 255, 0, 255, 0, 255);
	ParticleFilterCriteria2 criteria[] = {
										{IMAQ_MT_BOUNDING_RECT_WIDTH, 30, 400, false, false},
										{IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, 400, false, false}
	};
	
	AxisCamera& camera = AxisCamera::GetInstance("10.12.57.11");
	camera.WriteResolution(AxisCamera::kResolution_320x240);
	camera.WriteCompression(20);
	camera.WriteBrightness(0);
	//camera.WriteExposureControl(AxisCamera::kExposure_FlickerFree50Hz);
	//camera.WriteExposurePriority(0);
	camera.WriteWhiteBalance(AxisCamera::kWhiteBalance_FixedIndoor);
	
	bool driveBack = false;
	Timer robotTimer2;
	robotTimer2.Start();
	while(true/*IsAutonomous() || IsOperatorControl()*/)
	{
		if(robotTimer2.Get()>= 10)
			break;
		
		ColorImage* image = new RGBImage;
		camera.GetImage(image);            // get the sample image from the cRIO flash
		BinaryImage *thresholdImage = image->ThresholdRGB(threshold);   // get just the red target pixels
		BinaryImage *bigObjectsImage = thresholdImage->RemoveSmallObjects(false, 2);  // remove small objects (noise)
		BinaryImage *convexHullImage = bigObjectsImage->ConvexHull(false);  // fill in partial and full rectangles
		BinaryImage *filteredImage = convexHullImage->ParticleFilter(criteria, 2);  // find the rectangles
		vector<ParticleAnalysisReport>* reports = filteredImage->GetOrderedParticleAnalysisReports();  // get the results
	   
		// Identify the highest rectangle
		ParticleAnalysisReport basket;
		basket.boundingRect.top = 10000;
		basket.center_mass_x = 0;
		for(unsigned int i = 0; i < reports->size(); i++)
		{
			if(reports->at(i).boundingRect.top < basket.boundingRect.top && reports->at(i).particleArea / ((float)reports->at(i).boundingRect.width * (float)reports->at(i).boundingRect.height) > 0.7917011257314159)
			{
				basket = reports->at(i);
			}	
		}
		
		// Now, angle robot towards the target
		if(!driveBack)
		{
			if(basket.center_mass_x != 0 && basket.center_mass_x <= 320) // We have valid data!
			{
				double angle = (abs((double)basket.center_mass_x - ((double)image->GetWidth() / 2)) / ((double)image->GetWidth() / 2)) * 22.5;
	
				if(basket.center_mass_x < image->GetWidth() / 2) // To the right of us
					angle *= -1; // Negate angle
				if(angle < 0)
				{
					team1257Robot.SetLeftRightMotorOutputs(0.35, -0.35);
					Wait(0.001);
				}
				if(angle >= 0)
				{
					team1257Robot.SetLeftRightMotorOutputs(-0.35, 0.35);
					Wait(0.001);
				}
				if(abs(angle) < 1.75)
				{
					//break;
					driveBack = true;
				}
					
				team1257LCD->Printf(DriverStationLCD::kUser_Line4, 1, "Angle: %f", angle);
				team1257LCD->UpdateLCD();
			}
			
			/*else
			{
				team1257Robot.SetLeftRightMotorOutputs(0, 0);
			}*/
		}
		if(driveBack)
		{
			if(basket.center_mass_x != 0 && basket.center_mass_x <= 320) // We have valid data!
			{
				team1257LCD->Printf(DriverStationLCD::kUser_Line6, 1, "Driving back!");
				team1257LCD->UpdateLCD();
				if(basket.boundingRect.height < 32) // We are too far
				{
					team1257Robot.SetLeftRightMotorOutputs(-0.3, -0.3);
				}
				else if(basket.boundingRect.height > 33) // We are too close
				{
					team1257Robot.SetLeftRightMotorOutputs(0.3, 0.3);
				}
				else
				{
					break;
				}
			}
		}
		
		team1257LCD->Printf(DriverStationLCD::kUser_Line5, 1, "Width: %i", basket.boundingRect.height);
		team1257LCD->UpdateLCD();
	   
		// Clean up memory
		delete reports;
		delete filteredImage;
		delete convexHullImage;
		delete bigObjectsImage;
		delete thresholdImage;
		delete image;
		
		if(drivepad.GetRawButton(3) || controlpad.GetRawButton(3))
		{
			team1257LCD->Printf(DriverStationLCD::kUser_Line2, 1, "Aim n' Fire terminated");
			team1257LCD->UpdateLCD();
			break;
		}
	}
	
	team1257LCD->Printf(DriverStationLCD::kUser_Line2, 1, "Aim n' Fire terminated");
	team1257LCD->UpdateLCD();