-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #21 from GreenBlitz/vision
Vision
- Loading branch information
Showing
16 changed files
with
311 additions
and
13 deletions.
There are no files selected for viewing
8 changes: 8 additions & 0 deletions
8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,8 @@ | ||
| package org.firstinspires.ftc.teamcode; | ||
|
|
||
| public enum Alliance { | ||
|
|
||
| BLUE, | ||
| RED | ||
|
|
||
| } |
9 changes: 9 additions & 0 deletions
9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,9 @@ | ||
| package org.firstinspires.ftc.teamcode; | ||
|
|
||
| public enum Location { | ||
|
|
||
| LEFT, | ||
| CENTER, | ||
| RIGHT | ||
|
|
||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
19 changes: 19 additions & 0 deletions
19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autos/DefaultGil.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,19 @@ | ||
| package org.firstinspires.ftc.teamcode.autos; | ||
|
|
||
| import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; | ||
|
|
||
| import org.firstinspires.ftc.teamcode.Alliance; | ||
| import org.firstinspires.ftc.teamcode.Robot; | ||
|
|
||
| public abstract class DefaultGil extends LinearOpMode { | ||
|
|
||
| @Override | ||
| public void runOpMode() throws InterruptedException { | ||
| Robot.getInstance().setAlliance(setAlliance()); | ||
| run(); | ||
| } | ||
|
|
||
| public abstract Alliance setAlliance(); | ||
| public abstract void run() throws InterruptedException; | ||
|
|
||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
1 change: 1 addition & 0 deletions
1
...c/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazLauncherTest.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
27 changes: 27 additions & 0 deletions
27
...src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazVisionTest.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,27 @@ | ||
| package org.firstinspires.ftc.teamcode.opmodes.subsystemstesters; | ||
|
|
||
| import com.qualcomm.robotcore.eventloop.opmode.TeleOp; | ||
|
|
||
| import org.firstinspires.ftc.teamcode.Alliance; | ||
| import org.firstinspires.ftc.teamcode.Robot; | ||
| import org.firstinspires.ftc.teamcode.opmodes.DefaultRaz; | ||
|
|
||
| @TeleOp(name = "Raz Vision Test") | ||
| public class RazVisionTest extends DefaultRaz { | ||
|
|
||
| @Override | ||
| public void initialize() { | ||
|
|
||
| } | ||
|
|
||
| @Override | ||
| public void execute() { | ||
| Robot.getInstance().getVision().telemetry(telemetry); | ||
| } | ||
|
|
||
| @Override | ||
| public void configureBindings() { | ||
|
|
||
| } | ||
|
|
||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
92 changes: 92 additions & 0 deletions
92
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,92 @@ | ||
| package org.firstinspires.ftc.teamcode.subsystems.vision; | ||
|
|
||
| import android.graphics.Canvas; | ||
| import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; | ||
| import org.firstinspires.ftc.teamcode.Alliance; | ||
| import org.firstinspires.ftc.teamcode.Location; | ||
| import org.firstinspires.ftc.teamcode.Robot; | ||
| import org.firstinspires.ftc.vision.VisionProcessor; | ||
| import org.opencv.core.Core; | ||
| import org.opencv.core.Mat; | ||
| import org.opencv.core.Rect; | ||
| import org.opencv.core.Scalar; | ||
| import org.opencv.imgproc.Imgproc; | ||
|
|
||
| public class PropProcessor implements VisionProcessor { | ||
|
|
||
| private Location location; | ||
|
|
||
| @Override | ||
| public void init(int width, int height, CameraCalibration calibration) { | ||
|
|
||
| } | ||
|
|
||
| @Override | ||
| public Object processFrame(Mat frame, long captureTimeNanos) { | ||
| Mat leftZone = getLeftZoneMatrix(frame); | ||
| Mat centerZone = getCenterZoneMatrix(frame); | ||
|
|
||
| Scalar left = getAvgColor(leftZone); | ||
| Scalar center = getAvgColor(centerZone); | ||
|
|
||
| if (isPropPartOfAvgColor(left)) { | ||
| this.location = Location.LEFT; | ||
| } | ||
| else if (isPropPartOfAvgColor(center)) { | ||
| this.location = Location.CENTER; | ||
| } | ||
| else { | ||
| this.location = Location.RIGHT; | ||
| } | ||
|
|
||
| leftZone.release(); | ||
| centerZone.release(); | ||
|
|
||
| return null; | ||
| } | ||
|
|
||
| public Mat getLeftZoneMatrix(Mat frame) { | ||
| if (Robot.getInstance().getAlliance() == Alliance.RED) { | ||
| return frame.submat(VisionConstant.RED_LEFT_ZONE_AREA); | ||
| } | ||
| else { | ||
| return frame.submat(VisionConstant.BLUE_LEFT_ZONE_AREA); | ||
| } | ||
| } | ||
|
|
||
| public Mat getCenterZoneMatrix(Mat frame) { | ||
| if (Robot.getInstance().getAlliance() == Alliance.RED) { | ||
| return frame.submat(VisionConstant.RED_CENTER_ZONE_AREA); | ||
| } | ||
| else { | ||
| return frame.submat(VisionConstant.BLUE_CENTER_ZONE_AREA); | ||
| } | ||
| } | ||
|
|
||
| public Scalar getAvgColor(Mat matrix) { | ||
| Imgproc.blur(matrix, matrix, VisionConstant.BLUR_SIZE); | ||
| return Core.mean(matrix); | ||
| } | ||
|
|
||
| public double getAllianceColorThreshold() { | ||
| return Robot.getInstance().getAlliance() == Alliance.RED ? VisionConstant.RED_THRESHOLD : VisionConstant.BLUE_THRESHOLD; | ||
| } | ||
|
|
||
| public boolean isPropPartOfAvgColor(Scalar color) { | ||
| int allianceColorIndex = Robot.getInstance().getAlliance() == Alliance.RED ? VisionConstant.RED_INDEX : VisionConstant.BLUE_INDEX; | ||
| double allianceColor = color.val[allianceColorIndex]; | ||
| boolean allianceColorPassesThreshold = allianceColor > getAllianceColorThreshold(); | ||
| double sumOfColor = color.val[VisionConstant.RED_INDEX] + color.val[VisionConstant.GREEN_INDEX] + color.val[VisionConstant.BLUE_INDEX]; | ||
| return allianceColorPassesThreshold && sumOfColor < VisionConstant.AMOUNT_OF_COLOR * allianceColor; | ||
| } | ||
|
|
||
| @Override | ||
| public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { | ||
|
|
||
| } | ||
|
|
||
| public Location getLocation() { | ||
| return location; | ||
| } | ||
|
|
||
| } |
83 changes: 83 additions & 0 deletions
83
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,83 @@ | ||
| package org.firstinspires.ftc.teamcode.subsystems.vision; | ||
|
|
||
| import android.util.Pair; | ||
| import android.util.Size; | ||
| import com.arcrobotics.ftclib.command.SubsystemBase; | ||
| import com.qualcomm.robotcore.hardware.HardwareMap; | ||
| import org.firstinspires.ftc.robotcore.external.Telemetry; | ||
| import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; | ||
| import org.firstinspires.ftc.teamcode.Location; | ||
| import org.firstinspires.ftc.vision.VisionPortal; | ||
| import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; | ||
| import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc; | ||
| import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; | ||
| import org.firstinspires.ftc.vision.tfod.TfodProcessor; | ||
| import java.util.ArrayList; | ||
| import java.util.List; | ||
|
|
||
| public class Vision extends SubsystemBase { | ||
|
|
||
| private final AprilTagProcessor aprilTagProcessor; | ||
| private final TfodProcessor tfodProcessor; | ||
| private final PropProcessor propProcessor; | ||
| private final VisionPortal visionPortal; | ||
|
|
||
| public Vision(HardwareMap hardwareMap) { | ||
| this.aprilTagProcessor = new AprilTagProcessor.Builder() | ||
| .setDrawTagID(true) | ||
| .setDrawTagOutline(true) | ||
| .setDrawAxes(true) | ||
| .setDrawCubeProjection(true) | ||
| .build(); | ||
|
|
||
| this.tfodProcessor = new TfodProcessor.Builder() | ||
| .setMaxNumRecognitions(VisionConstant.MAX_NUM_RECOGNITIONS) | ||
| .setUseObjectTracker(true) | ||
| .setTrackerMaxOverlap(VisionConstant.TRACKER_MAX_OVERLAP) | ||
| .setTrackerMinSize(VisionConstant.TRACKER_MIN_SIZE) | ||
| .build(); | ||
|
|
||
| propProcessor = new PropProcessor(); | ||
|
|
||
| this.visionPortal = new VisionPortal.Builder() | ||
| .setCamera(hardwareMap.get(WebcamName.class, VisionConstant.CAMERA_ID)) | ||
| .addProcessors(aprilTagProcessor, tfodProcessor, propProcessor) | ||
| .setCameraResolution(new Size(VisionConstant.CAMERA_RESOLUTION_WIDTH, VisionConstant.CAMERA_RESOLUTION_HEIGHT)) | ||
| .setStreamFormat(VisionConstant.STREAM_FORMAT) | ||
| .enableLiveView(true) | ||
| .setAutoStopLiveView(true) | ||
| .build(); | ||
| } | ||
|
|
||
| public List<AprilTagDetection> getTagsDetections() { | ||
| return aprilTagProcessor.getDetections(); | ||
| } | ||
|
|
||
| public List<Pair<AprilTagPoseFtc,Integer>> getRelativeTagsPoses() { | ||
| List<AprilTagDetection> temp = getTagsDetections(); | ||
| List<Pair<AprilTagPoseFtc,Integer>> poses = new ArrayList<>(); | ||
| for(AprilTagDetection detection : temp) { | ||
| if (detection.metadata != null) { | ||
| poses.add(new Pair<>(detection.ftcPose, detection.id)); | ||
| } | ||
| } | ||
| return poses; | ||
| } | ||
|
|
||
| public void telemetry(Telemetry telemetry) { | ||
| List<Pair<AprilTagPoseFtc,Integer>> poses = getRelativeTagsPoses(); | ||
| if(!poses.isEmpty()) { | ||
| telemetry.addData("detected: ", poses.size() + " AprilTags"); | ||
| telemetry.addData("the first tag is: ", poses.get(0).second); | ||
| telemetry.addData("first tag x: ", poses.get(0).first.x); | ||
| telemetry.addData("first tag y: ", poses.get(0).first.y); | ||
| telemetry.addData("first tag z: ", poses.get(0).first.z); | ||
| } | ||
| Location propLocation = propProcessor.getLocation(); | ||
|
|
||
| if(propLocation != null) { | ||
| telemetry.addData("prop's location is: ", propLocation.toString()); | ||
| } | ||
| } | ||
|
|
||
| } |
Oops, something went wrong.