Quote:
Originally Posted by rich2202
|
I was able to get rid of the VisionThread's, and make a single thread. I'm going to decide what to do with the centerX and width values in my lineUp method based on which cam is selected (the cam also decides whether we're going for goals or gears).
Do you think the roboRio can handle this? (It will be much faster imo.)
Code:
new Thread(() -> {
double[] red = { 0, 0, 255 };
double[] green = { 0, 255, 0 };
Scalar color = new Scalar(red);
while (!Thread.interrupted()) {
selectedVid.grabFrame(image);
if (pipeline.filterContoursOutput().size() >= 2) {
pipeline.process(image);
if (!isTargetFound) {
color.set(red);
} else {
color.set(green);
}
Rect r = Imgproc.boundingRect(pipeline.filterContoursOutput().get(0));
Rect r1 = Imgproc.boundingRect(pipeline.filterContoursOutput().get(1));
Imgproc.rectangle(image, new Point(r.x, r.y), new Point(r.x + r.width, r.y + r.height), color, 2);
Imgproc.rectangle(image, new Point(r1.x, r1.y), new Point(r1.x + r1.width, r1.y + r1.height), color,
2);
synchronized (imgLock) {
centerX = r.x + (r.x + r.width) / 2;
width = r.width;
}
outputStream.putFrame(image);
} else {
synchronized (imgLock) {
centerX = 0.0;
width = 0.0;
isTargetFound = false;
}
outputStream.putFrame(image);
}
try {
Thread.sleep(100);
} catch (Exception e) {
e.printStackTrace();
}
}
}).start();
^ opposed to the code I pasted in the initial question.
Here is the basis of my lineUp and isLinedUp methods.
Code:
public void lineUp() {
// Default variables
double centerX = 0.0;
double width = 0.0;
double fwd = 0;
//Update the values.
this.isLinedUp();
// Check to see if the shot is lined up.
if (!isTargetFound) {
// Synchronize variables from thread to thread
synchronized (imgLock) {
centerX = this.centerX;
width = this.width;
}
/*
* If the robot is too far, drive forwards If the robot is too
* close, drive backwards
*/
if (width > tooFar) {
fwd = -width * 0.0025;
} else if (width < tooClose) {
fwd = width * 0.005;
} else {
fwd = 0;
}
double turn = centerX - imageCenter;
drive.mecanumDrive_Cartesian(fwd, 0, turn * 0.005, 0);
} else {
SmartDashboard.putString(shotReady, "true");
}
}
/*
* Decide which method of lining up to use, and line up the robot
* accordingly.
*/
public boolean isLinedUp() {
// Default values
double centerX = 0.0;
double width = 0.0;
// Sync the processed images from grip to the local variables
synchronized (imgLock) {
centerX = this.centerX;
width = this.width;
}
/*
* Check and see if the robot is where it needs to be if true: tell the
* driver to shoot if false: initiate this.lineUp and test again once
* the robot believes it is lined up.
*/
if (whichCam) {
//High goal logic
boolean isXCentered = (centerX + gripTolerance >= imageCenter) && (centerX - gripTolerance <= imageCenter);
boolean isDistance4ft4inch = (width + gripTolerance >= tooClose) && (width - gripTolerance <= tooFar);
if (isXCentered && isDistance4ft4inch) {
SmartDashboard.putString(shotReady, "true");
isTargetFound = true;
return true;
} else {
this.lineUp();
SmartDashboard.putString(gearReady, "false");
isTargetFound = false;
return false;
}
} else {
//TODO:
//Gear logic
}
return false;
}
Sorry to bombard you with code, but do you think I could get away with this?