Reading through it, it seems that this chunk is where all the actual information is extracted:
Code:
if(scoreCompare(scores[i], false))
{
System.out.println("particle: " + i + "is a High Goal centerX: " + report.center_mass_x_normalized + "centerY: " + report.center_mass_y_normalized);
System.out.println("Distance: " + computeDistance(thresholdImage, report, i, false));
} else if (scoreCompare(scores[i], true)) {
System.out.println("particle: " + i + "is a Middle Goal centerX: " + report.center_mass_x_normalized + "centerY: " + report.center_mass_y_normalized);
System.out.println("Distance: " + computeDistance(thresholdImage, report, i, true));
} else {
System.out.println("particle: " + i + "is not a goal centerX: " + report.center_mass_x_normalized + "centerY: " + report.center_mass_y_normalized);
}
If you want access to that data elsewhere, you can either return it from the function, or assign it to member variables in the class. An example of how to return it:
Code:
public class ImageResults {
public boolean isHighGoal;
public double centerX,centerY,distance;
public ImageResults(...) { ... } // just pass in data and initialize the members with this
}
public ImageResults processImage() { // a clearer name than auton
// .. snip ..
if(scoreCompare(scores[i], false))
{
return new ImageResults(true,report.center_mass_x_normalized,report.center_mass_y_normalized,computeDistance(thresholdImage,report,i,false));
} else if (scoreCompare(scores[i], true)) {
return new ImageResults(false,report.center_mass_x_normalized,report.center_mass_y_normalized,computeDistance(thresholdImage,report,i,false));
} else {
return null;
}
}
Then elsewhere in code:
Code:
ImageResults results = camera.processImage();
if(results == null) {
return; // no valid target found
}
double horizAngle = results.centerX*cameraHFOV/2; // centerX is normalized, so -1 is left, 1 is right. Multiply by cameraHFOV/2, and you get the horizontal angle of the target relative to the camera
new PIDTurn(gyro.getAngle()+horizAngle).start();
PIDTurn is assumed to be a class extending PIDCommand, taking an angle setpoint in its constructor. An (incomplete) example implementation would be:
Code:
class PIDTurn extends PIDCommand {
// ...
public PIDTurn(double setpoint) {
super(kP,kI,kD);
setSetpoint(setpoint);
requires(driveTrain);
// ...
}
protected double returnPIDInput() {
return driveTrain.gyro.getAngle();
}
protected void usePIDOutput(double output) {
driveTrain.tankDrive(output,-output);
}
// ...
}
Just as a caveat, this code will not work on its own, and may not work at all. I'm trying to get the theory across so that you can write (and understand) your own code.
In fact, reading over my code again, I'm pretty sure it will create a memory leak if inserted directly into your code. The free() calls in auton() are important.