Examples with RelicRecoveryVuMark org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark used on opensource projects

Example 1 with RelicRecoveryVuMark

use of org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark in project robotcode by OutoftheBoxFTC.

the class BlueLeft method run.

public void run() throws InterruptedException {
    RelicRecoveryVuMark image = imageProvider.getImageReading();
    long lastTime = System.nanoTime();
    while (image == RelicRecoveryVuMark.UNKNOWN && System.nanoTime() - lastTime <= 2000000000) {
        image = imageProvider.getImageReading();
        sleep(50);
    }
    // Check color sensor
    robot.knockOverJewel(Color.RED);
    robot.getIntakeTop().setPower(-1);
    // Drive off balancing stone
    robot.driveToInch(.3, 36);
    // Spring out
    robot.getSpring().setPosition(0.5);
    robot.getIntakeLift().setPower(1);
    sleep(100);
    robot.getIntakeLift().setPower(0.1);
    sleep(150);
    robot.getIntakeServo().setPosition(0.8);
    gyroscopePID.rotate(-45);
    robot.getIntakeBottom().setPower(-1);
    // Drive to glyph pit
    gyroscopePID.drive(1, 23);
    // 0.2 : 0.75
    robot.getIntakeServo().setPosition(0.2);
    sleep(1000);
    robot.getIntakeLift().setPower(1);
    sleep(100);
    robot.getIntakeLift().setPower(0.1);
    // Let block come into intake
    sleep(150);
    gyroscopePID.drive(-0.75, 15);
    int degrees = 200, d2 = 0;
    switch(image) {
        case LEFT:
            degrees = 180;
            gyroscopePID.rotate(170);
            gyroscopePID.drive(0.5, 46);
            gyroscopePID.drive(.6, 4);
            break;
        case RIGHT:
            degrees = 190;
            d2 = 5;
            gyroscopePID.rotate(153.1);
            gyroscopePID.drive(0.5, 43);
            gyroscopePID.drive(.6, 4);
            break;
        default:
            degrees = 180;
            gyroscopePID.rotate(159.5);
            gyroscopePID.drive(0.5, 44);
            gyroscopePID.drive(.6, 4);
    }
    robot.getIntakeBottom().setPower(1);
    robot.getIntakeTop().setPower(1);
    sleep(1000);
    gyroscopePID.drive(-0.3, 5);
    gyroscopePID.drive(0.3, 5);
    gyroscopePID.drive(-0.5, 28);
    gyroscopePID.rotate(degrees);
    robot.getIntakeServo().setPosition(0.8);
    robot.getIntakeTop().setPower(-1);
    robot.getIntakeBottom().setPower(-1);
    gyroscopePID.drive(0.5, 25);
    robot.getIntakeServo().setPosition(0.2);
    sleep(500);
    robot.getIntakeLift().setPower(-1);
    sleep(100);
    robot.getIntakeLift().setPower(0);
    gyroscopePID.rotate(d2);
    robot.getRelicSpool().setPower(-1);
    while (robot.getRelicSpool().getCurrentPosition() > -1500) ;
    robot.getRelicSpool().setPower(0);
    gyroscopePID.drive(-0.5, 2);
}

Example 2 with RelicRecoveryVuMark

use of org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark in project FTC-2017 by FIRST-4030.

the class ConceptVuMarkIdentification method runOpMode.

@Override
public void runOpMode() {
    /*
         * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone);
         * If no camera monitor is desired, use the parameterless constructor instead (commented out below).
         */
    int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
    VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
    // OR...  Do Not Activate the Camera Monitor View, to save power
    // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
    /*
         * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
         * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
         * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
         * web site at https://developer.vuforia.com/license-manager.
         *
         * Vuforia license keys are always 380 characters long, and look as if they contain mostly
         * random data. As an example, here is a example of a fragment of a valid key:
         *      ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
         * Once you've obtained a license key, copy the string from the Vuforia web site
         * and paste it in to your code onthe next line, between the double quotes.
         */
    parameters.vuforiaLicenseKey = "ATsODcD/////AAAAAVw2lR...d45oGpdljdOh5LuFB9nDNfckoxb8COxKSFX";
    /*
         * We also indicate which camera on the RC that we wish to use.
         * Here we chose the back (HiRes) camera (for greater range), but
         * for a competition robot, the front camera might be more convenient.
         */
    parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
    this.vuforia = ClassFactory.createVuforiaLocalizer(parameters);
    /**
     * Load the data set containing the VuMarks for Relic Recovery. There's only one trackable
     * in this data set: all three of the VuMarks in the game were created from this one template,
     * but differ in their instance id information.
     * @see VuMarkInstanceId
     */
    VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
    VuforiaTrackable relicTemplate = relicTrackables.get(0);
    // can help in debugging; otherwise not necessary
    relicTemplate.setName("relicVuMarkTemplate");
    telemetry.addData(">", "Press Play to start");
    telemetry.update();
    waitForStart();
    relicTrackables.activate();
    while (opModeIsActive()) {
        /**
         * See if any of the instances of {@link relicTemplate} are currently visible.
         * {@link RelicRecoveryVuMark} is an enum which can have the following values:
         * UNKNOWN, LEFT, CENTER, and RIGHT. When a VuMark is visible, something other than
         * UNKNOWN will be returned by {@link RelicRecoveryVuMark#from(VuforiaTrackable)}.
         */
        RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
        if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
            /* Found an instance of the template. In the actual game, you will probably
                 * loop until this condition occurs, then move on to act accordingly depending
                 * on which VuMark was visible. */
            telemetry.addData("VuMark", "%s visible", vuMark);
            /* For fun, we also exhibit the navigational pose. In the Relic Recovery game,
                 * it is perhaps unlikely that you will actually need to act on this pose information, but
                 * we illustrate it nevertheless, for completeness. */
            OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) relicTemplate.getListener()).getPose();
            telemetry.addData("Pose", format(pose));
            /* We further illustrate how to decompose the pose into useful rotational and
                 * translational components */
            if (pose != null) {
                VectorF trans = pose.getTranslation();
                Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
                // Extract the X, Y, and Z components of the offset of the target relative to the robot
                double tX = trans.get(0);
                double tY = trans.get(1);
                double tZ = trans.get(2);
                // Extract the rotational components of the target relative to the robot
                double rX = rot.firstAngle;
                double rY = rot.secondAngle;
                double rZ = rot.thirdAngle;
            }
        } else {
            telemetry.addData("VuMark", "not visible");
        }
        telemetry.update();
    }
}

Example 3 with RelicRecoveryVuMark

use of org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark in project robotcode by OutoftheBoxFTC.

the class BlueRight method run.

@Override
public void run() throws InterruptedException {
    RelicRecoveryVuMark image = imageProvider.getImageReading();
    long lastTime = System.nanoTime();
    while (image == RelicRecoveryVuMark.UNKNOWN && System.nanoTime() - lastTime <= 2000000000) {
        image = imageProvider.getImageReading();
        sleep(50);
    }
    telemetry.addData("Image", image);
    telemetry.update();
    // Check color sensor
    robot.knockOverJewel(Color.RED);
    robot.getIntakeTop().setPower(-1);
    robot.getIntakeBottom().setPower(-1);
    // Drive off balancing stone
    robot.driveToInch(.3, 30);
    // Spring out glyph
    robot.getSpring().setPosition(0.5);
    sleep(750);
    robot.getIntakeServo().setPosition(0.8);
    // Rotate to face glyph //90
    gyroscopePID.rotate(90);
    gyroscopePID.drive(1, 28);
    robot.getIntakeServo().setPosition(0.2);
    sleep(1000);
    robot.getIntakeLift().setPower(1);
    sleep(100);
    robot.getIntakeLift().setPower(0.1);
    // Let block come into intake
    sleep(150);
    gyroscopePID.drive(-0.75, 3);
    int degrees = 0;
    switch(image) {
        case RIGHT:
            degrees = 5;
            // 151
            gyroscopePID.rotate(61);
            gyroscopePID.drive(0.5, 47);
            break;
        case LEFT:
            // 169
            gyroscopePID.rotate(79);
            gyroscopePID.drive(0.5, 36);
            break;
        default:
            degrees = 5;
            // 159
            gyroscopePID.rotate(69);
            gyroscopePID.drive(0.5, 42);
    }
    robot.getIntakeBottom().setPower(1);
    robot.getIntakeTop().setPower(1);
    sleep(1000);
    gyroscopePID.drive(-0.3, 11);
    gyroscopePID.drive(0.5, 9);
    gyroscopePID.drive(-1, 13);
    // 
    gyroscopePID.rotate(170);
    robot.getIntakeServo().setPosition(0.8);
    robot.getIntakeTop().setPower(-1);
    robot.getIntakeBottom().setPower(-1);
    gyroscopePID.drive(0.5, 25);
    robot.getIntakeServo().setPosition(0.2);
    sleep(500);
    robot.getIntakeLift().setPower(-1);
    sleep(100);
    robot.getIntakeLift().setPower(0);
    gyroscopePID.rotate(degrees);
    robot.getRelicSpool().setPower(-1);
    while (robot.getRelicSpool().getCurrentPosition() > -1000) ;
    robot.getRelicSpool().setPower(0);
}

Example 4 with RelicRecoveryVuMark

use of org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark in project robotcode by OutoftheBoxFTC.

the class RedRight method run.

public void run() throws InterruptedException {
    RelicRecoveryVuMark image = imageProvider.getImageReading();
    long lastTime = System.nanoTime();
    while (image == RelicRecoveryVuMark.UNKNOWN && System.nanoTime() - lastTime <= 2000000000) {
        image = imageProvider.getImageReading();
        sleep(50);
    }
    telemetry.addData("Image", image);
    telemetry.update();
    // Check color sensor
    robot.knockOverJewel(Color.BLUE);
    robot.getIntakeTop().setPower(-1);
    // Drive off balancing stone
    robot.driveToInch(.3, 38);
    // Spring out glyph
    robot.getSpring().setPosition(0.5);
    robot.getIntakeLift().setPower(1);
    while (robot.getIntakeLift().getCurrentPosition() <= 400) {
    }
    robot.getIntakeLift().setPower(0.1);
    // Let block come into intake
    sleep(150);
    robot.getIntakeServo().setPosition(0.8);
    gyroscopePID.rotate(50);
    robot.getIntakeBottom().setPower(-1);
    // Drive to glyph pit
    gyroscopePID.drive(1, 10);
    // 0.2 : 0.75
    robot.getIntakeServo().setPosition(0.2);
    sleep(500);
    robot.getIntakeLift().setPower(-1);
    while (robot.getIntakeLift().getCurrentPosition() >= 100) {
    }
    robot.getIntakeLift().setPower(0.1);
    robot.driveIntakeVertical(0.5);
    sleep(500);
    robot.driveIntakeVertical(0);
    robot.getIntakeLift().setPower(1);
    while (robot.getIntakeLift().getCurrentPosition() <= 200) {
    }
    robot.getIntakeLift().setPower(0.1);
    gyroscopePID.drive(-1, 3);
    switch(image) {
        case LEFT:
            // -108
            gyroscopePID.rotate(-110);
            gyroscopePID.drive(0.5, 36);
            gyroscopePID.drive(.3, 4);
            break;
        case RIGHT:
            // -124
            gyroscopePID.rotate(-125);
            gyroscopePID.drive(0.5, 46);
            gyroscopePID.drive(.3, 4);
            break;
        default:
            // -115
            gyroscopePID.rotate(-116);
            gyroscopePID.drive(0.5, 40);
            gyroscopePID.drive(.3, 4);
    }
    outtake();
    gyroscopePID.drive(-0.5, 14);
    robot.getIntakeTop().setPower(-1);
    robot.getIntakeBottom().setPower(-1);
    gyroscopePID.drive(-0.8, 16);
    gyroscopePID.rotate(60);
    robot.getIntakeLift().setPower(1);
    while (robot.getIntakeLift().getCurrentPosition() <= 400) {
    }
    robot.getIntakeLift().setPower(0.1);
    robot.getIntakeServo().setPosition(0.8);
    gyroscopePID.drive(0.6, 30);
    robot.driveIntakeVertical(0.5);
    robot.getIntakeServo().setPosition(0.2);
    sleep(500);
    robot.getIntakeLift().setPower(-1);
    while (robot.getIntakeLift().getCurrentPosition() >= 100) {
    }
    robot.getIntakeLift().setPower(1);
    while (robot.getIntakeLift().getCurrentPosition() <= 200) {
    }
    robot.getIntakeLift().setPower(0.1);
    if (image.equals(RelicRecoveryVuMark.LEFT) || image.equals(RelicRecoveryVuMark.UNKNOWN) || image.equals(RelicRecoveryVuMark.CENTER))
        robot.driveIntakeVertical(0);
    gyroscopePID.drive(-1, 30);
    switch(image) {
        case LEFT:
            gyroscopePID.rotate(-125);
            gyroscopePID.drive(1, 26);
            break;
        case RIGHT:
            gyroscopePID.rotate(-120);
            gyroscopePID.drive(0.5, 46);
            gyroscopePID.drive(.3, 4);
            break;
        default:
            gyroscopePID.rotate(-102.5);
            gyroscopePID.drive(0.5, 27);
    }
    robot.driveIntakeVertical(0);
    outtake();
    gyroscopePID.drive(-0.5, 10);
}

Example 5 with RelicRecoveryVuMark

use of org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark in project robotcode by OutoftheBoxFTC.

the class RedLeft method run.

public void run() throws InterruptedException {
    RelicRecoveryVuMark image = imageProvider.getImageReading();
    long lastTime = System.nanoTime();
    while (image == RelicRecoveryVuMark.UNKNOWN && System.nanoTime() - lastTime <= 2000000000) {
        image = imageProvider.getImageReading();
        sleep(50);
    }
    telemetry.addData("Image", image);
    telemetry.update();
    // Check color sensor
    robot.knockOverJewel(Color.BLUE);
    robot.getIntakeTop().setPower(-1);
    robot.getIntakeBottom().setPower(-1);
    // Drive off balancing stone
    robot.driveToInch(.3, 30);
    // Spring out glyph
    robot.getSpring().setPosition(0.5);
    sleep(750);
    robot.getIntakeServo().setPosition(0.8);
    // Rotate to face glyph
    gyroscopePID.rotate(-90);
    gyroscopePID.drive(1, 28);
    robot.getIntakeServo().setPosition(0.2);
    sleep(1000);
    robot.getIntakeLift().setPower(1);
    sleep(100);
    robot.getIntakeLift().setPower(0.1);
    // Let block come into intake
    sleep(150);
    gyroscopePID.drive(-0.75, 3);
    int relicExtend = 1000, degrees = 10;
    switch(image) {
        case LEFT:
            // -150
            gyroscopePID.rotate(-60);
            gyroscopePID.drive(0.5, 47);
            break;
        case RIGHT:
            // -170
            gyroscopePID.rotate(-80);
            gyroscopePID.drive(0.5, 36);
            relicExtend = 1200;
            degrees = 15;
            break;
        default:
            // -161
            gyroscopePID.rotate(-71);
            gyroscopePID.drive(0.5, 42);
            relicExtend = 1200;
    }
    robot.getIntakeBottom().setPower(1);
    robot.getIntakeTop().setPower(1);
    sleep(1000);
    gyroscopePID.drive(-0.3, 11);
    gyroscopePID.drive(0.5, 9);
    gyroscopePID.drive(-1, 13);
    // 9
    gyroscopePID.rotate(170);
    robot.getIntakeServo().setPosition(0.8);
    robot.getIntakeTop().setPower(-1);
    robot.getIntakeBottom().setPower(-1);
    gyroscopePID.drive(0.5, 30);
    robot.getIntakeServo().setPosition(0.2);
    sleep(500);
    robot.getIntakeLift().setPower(-1);
    sleep(100);
    robot.getIntakeLift().setPower(0);
    // 19
    gyroscopePID.rotate(degrees);
    robot.getRelicSpool().setPower(-1);
    while (robot.getRelicSpool().getCurrentPosition() > -relicExtend) ;
    robot.getRelicSpool().setPower(0);
}