From c8fe6926d54c97999d444a80d39406bacb3b6301 Mon Sep 17 00:00:00 2001 From: Keerthana Thirukonda Date: Sat, 9 Nov 2024 18:08:08 -0500 Subject: [PATCH] added intake claw code and vision code for orientation of samples --- .../ftc16072/Mechanisms/IntakeClaw.java | 120 ++++++++++++++++++ .../ftc16072/OpModes/ColorLocator.java | 46 +++++-- .../teamcode/ftc16072/OpModes/QQOpMode.java | 4 +- .../ftc16072/OpModes/TestIntakeClaw.java | 12 ++ .../ftc/teamcode/ftc16072/Robot.java | 14 +- .../teamcode/ftc16072/Tests/TestWebcam.java | 18 +++ 6 files changed, 196 insertions(+), 18 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeClaw.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TestIntakeClaw.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestWebcam.java 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 new file mode 100644 index 0000000..87cdb56 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeClaw.java @@ -0,0 +1,120 @@ +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 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.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.Collections; +import java.util.List; + +@Config +public class IntakeClaw extends QQMechanism { + public static double CLAW_CLOSE_POSITION = 0.3; + public static double CLAW_OPEN_POSITION = 0; + public static double WRIST_START_POSITION = 0; + public static double WRIST_END_POSITION = 0.67; + + Servo clawServo; + Servo wristServo; + WebcamName webcamName; + public Telemetry telemetry; + + 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"); + wristServo = hwMap.get(Servo.class, "intake_wrist_servo"); + webcamName = hwMap.get(WebcamName.class, "Webcam 1"); + + 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(); + } + + @Override + public List getTests() { + return Arrays.asList( + new TestServo("claw_movement", CLAW_OPEN_POSITION, CLAW_CLOSE_POSITION, clawServo), + new TestServo("wrist_movement", WRIST_START_POSITION, WRIST_END_POSITION, wristServo), + new TestWebcam("Webcam", webcamName) + ); + } + + @Override + public void update() { + ColorBlobLocatorProcessor.Blob blob = blobClosestToCenter(); + if (blob != null) { + double angle = blob.getBoxFit().angle; + if (blob.getBoxFit().size.height > blob.getBoxFit().size.width) { + angle = 90 + angle; + } + setWristAngle(angle); + } + } + + public ColorBlobLocatorProcessor.Blob blobClosestToCenter() { + double closest = 100000000; + ColorBlobLocatorProcessor.Blob returnBlob = null; + + List 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); + } + + public void open() { + clawServo.setPosition(CLAW_OPEN_POSITION); + } + + public void close() { + clawServo.setPosition(CLAW_CLOSE_POSITION); + } + public void setWristAngle(double angle){ + double servoAngle = WRIST_START_POSITION + (WRIST_END_POSITION - WRIST_START_POSITION) * (angle/180); + if(telemetry != null){ + telemetry.addData("angle", angle); + telemetry.addData("ServoAngle", servoAngle); + } + wristServo.setPosition(servoAngle); + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ColorLocator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ColorLocator.java index 79b74bb..e2348f0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ColorLocator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ColorLocator.java @@ -18,34 +18,58 @@ @Autonomous @SuppressWarnings("unused") public class ColorLocator extends OpMode { + + 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; public void init(){ colorLocator = new ColorBlobLocatorProcessor.Builder() - .setTargetColorRange(ColorRange.BLUE) // use a predefined color match + .setTargetColorRange(ColorRange.RED) // use a predefined color match .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) // exclude blobs inside blobs - .setRoi(ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5)) // search central 1/4 of camera view + .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(320, 240)) + .setCameraResolution(new Size(CAMERA_WIDTH, CAMERA_HEIGHT)) .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) .build(); } public void loop (){ - List blobs = colorLocator.getBlobs(); - ColorBlobLocatorProcessor.Util.filterByArea(50, 2000, blobs); // filter out very small blobs. - telemetry.addLine(" Area Density Aspect Center"); - for(ColorBlobLocatorProcessor.Blob b : blobs) - { - RotatedRect boxFit = b.getBoxFit(); - telemetry.addLine(String.format(Locale.US, "%5d %4.2f %5.2f (%3d,%3d)", - b.getContourArea(), b.getDensity(), b.getAspectRatio(), (int) boxFit.center.x, (int) boxFit.center.y)); + ColorBlobLocatorProcessor.Blob blob = blobClosestToCenter(); + if(blob == null){ + telemetry.addData("Closest", "none"); + } + else{ + double angle = blob.getBoxFit().angle; + if (blob.getBoxFit().size.height > blob.getBoxFit().size.width){ + angle = 90 + angle; + } + telemetry.addData("Closest angle", angle); } + } + public ColorBlobLocatorProcessor.Blob blobClosestToCenter(){ + double closest = 100000000; + ColorBlobLocatorProcessor.Blob returnBlob = null; + List blobs = colorLocator.getBlobs(); + for(ColorBlobLocatorProcessor.Blob blob : blobs) { + double distance = getDistanceFromCenter(blob.getBoxFit()); + if (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); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java index 7a6bd1e..88546be 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java @@ -17,8 +17,8 @@ public void init() { robot.controlHub.resetGyro(); telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); robot.controlHub.resetGyro(); - robot.claw.close(); - robot.arm.goToDrive(); + //robot.claw.close(); + //robot.arm.goToDrive(); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TestIntakeClaw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TestIntakeClaw.java new file mode 100644 index 0000000..06e613c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TestIntakeClaw.java @@ -0,0 +1,12 @@ +package org.firstinspires.ftc.teamcode.ftc16072.OpModes; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +@TeleOp +public class TestIntakeClaw extends QQOpMode{ + @Override + public void init(){ + super.init(); + robot.intakeClaw.telemetry = telemetry; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java index c9e2616..cf5a431 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java @@ -5,6 +5,7 @@ import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Arm; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ControlHub; +import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.IntakeClaw; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.OpticalTrackingOdometrySensor; import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.QQMechanism; @@ -21,6 +22,7 @@ public class Robot { //public DoubleReverse4Bar doubleReverse4Bar; // public Slides slides; public Arm arm; + public IntakeClaw intakeClaw; List mechanisms; public Robot() { @@ -31,16 +33,18 @@ public Robot() { //doubleReverse4Bar = new DoubleReverse4Bar(); //slides = new Slides(); arm = new Arm(); + intakeClaw = new IntakeClaw(); mechanisms = Arrays.asList( controlHub, - mecanumDrive, - otos, - claw, + //mecanumDrive, + //otos, + //claw, // slides, - claw, + //doubleReverse4Bar, - arm); + //arm + intakeClaw); } public void init(HardwareMap hwMap) { for (QQMechanism mechanism : mechanisms) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestWebcam.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestWebcam.java new file mode 100644 index 0000000..9eb583e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestWebcam.java @@ -0,0 +1,18 @@ +package org.firstinspires.ftc.teamcode.ftc16072.Tests; + +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; + +public class TestWebcam extends QQTest{ +WebcamName webcamName; + public TestWebcam(String name, WebcamName webcamName){ + super(name); + this.webcamName = webcamName; + } + @Override + public void run(boolean on, Telemetry telemetry) { + telemetry.addData("Attached", webcamName.isAttached()); + } +}