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

Working camera and gantry

This commit is contained in:
Scott Barnes 2023-11-14 18:28:53 -06:00
parent dcdb25632f
commit 0dfcef62af
6 changed files with 85 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
TeamCode/src/main/java/opmodes/MainTeleOp.java
View File

@ -1,7 +1,11 @@
package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_LIFT_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLIDE_POWER;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLIDE_UP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_CENTER;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MIN;
@ -25,11 +29,11 @@ public class MainTeleOp extends OpMode {
private boolean previousRobotLiftExtend = false;
private boolean liftArmShouldBeUp = false;
private boolean screwArmIsMoving = false;
private Telemetry dashboard;
private FtcDashboard dashboard;
@Override
public void init() {
this.dashboard = FtcDashboard.getInstance().getTelemetry();
this.dashboard = FtcDashboard.getInstance();
this.clawArmPosition = PICKUP_ARM_MAX;
this.robot = new Robot(this.hardwareMap, telemetry);
@ -60,8 +64,7 @@ public class MainTeleOp extends OpMode {
boolean screwIntake = gamepad2.left_bumper; // LB
boolean slideUp = gamepad2.left_stick_y < -0.25; // Left Y Axis Joystick
boolean slideDown = gamepad2.left_stick_y > 0.25; // Left Y Axis Joystick
boolean gantryLeft = gamepad2.dpad_left; // dpad left
boolean gantryRight = gamepad2.dpad_right; // dpad right
double gantryXInput = -gamepad2.right_stick_x;
// Claw
if (openClaw) {
@ -102,19 +105,19 @@ public class MainTeleOp extends OpMode {
} else {
this.robot.getGantry().stop();
}
// if ((!previousSlideUp && slideUp) || (!previousSlideDown && slideDown)) {
// int currentPosition = this.robot.getGantry().getSlidePosition();
// this.robot.getGantry().setTarget(currentPosition + GANTRY_LIFT_DELTA);
// }
//
if (gantryRight) {
this.robot.getGantry().setX(X_MIN);
} else if (gantryLeft) {
this.robot.getGantry().setX(X_MAX);
} else {
this.robot.getGantry().center();
if (slideUp) {
this.robot.getGantry().setSlideTarget(SLIDE_UP);
} else if (slideDown) {
this.robot.getGantry().setSlideTarget(0);
} else if (previousSlideUp || previousSlideDown){
this.robot.getGantry().setSlideTarget(this.robot.getGantry().getSlidePosition());
}
double gantryXPosition = X_CENTER + (gantryXInput * 0.5);
gantryXPosition = Math.max(X_MIN, Math.min(gantryXPosition, X_MAX));
this.robot.getGantry().setX(gantryXPosition);
// Robot Lift
if (robotLiftRotation || this.liftArmShouldBeUp) {

56
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java
View File

@ -2,54 +2,38 @@ package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.WEBCAM_NAME;
import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION;
import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_HEIGHT;
import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_ROTATION;
import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_WIDTH;
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.vision.Detection;
import org.firstinspires.ftc.teamcode.vision.TargetingPipeline;
import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
import org.firstinspires.ftc.teamcode.vision.ColorDetectionPipeline;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
public class Camera {
private final HardwareMap hardwareMap;
private OpenCvCamera targetingCamera;
private TargetingPipeline targetingPipeline;
private boolean targetingCameraInitialized;
private ColorDetectionPipeline colorDetectionPipeline;
private AprilTagProcessor aprilTag;
private VisionPortal visionPortal;
private Telemetry telemetry;
public Camera(HardwareMap hardwareMap) {
this.hardwareMap = hardwareMap;
private boolean initialized;
public Camera(HardwareMap hardwareMap, Telemetry telemetry) {
this.telemetry = telemetry;
this.init(hardwareMap);
}
public void initTargetingCamera() {
int targetingCameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
this.targetingCamera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, WEBCAM_NAME), targetingCameraMonitorViewId);
this.targetingPipeline = new TargetingPipeline();
targetingCamera.setPipeline(targetingPipeline);
targetingCamera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
@Override
public void onOpened() {
targetingCamera.startStreaming(WEBCAM_WIDTH, WEBCAM_HEIGHT, WEBCAM_ROTATION);
targetingCameraInitialized = true;
}
@Override
public void onError(int errorCode) {
}
});
}
public void stopTargetingCamera() {
if (targetingCameraInitialized) {
targetingCamera.closeCameraDeviceAsync(() -> targetingCameraInitialized = false);
}
private void init(HardwareMap hardwareMap) {
this.aprilTag = AprilTagProcessor.easyCreateWithDefaults();
this.colorDetectionPipeline = new ColorDetectionPipeline();
this.visionPortal = VisionPortal.easyCreateWithDefaults(
hardwareMap.get(WebcamName.class, WEBCAM_NAME), aprilTag, colorDetectionPipeline);
this.initialized = true;
}
public Detection getRed() {
return (targetingCameraInitialized ? targetingPipeline.getRed() : INVALID_DETECTION);
return (initialized ? colorDetectionPipeline.getRed() : INVALID_DETECTION);
}
}

26
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java
View File

@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_MIN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLIDE_POWER;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_CENTER;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_NAME;
@ -28,7 +29,7 @@ public class Gantry {
private final Servo armServo;
private final CRServo screwServo;
private final DcMotor liftLeft;
private final DcMotor right;
private final DcMotor liftRight;
PController armController = new PController(GANTRY_ARM_KP);
private double armControllerTarget;
PController xController = new PController(X_KP);
@ -43,11 +44,14 @@ public class Gantry {
this.liftLeft = hardwareMap.get(DcMotor.class, LEFT_SLIDE_MOTOR_NAME);
this.liftLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.liftLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
this.liftLeft.setTargetPosition(0);
this.liftLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.right = hardwareMap.get(DcMotor.class, RIGHT_SLIDE_MOTOR_NAME);
this.right.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.right.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
this.liftRight = hardwareMap.get(DcMotor.class, RIGHT_SLIDE_MOTOR_NAME);
this.liftRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.liftRight.setTargetPosition(0);
this.liftRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.liftRight.setDirection(DcMotorSimple.Direction.REVERSE);
this.xControllerTarget = X_MIN;
}
@ -59,12 +63,10 @@ public class Gantry {
public void setSlideTarget(int target) {
this.liftLeft.setTargetPosition(target);
this.liftLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.liftLeft.setPower(1);
this.liftLeft.setPower(SLIDE_POWER);
this.right.setTargetPosition(target);
this.right.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.right.setPower(1);
this.liftRight.setTargetPosition(target);
this.liftRight.setPower(SLIDE_POWER);
}
public void setX(double x)
@ -102,6 +104,10 @@ public class Gantry {
this.setX(X_CENTER);
}
public int getSlidePosition() {
return this.liftLeft.getCurrentPosition();
}
public boolean isIn() {
double fudge = (GANTRY_ARM_MAX - GANTRY_ARM_MIN) * .75;
return this.armServo.getPosition() < GANTRY_ARM_MIN + fudge;

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -4,6 +4,7 @@ import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.checkerframework.checker.units.qual.C;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
@ -23,6 +24,9 @@ public class Robot {
@Getter
private RobotLift lift;
@Getter
private Camera camera;
private final Telemetry telemetry;
public Robot(HardwareMap hardwareMap, Telemetry telemetry) {
@ -36,6 +40,7 @@ public class Robot {
this.gantry = new Gantry(hardwareMap, telemetry);
this.claw = new Claw(hardwareMap, telemetry);
this.lift = new RobotLift(hardwareMap, telemetry);
this.camera = new Camera(hardwareMap, telemetry);
}
public void update() {

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java
View File

@ -28,8 +28,6 @@ public class RobotConstants {
public static double SPEED = 1f;
public static double TURN = 1f;
// Slide
// Arm
public static double PICKUP_ARM_MIN = 0.185;
public static double PICKUP_ARM_MAX = 0.755;
@ -47,7 +45,8 @@ public class RobotConstants {
public static double X_MIN = 0.16;
public static double X_CENTER = 0.54;
public static double GANTRY_ARM_DELTA_MAX = 0.013;
public static int SLIDE_UP = 100;
public static int SLIDE_UP = 820;
public static double SLIDE_POWER = 0.5;
// Robot Lift

38
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java
View File

@ -11,38 +11,43 @@ import static org.firstinspires.ftc.teamcode.util.Constants.RED;
import static org.firstinspires.ftc.teamcode.util.Constants.STRUCTURING_ELEMENT;
import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getLargestContour;
import android.graphics.Canvas;
import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
import org.firstinspires.ftc.vision.VisionProcessor;
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import org.openftc.easyopencv.OpenCvPipeline;
import java.util.ArrayList;
public class TargetingPipeline extends OpenCvPipeline {
public class ColorDetectionPipeline implements VisionProcessor {
Mat blurred = new Mat();
Mat hsv = new Mat();
Mat redMask1 = new Mat();
Mat redMask2 = new Mat();
Mat redMask = new Mat();
Mat whiteMask = new Mat();
Scalar redGoalLower1;
Scalar redGoalUpper1;
Scalar redGoalLower2;
Scalar redGoalUpper2;
Scalar redLower1;
Scalar redUpper1;
Scalar redLower2;
Scalar redUpper2;
private Detection red;
// Init
@Override
public void init(Mat input) {
red = new Detection(input.size(), CV_MIN_GOAL_AREA);
public void init(int width, int height, CameraCalibration calibration) {
red = new Detection(new Size(width, height), CV_MIN_GOAL_AREA);
}
// Process each frame that is received from the webcam
@Override
public Mat processFrame(Mat input) {
public Object processFrame(Mat input, long captureTimeNanos) {
Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0);
Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV);
@ -51,15 +56,20 @@ public class TargetingPipeline extends OpenCvPipeline {
return input;
}
@Override
public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) {
}
// Update the Red Goal Detection
private void updateRed(Mat input) {
// take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise
redGoalLower1 = new Scalar(FTC_RED_LOWER.getH(), FTC_RED_LOWER.getS(), FTC_RED_LOWER.getV());
redGoalUpper1 = new Scalar(180, FTC_RED_UPPER.getS(), FTC_RED_UPPER.getV());
redGoalLower2 = new Scalar(0, FTC_RED_LOWER.getS(), FTC_RED_LOWER.getV());
redGoalUpper2 = new Scalar(FTC_RED_UPPER.getH(), FTC_RED_UPPER.getS(), FTC_RED_UPPER.getV());
Core.inRange(hsv, redGoalLower1, redGoalUpper1, redMask1);
Core.inRange(hsv, redGoalLower2, redGoalUpper2, redMask2);
redLower1 = new Scalar(FTC_RED_LOWER.getH(), FTC_RED_LOWER.getS(), FTC_RED_LOWER.getV());
redUpper1 = new Scalar(180, FTC_RED_UPPER.getS(), FTC_RED_UPPER.getV());
redLower2 = new Scalar(0, FTC_RED_LOWER.getS(), FTC_RED_LOWER.getV());
redUpper2 = new Scalar(FTC_RED_UPPER.getH(), FTC_RED_UPPER.getS(), FTC_RED_UPPER.getV());
Core.inRange(hsv, redLower1, redUpper1, redMask1);
Core.inRange(hsv, redLower2, redUpper2, redMask2);
Core.add(redMask1, redMask2, redMask);
Imgproc.erode(redMask, redMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
Imgproc.dilate(redMask, redMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);