vision portal testing

ftc16072 · Jan 21, 2025 · f9f0271 · f9f0271
1 parent 589ba21
commit f9f0271
Show file tree

Hide file tree

Showing 2 changed files with 88 additions and 1 deletion.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeClaw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeClaw.java
@@ -1,14 +1,23 @@
 package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;
 
+import android.util.Size;
+
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest;
 import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestServo;
 import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestTwoServos;
+import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestWebcam;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor;
+import org.firstinspires.ftc.vision.opencv.ColorRange;
+import org.firstinspires.ftc.vision.opencv.ImageRegion;
+import org.opencv.core.RotatedRect;
 
 import java.util.Arrays;
 import java.util.List;
@@ -26,25 +35,69 @@ public class IntakeClaw extends QQMechanism {
     Servo clawServo;
     Servo leftWristServo;
     Servo rightWristServo;
+    WebcamName webcamName;
     double CLOSED_TIME = 0.5;
     double OPEN_TIME = 0.5;
     double leftWristServoPos;
     double rightWristServoPos;
 
+    public static final int CAMERA_WIDTH = 640;
+    public static final int CAMERA_HEIGHT = 480;
+    int CENTER_X = CAMERA_WIDTH / 2;
+    int CENTER_Y = CAMERA_HEIGHT / 2;
+
+    ColorBlobLocatorProcessor colorLocator;
+
 
     @Override
     public void init(HardwareMap hwMap) {
         clawServo = hwMap.get(Servo.class, "intake_claw_servo");
         leftWristServo = hwMap.get(Servo.class, "left_intake_wrist");
         rightWristServo = hwMap.get(Servo.class,"right_intake_wrist");
+        webcamName = hwMap.get(WebcamName.class, "Webcam 1");
         leftWristServo.setDirection(Servo.Direction.REVERSE);
+
+        colorLocator = new ColorBlobLocatorProcessor.Builder()
+                .setTargetColorRange(ColorRange.RED)         // use a predefined color match
+                .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY)    // exclude blobs inside blobs
+                .setRoi(ImageRegion.asUnityCenterCoordinates(-1, 1, 1, -1))  // search central 1/4 of camera view
+                .setDrawContours(true)                        // Show contours on the Stream Preview
+                .setBlurSize(5)                               // Smooth the transitions between different colors in image
+                .build();
+        @SuppressWarnings("unused")
+        VisionPortal portal = new VisionPortal.Builder()
+                .addProcessor(colorLocator)
+                .setCameraResolution(new Size(CAMERA_WIDTH, CAMERA_HEIGHT))
+                .setCamera(webcamName)
+                .build();
+    }
+
+
+    public ColorBlobLocatorProcessor.Blob blobClosestToCenter() {
+        double closest = 100000000;
+        ColorBlobLocatorProcessor.Blob returnBlob = null;
+
+        List<ColorBlobLocatorProcessor.Blob> blobs = colorLocator.getBlobs();
+        for (ColorBlobLocatorProcessor.Blob blob : blobs) {
+            double distance = getDistanceFromCenter(blob.getBoxFit());
+            if ((blob.getContourArea() > 50) && (distance < closest)) {
+                closest = distance;
+                returnBlob = blob;
+            }
+        }
+        return returnBlob;
+    }
+
+    private double getDistanceFromCenter(RotatedRect boxFit) {
+        return Math.abs(boxFit.center.x - CENTER_X) + Math.abs(boxFit.center.y - CENTER_Y);
     }
 
     @Override
     public List<QQTest> getTests() {
         return Arrays.asList(
                 new TestServo("claw_movement", CLAW_OPEN_POSITION, CLAW_CLOSE_POSITION, clawServo),
-                new TestTwoServos("wrist_movement", WRIST_START_POSITION, WRIST_TRANSFER_POSITION, leftWristServo,rightWristServo)
+                new TestTwoServos("wrist_movement", WRIST_START_POSITION, WRIST_TRANSFER_POSITION, leftWristServo,rightWristServo),
+                new TestWebcam("Webcam", webcamName)
         );
     }
     public void wristTransfer(){
@@ -90,6 +143,8 @@ public boolean isClawClosed() {
     public void update(Telemetry telemetry){
         rightWristServo.setPosition(rightWristServoPos);
         leftWristServo.setPosition(leftWristServoPos);
+        ColorBlobLocatorProcessor.Blob blob = blobClosestToCenter();
+        telemetry.addData("Blob angle", blob.getBoxFit().angle);
     }
 
 }

diff --git a/TeamCode/src/main/res/xml/specimen_bot.xml b/TeamCode/src/main/res/xml/specimen_bot.xml
@@ -0,0 +1,32 @@
+<?xml version='1.0' encoding='UTF-8' standalone='yes' ?>
+<Robot type="FirstInspires-FTC">
+    <LynxUsbDevice name="Control Hub Portal" serialNumber="(embedded)" parentModuleAddress="173">
+        <LynxModule name="Expansion Hub 2" port="2">
+            <goBILDA5202SeriesMotor name="right_score_arm_motor" port="0" />
+            <goBILDA5202SeriesMotor name="right_intake_slide_motor" port="1" />
+            <goBILDA5202SeriesMotor name="left_intake_slide_motor" port="2" />
+            <goBILDA5202SeriesMotor name="left_score_arm_motor" port="3" />
+            <Servo name="right_intake_wrist" port="0" />
+            <Servo name="claw_servo" port="2" />
+            <Servo name="intake_claw_servo" port="4" />
+            <RevTouchSensor name="right_chamber_contact_switch" port="1" />
+            <RevTouchSensor name="left_chamber_contact_switch" port="3" />
+            <RevTouchSensor name="score_switch" port="5" />
+            <RevColorSensorV3 name="scoreclaw_color" port="0" bus="0" />
+            <SparkFunOTOS name="sensor_otos" port="0" bus="1" />
+        </LynxModule>
+        <LynxModule name="Control Hub" port="173">
+            <goBILDA5202SeriesMotor name="back_right_motor" port="0" />
+            <goBILDA5202SeriesMotor name="front_right_motor" port="1" />
+            <goBILDA5202SeriesMotor name="back_left_motor" port="2" />
+            <goBILDA5202SeriesMotor name="front_left_motor" port="3" />
+            <Servo name="right_arm_servo" port="0" />
+            <Servo name="left_intake_wrist" port="2" />
+            <Servo name="left_arm_servo" port="4" />
+            <RevTouchSensor name="score_arm_switch" port="1" />
+            <RevTouchSensor name="slides_switch" port="3" />
+            <LynxEmbeddedIMU name="imu" port="0" bus="0" />
+        </LynxModule>
+    </LynxUsbDevice>
+    <Webcam name="Webcam 1" serialNumber="VGA USB Camera" />
+</Robot>