diff --git a/FtcRobotController/build.gradle b/FtcRobotController/build.gradle index 13a0bb952f5..6cf3668d107 100644 --- a/FtcRobotController/build.gradle +++ b/FtcRobotController/build.gradle @@ -22,6 +22,8 @@ repositories { flatDir { dirs '../libs' } + google() + jcenter() } apply from: 'build.release.gradle' diff --git a/FtcRobotController/build.release.gradle b/FtcRobotController/build.release.gradle index 1cb2035ed48..fe5b8d77c56 100644 --- a/FtcRobotController/build.release.gradle +++ b/FtcRobotController/build.release.gradle @@ -1,8 +1,8 @@ dependencies { - compile (name:'Inspection-release', ext: 'aar') - compile (name:'Blocks-release', ext: 'aar') - compile (name:'RobotCore-release', ext: 'aar') - compile (name:'Hardware-release', ext: 'aar') - compile (name:'FtcCommon-release', ext: 'aar') - compile (name:'WirelessP2p-release', ext:'aar') + implementation (name:'Inspection-release', ext: 'aar') + implementation (name:'Blocks-release', ext: 'aar') + implementation (name:'RobotCore-release', ext: 'aar') + implementation (name:'Hardware-release', ext: 'aar') + implementation (name:'FtcCommon-release', ext: 'aar') + implementation (name:'WirelessP2p-release', ext:'aar') } diff --git a/TeamCode/build.release.gradle b/TeamCode/build.release.gradle index 8a645d1b6fc..c387347776f 100644 --- a/TeamCode/build.release.gradle +++ b/TeamCode/build.release.gradle @@ -4,4 +4,5 @@ dependencies { implementation (name: 'Hardware-release', ext: 'aar') implementation (name: 'FtcCommon-release', ext: 'aar') implementation (name:'WirelessP2p-release', ext:'aar') + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tele.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tele.java new file mode 100644 index 00000000000..4aa339b2b21 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tele.java @@ -0,0 +1,76 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import java.lang.Math; + +@TeleOp(name="TeleOp", group="Linear Opmode") +public class Tele extends LinearOpMode { + + // Declare OpMode members. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftMotorFront = null; + private DcMotor leftMotorBack = null; + private DcMotor rightMotorFront = null; + private DcMotor rightMotorBack = null; + + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + + leftMotorFront = hardwareMap.get(DcMotor.class, "left_motor_front"); + leftMotorBack = hardwareMap.get(DcMotor.class, "left_motor_back"); + rightMotorFront = hardwareMap.get(DcMotor.class, "right_motor_front"); + rightMotorBack = hardwareMap.get(DcMotor.class, "right_motor_back"); + leftMotorFront.setDirection(DcMotor.Direction.REVERSE); + leftMotorBack.setDirection(DcMotor.Direction.REVERSE); + + waitForStart(); + runtime.reset(); + + while (opModeIsActive()) { + //Stuff to display for Telemetry + telemetry.addData("Left Motor Power", leftMotorFront.getPower()); + telemetry.addData("Right Motor Power", rightMotorFront.getPower()); + telemetry.update(); + + if (gamepad1.left_stick_y != 0 && gamepad1.right_stick_x == 0) { + leftMotorFront.setPower(gamepad1.left_stick_y); + leftMotorBack.setPower(gamepad1.left_stick_y); + rightMotorFront.setPower(gamepad1.left_stick_y); + rightMotorBack.setPower(gamepad1.left_stick_y); + } + if (gamepad1.right_stick_x != 0 && gamepad1.left_stick_y == 0) { + leftMotorFront.setPower(-gamepad1.right_stick_x); + leftMotorBack.setPower(-gamepad1.right_stick_x); + rightMotorFront.setPower(gamepad1.right_stick_x); + rightMotorBack.setPower(gamepad1.right_stick_x); + } + if (gamepad1.left_stick_y != 0 && gamepad1.right_stick_x > 0) { + leftMotorFront.setPower(0); + leftMotorBack.setPower(0); + rightMotorFront.setPower(Math.abs(gamepad1.right_stick_x) + Math.abs(gamepad1.left_stick_y)/2); + rightMotorBack.setPower(Math.abs(gamepad1.right_stick_x) + Math.abs(gamepad1.left_stick_y)/2); + } + if (gamepad1.left_stick_y != 0 && gamepad1.right_stick_x < 0) { + leftMotorFront.setPower(Math.abs(gamepad1.right_stick_x) + Math.abs(gamepad1.left_stick_y)/2); + leftMotorBack.setPower(Math.abs(gamepad1.right_stick_x) + Math.abs(gamepad1.left_stick_y)/2); + rightMotorFront.setPower(0); + rightMotorBack.setPower(0); + } + else { + leftMotorFront.setPower(0); + leftMotorBack.setPower(0); + rightMotorFront.setPower(0); + rightMotorBack.setPower(0); + } + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.java index 3b28d90f8e2..c41b91c11a6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.java @@ -20,6 +20,7 @@ public void runOpMode() { telemetry.addData("Status", "Initialized"); telemetry.update(); + test = hardwareMap.get(DcMotor.class, "test"); test.setDirection(DcMotor.Direction.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaID.java new file mode 100644 index 00000000000..b00b4bfdeb6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaID.java @@ -0,0 +1,186 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcontroller.external.samples.ConceptVuforiaNavigation; +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +import org.firstinspires.ftc.robotcore.external.matrices.VectorF; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark; +import org.firstinspires.ftc.robotcore.external.navigation.VuMarkInstanceId; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; + +/** + * This OpMode illustrates the basics of using the Vuforia engine to determine + * the identity of Vuforia VuMarks encountered on the field. The code is structured as + * a LinearOpMode. It shares much structure with {@link ConceptVuforiaNavigation}; we do not here + * duplicate the core Vuforia documentation found there, but rather instead focus on the + * differences between the use of Vuforia for navigation vs VuMark identification. + * + * @see ConceptVuforiaNavigation + * @see VuforiaLocalizer + * @see VuforiaTrackableDefaultListener + * see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list. + * + * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as + * is explained in {@link ConceptVuforiaNavigation}. + */ + +@TeleOp(name="VuforiaID", group ="Concept") +public class VuforiaID extends LinearOpMode { + + public static final String TAG = "Vuforia VuMark Sample"; + + OpenGLMatrix lastLocation = null; + + /** + * {@link #vuforia} is the variable we will use to store our instance of the Vuforia + * localization engine. + */ + VuforiaLocalizer vuforia; + + @Override public void runOpMode() { + + /* + * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone); + */ + 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 on the next line, between the double quotes. + */ + parameters.vuforiaLicenseKey = "AfDTG3b/////AAABmQnAWkqPDkVztdcIurPa8w5lO6BWlTpltO5r1m4s9oA9w3cfFdIBqJ4rjZB0We4YHcRMdc5LkWwOvJk+xcDr2VFpLP8m6zEqLWlrVNQvBnNxmGO8BeZ+xRQeQ34AgPhrqQQqeheWJbfoOn+lekIz2ZMm9f+j+1ng/X0vKDHyFGfxbtXbJuUx4Qh6E3t0esH0b3VQtbuJiOOTpWi9xFAqBsHWp+DQbwub+a6HZV5q42OabnOAyr0GZ7u1vJZs+I/Vlnf7qEMLD4RTIYA5OmMyzOdl5aikZqDSgG223ETSwcbwd3QFKewYE3oXXxkpI0vmsxCiaqBJ1oL9e6n0RXbC8Zdvn2VYwh6oSemcpp+fSjGa"; + + /* + * 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; + + /** + * Instantiate the Vuforia engine + */ + vuforia = ClassFactory.getInstance().createVuforia(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 diffaer in their instance id information. + * @see VuMarkInstanceId + */ + VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark"); + VuforiaTrackable relicTemplate = relicTrackables.get(0); + relicTemplate.setName("relicVuMarkTemplate"); // can help in debugging; otherwise not necessary + + 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(); + } + } + + String format(OpenGLMatrix transformationMatrix) { + return (transformationMatrix != null) ? transformationMatrix.formatAsTransform() : "null"; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaNav.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaNav.java new file mode 100644 index 00000000000..d1de5a56403 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VuforiaNav.java @@ -0,0 +1,310 @@ +/* Copyright (c) 2018 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +import org.firstinspires.ftc.robotcore.external.matrices.VectorF; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; + +import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES; +import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ; +import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.YZX; +import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC; +import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection.BACK; +import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection.FRONT; + +import java.util.ArrayList; +import java.util.List; + + +/** + * This 2018-2019 OpMode illustrates the basics of using the Vuforia localizer to determine + * positioning and orientation of robot on the FTC field. + * The code is structured as a LinearOpMode + * + * Vuforia uses the phone's camera to inspect it's surroundings, and attempt to locate target images. + * + * When images are located, Vuforia is able to determine the position and orientation of the + * image relative to the camera. This sample code than combines that information with a + * knowledge of where the target images are on the field, to determine the location of the camera. + * + * This example assumes a "square" field configuration where the red and blue alliance stations + * are on opposite walls of each other. + * + * From the Audience perspective, the Red Alliance station is on the right and the + * Blue Alliance Station is on the left. + + * The four vision targets are located in the center of each of the perimeter walls with + * the images facing inwards towards the robots: + * - BlueRover is the Mars Rover image target on the wall closest to the blue alliance + * - RedFootprint is the Lunar Footprint target on the wall closest to the red alliance + * - FrontCraters is the Lunar Craters image target on the wall closest to the audience + * - BackSpace is the Deep Space image target on the wall farthest from the audience + * + * A final calculation then uses the location of the camera on the robot to determine the + * robot's location and orientation on the field. + * + * @see VuforiaLocalizer + * @see VuforiaTrackableDefaultListener + * see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list. + * + * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as + * is explained below. + */ + +@TeleOp(name="VuforiaNavigation", group ="Concept") +public class VuforiaNav extends LinearOpMode { + + /* + * 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 on the next line, between the double quotes. + */ + private static final String VUFORIA_KEY = "AfDTG3b/////AAABmQnAWkqPDkVztdcIurPa8w5lO6BWlTpltO5r1m4s9oA9w3cfFdIBqJ4rjZB0We4YHcRMdc5LkWwOvJk+xcDr2VFpLP8m6zEqLWlrVNQvBnNxmGO8BeZ+xRQeQ34AgPhrqQQqeheWJbfoOn+lekIz2ZMm9f+j+1ng/X0vKDHyFGfxbtXbJuUx4Qh6E3t0esH0b3VQtbuJiOOTpWi9xFAqBsHWp+DQbwub+a6HZV5q42OabnOAyr0GZ7u1vJZs+I/Vlnf7qEMLD4RTIYA5OmMyzOdl5aikZqDSgG223ETSwcbwd3QFKewYE3oXXxkpI0vmsxCiaqBJ1oL9e6n0RXbC8Zdvn2VYwh6oSemcpp+fSjGa"; + + // Since ImageTarget trackables use mm to specifiy their dimensions, we must use mm for all the physical dimension. + // We will define some constants and conversions here + private static final float mmPerInch = 25.4f; + private static final float mmFTCFieldWidth = (12*6) * mmPerInch; // the width of the FTC field (from the center point to the outer panels) + private static final float mmTargetHeight = (6) * mmPerInch; // the height of the center of the target image above the floor + + // Select which camera you want use. The FRONT camera is the one on the same side as the screen. + // Valid choices are: BACK or FRONT + private static final VuforiaLocalizer.CameraDirection CAMERA_CHOICE = BACK; + + private OpenGLMatrix lastLocation = null; + private boolean targetVisible = false; + + /** + * {@link #vuforia} is the variable we will use to store our instance of the Vuforia + * localization engine. + */ + VuforiaLocalizer vuforia; + + @Override public void runOpMode() { + /* + * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine. + * We can pass Vuforia the handle to a camera preview resource (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); + + // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + parameters.vuforiaLicenseKey = VUFORIA_KEY ; + parameters.cameraDirection = CAMERA_CHOICE; + + // Instantiate the Vuforia engine + vuforia = ClassFactory.getInstance().createVuforia(parameters); + + // Load the data sets that for the trackable objects. These particular data + // sets are stored in the 'assets' part of our application. + VuforiaTrackables targetsRoverRuckus = this.vuforia.loadTrackablesFromAsset("RoverRuckus"); + VuforiaTrackable blueRover = targetsRoverRuckus.get(0); + blueRover.setName("Blue-Rover"); + VuforiaTrackable redFootprint = targetsRoverRuckus.get(1); + redFootprint.setName("Red-Footprint"); + VuforiaTrackable frontCraters = targetsRoverRuckus.get(2); + frontCraters.setName("Front-Craters"); + VuforiaTrackable backSpace = targetsRoverRuckus.get(3); + backSpace.setName("Back-Space"); + + // For convenience, gather together all the trackable objects in one easily-iterable collection */ + List allTrackables = new ArrayList(); + allTrackables.addAll(targetsRoverRuckus); + + /** + * In order for localization to work, we need to tell the system where each target is on the field, and + * where the phone resides on the robot. These specifications are in the form of transformation matrices. + * Transformation matrices are a central, important concept in the math here involved in localization. + * See Transformation Matrix + * for detailed information. Commonly, you'll encounter transformation matrices as instances + * of the {@link OpenGLMatrix} class. + * + * If you are standing in the Red Alliance Station looking towards the center of the field, + * - The X axis runs from your left to the right. (positive from the center to the right) + * - The Y axis runs from the Red Alliance Station towards the other side of the field + * where the Blue Alliance Station is. (Positive is from the center, towards the BlueAlliance station) + * - The Z axis runs from the floor, upwards towards the ceiling. (Positive is above the floor) + * + * This Rover Ruckus sample places a specific target in the middle of each perimeter wall. + * + * Before being transformed, each target image is conceptually located at the origin of the field's + * coordinate system (the center of the field), facing up. + */ + + /** + * To place the BlueRover target in the middle of the blue perimeter wall: + * - First we rotate it 90 around the field's X axis to flip it upright. + * - Then, we translate it along the Y axis to the blue perimeter wall. + */ + OpenGLMatrix blueRoverLocationOnField = OpenGLMatrix + .translation(0, mmFTCFieldWidth, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 0)); + blueRover.setLocation(blueRoverLocationOnField); + + /** + * To place the RedFootprint target in the middle of the red perimeter wall: + * - First we rotate it 90 around the field's X axis to flip it upright. + * - Second, we rotate it 180 around the field's Z axis so the image is flat against the red perimeter wall + * and facing inwards to the center of the field. + * - Then, we translate it along the negative Y axis to the red perimeter wall. + */ + OpenGLMatrix redFootprintLocationOnField = OpenGLMatrix + .translation(0, -mmFTCFieldWidth, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 180)); + redFootprint.setLocation(redFootprintLocationOnField); + + /** + * To place the FrontCraters target in the middle of the front perimeter wall: + * - First we rotate it 90 around the field's X axis to flip it upright. + * - Second, we rotate it 90 around the field's Z axis so the image is flat against the front wall + * and facing inwards to the center of the field. + * - Then, we translate it along the negative X axis to the front perimeter wall. + */ + OpenGLMatrix frontCratersLocationOnField = OpenGLMatrix + .translation(-mmFTCFieldWidth, 0, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0 , 90)); + frontCraters.setLocation(frontCratersLocationOnField); + + /** + * To place the BackSpace target in the middle of the back perimeter wall: + * - First we rotate it 90 around the field's X axis to flip it upright. + * - Second, we rotate it -90 around the field's Z axis so the image is flat against the back wall + * and facing inwards to the center of the field. + * - Then, we translate it along the X axis to the back perimeter wall. + */ + OpenGLMatrix backSpaceLocationOnField = OpenGLMatrix + .translation(mmFTCFieldWidth, 0, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90)); + backSpace.setLocation(backSpaceLocationOnField); + + /** + * Create a transformation matrix describing where the phone is on the robot. + * + * The coordinate frame for the robot looks the same as the field. + * The robot's "forward" direction is facing out along X axis, with the LEFT side facing out along the Y axis. + * Z is UP on the robot. This equates to a bearing angle of Zero degrees. + * + * The phone starts out lying flat, with the screen facing Up and with the physical top of the phone + * pointing to the LEFT side of the Robot. It's very important when you test this code that the top of the + * camera is pointing to the left side of the robot. The rotation angles don't work if you flip the phone. + * + * If using the rear (High Res) camera: + * We need to rotate the camera around it's long axis to bring the rear camera forward. + * This requires a negative 90 degree rotation on the Y axis + * + * If using the Front (Low Res) camera + * We need to rotate the camera around it's long axis to bring the FRONT camera forward. + * This requires a Positive 90 degree rotation on the Y axis + * + * Next, translate the camera lens to where it is on the robot. + * In this example, it is centered (left to right), but 110 mm forward of the middle of the robot, and 200 mm above ground level. + */ + + final int CAMERA_FORWARD_DISPLACEMENT = 110; // eg: Camera is 110 mm in front of robot center + final int CAMERA_VERTICAL_DISPLACEMENT = 200; // eg: Camera is 200 mm above ground + final int CAMERA_LEFT_DISPLACEMENT = 0; // eg: Camera is ON the robot's center line + + OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix + .translation(CAMERA_FORWARD_DISPLACEMENT, CAMERA_LEFT_DISPLACEMENT, CAMERA_VERTICAL_DISPLACEMENT) + .multiplied(Orientation.getRotationMatrix(EXTRINSIC, YZX, DEGREES, + CAMERA_CHOICE == FRONT ? 90 : -90, 0, 0)); + + /** Let all the trackable listeners know where the phone is. */ + for (VuforiaTrackable trackable : allTrackables) + { + ((VuforiaTrackableDefaultListener)trackable.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); + } + + /** Wait for the game to begin */ + telemetry.addData(">", "Press Play to start tracking"); + telemetry.update(); + waitForStart(); + + /** Start tracking the data sets we care about. */ + targetsRoverRuckus.activate(); + while (opModeIsActive()) { + + // check all the trackable target to see which one (if any) is visible. + targetVisible = false; + for (VuforiaTrackable trackable : allTrackables) { + if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) { + telemetry.addData("Visible Target", trackable.getName()); + targetVisible = true; + + // getUpdatedRobotLocation() will return null if no new information is available since + // the last time that call was made, or if the trackable is not currently visible. + OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation(); + if (robotLocationTransform != null) { + lastLocation = robotLocationTransform; + } + break; + } + } + + // Provide feedback as to where the robot is located (if we know). + if (targetVisible) { + // express position (translation) of robot in inches. + VectorF translation = lastLocation.getTranslation(); + telemetry.addData("Pos (in)", "{X, Y, Z} = %.1f, %.1f, %.1f", + translation.get(0) / mmPerInch, translation.get(1) / mmPerInch, translation.get(2) / mmPerInch); + + // express the rotation of the robot in degrees. + Orientation rotation = Orientation.getOrientation(lastLocation, EXTRINSIC, XYZ, DEGREES); + telemetry.addData("Rot (deg)", "{Roll, Pitch, Heading} = %.0f, %.0f, %.0f", rotation.firstAngle, rotation.secondAngle, rotation.thirdAngle); + } + else { + telemetry.addData("Visible Target", "none"); + } + telemetry.update(); + } + } +} diff --git a/build.gradle b/build.gradle index b20f6398825..2a7d51dfa28 100644 --- a/build.gradle +++ b/build.gradle @@ -9,7 +9,13 @@ buildscript { jcenter() } dependencies { - classpath 'com.android.tools.build:gradle:3.1.3' + classpath 'com.android.tools.build:gradle:3.2.1' + } +} +allprojects { + repositories { + google() + jcenter() } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index b6517bb1d16..087030eb518 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,6 @@ +#Fri Oct 19 15:57:10 PDT 2018 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-4.4-all.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-4.6-all.zip