My team would like to have our robot shoot into the hot goal. So I’ve been trying to use the 2014 Camera code provided by FIRST to get the image from the camera, then detect if the image is hot and then trigger a motor we have hooked up. The problem is we haven’t been able to get the camera to take a picture and then send it to the Crio. All it’s doing is using the image that we put in the Crio from FIRST to detect if the goal is Hot, which it does. The problem is we can’t figure out how to get the camera to put an image in the Crio using the given code. We’ve tried commenting back in the code that is for getting the image such as ColorImage image = camera.getImage(); but nothing seems to be working.
Here’s the default camera code were given, we’ve commented back in everything in all sorts of combinations. Is there anything we need to add? Or is there a certain combination of code that needs to be put back in.
Any help is appreciated.
package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.*;
import edu.wpi.first.wpilibj.image.NIVision.MeasurementType;
import edu.wpi.first.wpilibj.Relay;
/**
- Sample program to use NIVision to find rectangles in the scene that are illuminated
- by a LED ring light (similar to the model from FIRSTChoice). The camera sensitivity
- is set very low so as to only show light sources and remove any distracting parts
- of the image.
- The CriteriaCollection is the set of criteria that is used to filter the set of
- rectangles that are detected. In this example we’re looking for rectangles with
- a minimum width of 30 pixels and maximum of 400 pixels.
- The algorithm first does a color threshold operation that only takes objects in the
- scene that have a bright green color component. Then a small object filter
- removes small particles that might be caused by green reflection scattered from other
- parts of the scene. Finally all particles are scored on rectangularity, and aspect ratio,
- to determine if they are a target.
- Look in the VisionImages directory inside the project that is created for the sample
- images.
*/
public class VisionSampleProject2014 extends SimpleRobot {
Relay spike = new Relay(1);
boolean hot;
//Camera constants used for distance calculation
final int Y_IMAGE_RES = 480; //X Image resolution in pixels, should be 120, 240 or 480
final double VIEW_ANGLE = 49; //Axis M1013
//final double VIEW_ANGLE = 41.7; //Axis 206 camera
//final double VIEW_ANGLE = 37.4; //Axis M1011 camera
final double PI = 3.141592653;
//Score limits used for target identification
final int RECTANGULARITY_LIMIT = 40;
final int ASPECT_RATIO_LIMIT = 55;
//Score limits used for hot target determination
final int TAPE_WIDTH_LIMIT = 50;
final int VERTICAL_SCORE_LIMIT = 50;
final int LR_SCORE_LIMIT = 50;
//Minimum area of particles to be considered
final int AREA_MINIMUM = 70;
//Maximum number of particles to process
final int MAX_PARTICLES = 8;
AxisCamera camera; // the axis camera object (connected to the switch)
CriteriaCollection cc; // the criteria for doing the particle filter operation
public class Scores {
double rectangularity;
double aspectRatioVertical;
double aspectRatioHorizontal;
}
public class TargetReport {
int verticalIndex;
int horizontalIndex;
boolean Hot;
double totalScore;
double leftScore;
double rightScore;
double tapeWidthScore;
double verticalScore;
};
public void robotInit() {
camera = AxisCamera.getInstance(); // get an instance of the camera
cc = new CriteriaCollection(); // create the criteria for the particle filter
cc.addCriteria(MeasurementType.IMAQ_MT_AREA, AREA_MINIMUM, 65535, false);
}
public void autonomous() {
TargetReport target = new TargetReport();
int verticalTargets] = new int[MAX_PARTICLES];
int horizontalTargets] = new int[MAX_PARTICLES];
int verticalTargetCount, horizontalTargetCount;
//while (isAutonomous() && isEnabled()) {
try {
/**
* Do the image capture with the camera and apply the algorithm described above. This
* sample will either get images from the camera or from an image file stored in the top
* level directory in the flash memory on the cRIO. The file name in this case is "testImage.jpg"
*
*/
ColorImage image = camera.getImage(); // comment if using stored images
//ColorImage image; // next 2 lines read image from flash on cRIO
//image = new RGBImage("/testImage.jpg"); // get the sample image from the cRIO flash
BinaryImage thresholdImage = image.thresholdHSV(107, 141, 140, 255, 75, 180); // keep only green objects
thresholdImage.write("/threshold.bmp");
BinaryImage filteredImage = thresholdImage.particleFilter(cc); // filter out small particles
filteredImage.write("/filteredImage.bmp");
//iterate through each particle and score to see if it is a target
Scores scores] = new Scores[filteredImage.getNumberParticles()];
horizontalTargetCount = verticalTargetCount = 0;
if(filteredImage.getNumberParticles() > 0)
{
for (int i = 0; i < MAX_PARTICLES && i < filteredImage.getNumberParticles(); i++) {
ParticleAnalysisReport report = filteredImage.getParticleAnalysisReport(i);
scores* = new Scores();
//Score each particle on rectangularity and aspect ratio
scores*.rectangularity = scoreRectangularity(report);
scores*.aspectRatioVertical = scoreAspectRatio(filteredImage, report, i, true);
scores*.aspectRatioHorizontal = scoreAspectRatio(filteredImage, report, i, false);
//Check if the particle is a horizontal target, if not, check if it's a vertical target
if(scoreCompare(scores*, false))
{
System.out.println("particle: " + i + "is a Horizontal Target centerX: " + report.center_mass_x + "centerY: " + report.center_mass_y);
horizontalTargets[horizontalTargetCount++] = i; //Add particle to target array and increment count
} else if (scoreCompare(scores*, true)) {
System.out.println("particle: " + i + "is a Vertical Target centerX: " + report.center_mass_x + "centerY: " + report.center_mass_y);
verticalTargets[verticalTargetCount++] = i; //Add particle to target array and increment count
} else {
System.out.println("particle: " + i + "is not a Target centerX: " + report.center_mass_x + "centerY: " + report.center_mass_y);
}
System.out.println("rect: " + scores*.rectangularity + "ARHoriz: " + scores*.aspectRatioHorizontal);
System.out.println("ARVert: " + scores*.aspectRatioVertical);
}
//Zero out scores and set verticalIndex to first target in case there are no horizontal targets
target.totalScore = target.leftScore = target.rightScore = target.tapeWidthScore = target.verticalScore = 0;
target.verticalIndex = verticalTargets[0];
for (int i = 0; i < verticalTargetCount; i++)
{
ParticleAnalysisReport verticalReport = filteredImage.getParticleAnalysisReport(verticalTargets*);
for (int j = 0; j < horizontalTargetCount; j++)
{
ParticleAnalysisReport horizontalReport = filteredImage.getParticleAnalysisReport(horizontalTargets[j]);
double horizWidth, horizHeight, vertWidth, leftScore, rightScore, tapeWidthScore, verticalScore, total;
//Measure equivalent rectangle sides for use in score calculation
horizWidth = NIVision.MeasureParticle(filteredImage.image, horizontalTargets[j], false, MeasurementType.IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE);
vertWidth = NIVision.MeasureParticle(filteredImage.image, verticalTargets*, false, MeasurementType.IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE);
horizHeight = NIVision.MeasureParticle(filteredImage.image, horizontalTargets[j], false, MeasurementType.IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE);
//Determine if the horizontal target is in the expected location to the left of the vertical target
leftScore = ratioToScore(1.2*(verticalReport.boundingRectLeft - horizontalReport.center_mass_x)/horizWidth);
//Determine if the horizontal target is in the expected location to the right of the vertical target
rightScore = ratioToScore(1.2*(horizontalReport.center_mass_x - verticalReport.boundingRectLeft - verticalReport.boundingRectWidth)/horizWidth);
//Determine if the width of the tape on the two targets appears to be the same
tapeWidthScore = ratioToScore(vertWidth/horizHeight);
//Determine if the vertical location of the horizontal target appears to be correct
verticalScore = ratioToScore(1-(verticalReport.boundingRectTop - horizontalReport.center_mass_y)/(4*horizHeight));
total = leftScore > rightScore ? leftScore:rightScore;
total += tapeWidthScore + verticalScore;
//If the target is the best detected so far store the information about it
if(total > target.totalScore)
{
target.horizontalIndex = horizontalTargets[j];
target.verticalIndex = verticalTargets*;
target.totalScore = total;
target.leftScore = leftScore;
target.rightScore = rightScore;
target.tapeWidthScore = tapeWidthScore;
target.verticalScore = verticalScore;
}
}
//Determine if the best target is a Hot target
target.Hot = hotOrNot(target);
}
if(verticalTargetCount > 0)
{
//Information about the target is contained in the "target" structure
//To get measurement information such as sizes or locations use the
//horizontal or vertical index to get the particle report as shown below
ParticleAnalysisReport distanceReport = filteredImage.getParticleAnalysisReport(target.verticalIndex);
double distance = computeDistance(filteredImage, distanceReport, target.verticalIndex);
if(target.Hot)
{
System.out.println("Hot target located");
System.out.println("Distance: " + distance);
} else {
System.out.println("No hot target present");
System.out.println("Distance: " + distance);
}
}
}
/**
* all images in Java must be freed after they are used since they are allocated out
* of C data structures. Not calling free() will cause the memory to accumulate over
* each pass of this loop.
*/
filteredImage.free();
thresholdImage.free();
image.free();
} catch (NIVisionException ex) {
ex.printStackTrace();
} catch (AxisCameraException ex) {
ex.printStackTrace();
}
if(target.Hot){
spike.set(Relay.Value.kForward);
}
// }
}
/**
* This function is called once each time the robot enters operator control.
*/
public void operatorControl() {
while (isOperatorControl() && isEnabled()) {
Timer.delay(1);
}
}
/**
* Computes the estimated distance to a target using the height of the particle in the image. For more information and graphics
* showing the math behind this approach see the Vision Processing section of the ScreenStepsLive documentation.
*
* @param image The image to use for measuring the particle estimated rectangle
* @param report The Particle Analysis Report for the particle
* @param outer True if the particle should be treated as an outer target, false to treat it as a center target
* @return The estimated distance to the target in Inches.
*/
double computeDistance (BinaryImage image, ParticleAnalysisReport report, int particleNumber) throws NIVisionException {
double rectLong, height;
int targetHeight;
rectLong = NIVision.MeasureParticle(image.image, particleNumber, false, MeasurementType.IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE);
//using the smaller of the estimated rectangle long side and the bounding rectangle height results in better performance
//on skewed rectangles
height = Math.min(report.boundingRectHeight, rectLong);
targetHeight = 32;
return Y_IMAGE_RES * targetHeight / (height * 12 * 2 * Math.tan(VIEW_ANGLE*Math.PI/(180*2)));
}
/**
* Computes a score (0-100) comparing the aspect ratio to the ideal aspect ratio for the target. This method uses
* the equivalent rectangle sides to determine aspect ratio as it performs better as the target gets skewed by moving
* to the left or right. The equivalent rectangle is the rectangle with sides x and y where particle area= x*y
* and particle perimeter= 2x+2y
*
* @param image The image containing the particle to score, needed to perform additional measurements
* @param report The Particle Analysis Report for the particle, used for the width, height, and particle number
* @param outer Indicates whether the particle aspect ratio should be compared to the ratio for the inner target or the outer
* @return The aspect ratio score (0-100)
*/
public double scoreAspectRatio(BinaryImage image, ParticleAnalysisReport report, int particleNumber, boolean vertical) throws NIVisionException
{
double rectLong, rectShort, aspectRatio, idealAspectRatio;
rectLong = NIVision.MeasureParticle(image.image, particleNumber, false, MeasurementType.IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE);
rectShort = NIVision.MeasureParticle(image.image, particleNumber, false, MeasurementType.IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE);
idealAspectRatio = vertical ? (4.0/32) : (23.5/4); //Vertical reflector 4" wide x 32" tall, horizontal 23.5" wide x 4" tall
//Divide width by height to measure aspect ratio
if(report.boundingRectWidth > report.boundingRectHeight){
//particle is wider than it is tall, divide long by short
aspectRatio = ratioToScore((rectLong/rectShort)/idealAspectRatio);
} else {
//particle is taller than it is wide, divide short by long
aspectRatio = ratioToScore((rectShort/rectLong)/idealAspectRatio);
}
return aspectRatio;
}
/**
* Compares scores to defined limits and returns true if the particle appears to be a target
*
* @param scores The structure containing the scores to compare
* @param outer True if the particle should be treated as an outer target, false to treat it as a center target
*
* @return True if the particle meets all limits, false otherwise
*/
boolean scoreCompare(Scores scores, boolean vertical){
boolean isTarget = true;
isTarget &= scores.rectangularity > RECTANGULARITY_LIMIT;
if(vertical){
isTarget &= scores.aspectRatioVertical > ASPECT_RATIO_LIMIT;
} else {
isTarget &= scores.aspectRatioHorizontal > ASPECT_RATIO_LIMIT;
}
return isTarget;
}
/**
* Computes a score (0-100) estimating how rectangular the particle is by comparing the area of the particle
* to the area of the bounding box surrounding it. A perfect rectangle would cover the entire bounding box.
*
* @param report The Particle Analysis Report for the particle to score
* @return The rectangularity score (0-100)
*/
double scoreRectangularity(ParticleAnalysisReport report){
if(report.boundingRectWidth*report.boundingRectHeight !=0){
return 100*report.particleArea/(report.boundingRectWidth*report.boundingRectHeight);
} else {
return 0;
}
}
/**
* Converts a ratio with ideal value of 1 to a score. The resulting function is piecewise
* linear going from (0,0) to (1,100) to (2,0) and is 0 for all inputs outside the range 0-2
*/
double ratioToScore(double ratio)
{
return (Math.max(0, Math.min(100*(1-Math.abs(1-ratio)), 100)));
}
/**
* Takes in a report on a target and compares the scores to the defined score limits to evaluate
* if the target is a hot target or not.
*
* Returns True if the target is hot. False if it is not.
*/
boolean hotOrNot(TargetReport target)
{
boolean isHot = true;
isHot &= target.tapeWidthScore >= TAPE_WIDTH_LIMIT;
isHot &= target.verticalScore >= VERTICAL_SCORE_LIMIT;
isHot &= (target.leftScore > LR_SCORE_LIMIT) | (target.rightScore > LR_SCORE_LIMIT);
return isHot;
}
}************