eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Try to fix camera.

This commit is contained in:
Scott Barnes 2024-03-20 17:37:42 -05:00
parent da3701612b
commit 87399f0670
4 changed files with 18 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java
View File

@ -75,8 +75,8 @@ public class AutoBlue extends LinearOpMode {
// Do super fancy chinese shit // Do super fancy chinese shit
while (!this.isStarted()) { while (!this.isStarted()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue()); this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
this.telemetry.update(); this.telemetry.update();
} }
boardScore(); boardScore();

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java
View File

@ -132,8 +132,8 @@ public class AutoBlueFarStem extends LinearOpMode {
// Do super fancy chinese shit // Do super fancy chinese shit
while (!this.isStarted()) { while (!this.isStarted()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue()); this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
if (gamepad2.dpad_left) { if (gamepad2.dpad_left) {
parkLocation="LEFT"; parkLocation="LEFT";
} else if (gamepad2.dpad_right) { } else if (gamepad2.dpad_right) {

72
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java
View File

@ -67,37 +67,27 @@ public class Camera {
this.initialized = true; this.initialized = true;
} }
// Close the Targeting Camera
public void stopTargetingCamera() {
if (targetingCameraInitialized) {
targetingCamera.closeCameraDeviceAsync(() -> targetingCameraInitialized = false);
}
}
// Get the Red Goal Detection
public Detection getRed() {
return targetingCameraInitialized ? prop.getRed() : INVALID_DETECTION;
}
public void setAlliance(Alliance alliance) {
this.prop.setAlliance(alliance);
}
public StarterPosition getStartingPosition() { public StarterPosition getStartingPosition() {
if (!targetingCameraInitialized) { Detection red = this.prop.getRed();
Detection blue = this.prop.getBlue();
double redArea = red.getArea();
double blueArea = blue.getArea();
return getStartingPosition(redArea > blueArea ? red : blue);
}
private static StarterPosition getStartingPosition(Detection detection) {
if (detection == null) {
return StarterPosition.UNKNOWN; return StarterPosition.UNKNOWN;
} }
Detection detection = this.getRed();
if (detection == null || !detection.isValid()) {
return StarterPosition.UNKNOWN;
}
Point center = detection.getCenter(); Point center = detection.getCenter();
if (center == null) { if (center == null) {
return StarterPosition.UNKNOWN; return StarterPosition.UNKNOWN;
} }
double centerX = this.getRed().getCenter().x + 50; double centerX = detection.getCenter().x + 50;
if (centerX < 33) { if (centerX < 33) {
return StarterPosition.LEFT; return StarterPosition.LEFT;
} else if (centerX < 66) { } else if (centerX < 66) {
@ -106,51 +96,14 @@ public class Camera {
return StarterPosition.RIGHT; return StarterPosition.RIGHT;
} }
return StarterPosition.UNKNOWN; return StarterPosition.UNKNOWN;
} }
public Detection getBlue() {
return targetingCameraInitialized ? prop.getBlue() : INVALID_DETECTION;
}
public StarterPositionBlue getStartingPositionBlue() {
if (!targetingCameraInitialized) {
return StarterPositionBlue.UNKNOWN;
}
Detection detection = this.getBlue();
if (detection == null || !detection.isValid()) {
return StarterPositionBlue.UNKNOWN;
}
Point center = detection.getCenter();
if (center == null) {
return StarterPositionBlue.UNKNOWN;
}
double centerX = this.getBlue().getCenter().x + 50;
if (centerX < 33) {
return StarterPositionBlue.LEFT;
} else if (centerX < 66) {
return StarterPositionBlue.CENTER;
} else if (centerX < 100) {
return StarterPositionBlue.RIGHT;
}
return StarterPositionBlue.UNKNOWN;
}
public enum StarterPosition { public enum StarterPosition {
UNKNOWN, LEFT, CENTER, RIGHT UNKNOWN, LEFT, CENTER, RIGHT
} }
public enum StarterPositionBlue {
UNKNOWN, LEFT, CENTER, RIGHT
}
public AprilTagDetection getBestAprilTagDetection() { public AprilTagDetection getBestAprilTagDetection() {
return this.aprilTag.getDetections() return this.aprilTag.getDetections()
.stream().max((d1, d2) -> Float.compare(d2.decisionMargin, d1.decisionMargin)) .stream().max((d1, d2) -> Float.compare(d2.decisionMargin, d1.decisionMargin))
@ -164,5 +117,4 @@ public class Camera {
} }
return this.aprilTagPoseEstimator.estimatePose(detection); return this.aprilTagPoseEstimator.estimatePose(detection);
} }
} }

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java
View File

@ -55,13 +55,8 @@ public class PropDetectionPipeline implements VisionProcessor {
Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0); Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0);
Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV); Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV);
if (alliance == Alliance.Red) {
red.setContour(detect(FTC_RED_RANGE_1, FTC_RED_RANGE_2)); red.setContour(detect(FTC_RED_RANGE_1, FTC_RED_RANGE_2));
}
if (alliance == Alliance.Blue) {
blue.setContour(detect(FTC_BLUE_RANGE)); blue.setContour(detect(FTC_BLUE_RANGE));
}
return input; return input;
} }