TeamCode/src/main/java/org/firstinspires/ftc/teamcode/examples/CameraNavigationAuto.java
@Override
    public void runOpMode() throws InterruptedException {
        telemetry.addData("STATUS", "Initializing");
        telemetry.update();

        int id = DataBinder.getInstance().integers().get(DataBinder.CAMERA_VIEW);
        VuforiaLocalizer.Parameters vuforiaParams = new VuforiaLocalizer.Parameters(id);
        try {
            vuforiaParams.vuforiaLicenseKey = Files.toString(new File("/sdcard/robot/vuforia.key"), Charset.forName("utf-8"));
        } catch (IOException e) {
            e.printStackTrace();
        }
        vuforiaParams.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
        VuforiaLocalizer vuforia = ClassFactory.createVuforiaLocalizer(vuforiaParams);
        VuforiaTrackables vuforiaTrackables = vuforia.loadTrackablesFromAsset("FTC_2016-17");

        // Set names of trackables
        VUF_WHEEL_TARGET = 0;
        vuforiaTrackables.get(VUF_WHEEL_TARGET).setName("Wheels");
        vuforiaTrackables.get(VUF_TOOL_TARGET).setName("Tools");


        VUF_LEGOS_TARGET = 2;
        vuforiaTrackables.get(VUF_LEGOS_TARGET).setName("Legos");

        VUF_GEARS_TARGET = 3;
        vuforiaTrackables.get(VUF_GEARS_TARGET).setName("Gears");


        // Location on the field
        /*
        * Blue Audience Wall  |
        *                     |
        *              Wheels |
        *                     |
        *               Legos |
        *                     |
        *                     |_____________
        *                        |    |
        *                    Tools    Gears
        *
        *                    Red Audience Wall
        *
        *  Each "_" is equal to 12", whereas each "|" is equal to 24." However the edges are equal to
        *  their respective figures minus 1" (edge "_" = 11"; edge "|' = 23")
        */

        float mmPerInch = 25.4f;
        float mmBotWidth = 18 * mmPerInch;            // ... or whatever is right for your robot
        float mmFTCFieldWidth = (12 * 12 - 2) * mmPerInch;   // the FTC field is ~11'10" center-to-center of the glass panels

        final OpenGLMatrix gearsFieldLocation = OpenGLMatrix
                /* Then we translate the target off to the RED WALL. Our translation here
                is a negative translation in X.*/
                .translation(-mmFTCFieldWidth / 2, -12 * mmPerInch, 0)
                .multiplied(Orientation.getRotationMatrix(
                        /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */
                        AxesReference.EXTRINSIC, AxesOrder.XZX,
                        AngleUnit.DEGREES, 90, 90, 0));
        vuforiaTrackables.get(3).setLocation(gearsFieldLocation);
        final OpenGLMatrix toolsFieldLocation = OpenGLMatrix
                /* Then we translate the target off to the RED WALL. Our translation here
                is a negative translation in X.*/
                .translation(-mmFTCFieldWidth / 2, (24 + 12) * mmPerInch, 0)
                .multiplied(Orientation.getRotationMatrix(
                        /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */
                        AxesReference.EXTRINSIC, AxesOrder.XZX,
                        AngleUnit.DEGREES, 90, 90, 0));
        vuforiaTrackables.get(1).setLocation(toolsFieldLocation);
        final OpenGLMatrix wheelsLocationOnField = OpenGLMatrix
                /* Then we translate the target off to the Blue Audience wall.
                Our translation here is a positive translation in Y.*/
                .translation(12 * mmPerInch, mmFTCFieldWidth / 2, 0)
                .multiplied(Orientation.getRotationMatrix(
                        /* First, in the fixed (field) coordinate system, we rotate 90deg in X */
                        AxesReference.EXTRINSIC, AxesOrder.XZX,
                        AngleUnit.DEGREES, 90, 0, 0));
        vuforiaTrackables.get(0).setLocation(wheelsLocationOnField);
        final OpenGLMatrix legosLocationOnField = OpenGLMatrix
                /* Then we translate the target off to the Blue Audience wall.
                Our translation here is a positive translation in Y.*/
                .translation((-24 - 12) * mmPerInch, mmFTCFieldWidth / 2, 0)
                .multiplied(Orientation.getRotationMatrix(
                        /* First, in the fixed (field) coordinate system, we rotate 90deg in X */
                        AxesReference.EXTRINSIC, AxesOrder.XZX,
                        AngleUnit.DEGREES, 90, 0, 0));
        vuforiaTrackables.get(2).setLocation(legosLocationOnField);

        // Assume the phone is at the center of the robot
        final OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix
                .translation(mmBotWidth / 2, mmBotWidth / 2, mmBotWidth / 2)
                .multiplied(Orientation.getRotationMatrix(
                        AxesReference.EXTRINSIC, AxesOrder.YZY,
                        AngleUnit.DEGREES, -90, 0, 0));

        for (VuforiaTrackable trackable : vuforiaTrackables) {
            ((VuforiaTrackableDefaultListener) trackable.getListener()).setPhoneInformation(phoneLocationOnRobot, VuforiaLocalizer.CameraDirection.BACK);
        }

        idle();
        final SensorManager systemService = (SensorManager) hardwareMap.appContext.getSystemService(Context.SENSOR_SERVICE);
        final Sensor accelerometer = systemService.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        AccelerometerListener listener1 = new AccelerometerListener();
        systemService.registerListener(listener1, accelerometer, SensorManager.SENSOR_DELAY_NORMAL);
        Thread aDataLoop = new Thread(new Runnable() {
            @Override
            public void run() {
                listener1.processData();
            }
        });
        aDataLoop.start();
        try {
            telemetry.addData("STATUS", "Ready");
            telemetry.update();
            waitForStart();
            telemetry.addData("STATUS", "Running");
        } catch (RuntimeException ex) {
            // ignore bug
        }

        vuforiaTrackables.activate();
        while (opModeIsActive()) {
            for (VuforiaTrackable trackable : vuforiaTrackables) {
                VuforiaTrackableDefaultListener listener = (VuforiaTrackableDefaultListener) trackable.getListener();
                telemetry.addData(trackable.getName(), listener.isVisible());
                OpenGLMatrix updatedRobotLocation = listener.getUpdatedRobotLocation();
                if (updatedRobotLocation != null)
                    lastLocation = updatedRobotLocation;

            }
            if (lastLocation != null)
                telemetry.addData("I_LOCATION", lastLocation.formatAsTransform());
            Position currentPosition = listener1.getCurrentPosition();
            telemetry.addData("A_LOCATION", "X: " + currentPosition.x + " Y: " + currentPosition.y + " Z: " + currentPosition.z);
            telemetry.addData("ACCEL_DATA", listener1.getDataDisplay());
            telemetry.addData("ACCEL_DATA_READY", listener1.isInitialized());
            telemetry.update();
        }
        aDataLoop.interrupt();
        vuforiaTrackables.deactivate();


    }
/**
     * Override this method and place your code here.
     * <p>
     * Please do not swallow the InterruptedException, as it is used in cases
     * where the op mode needs to be terminated early.
     *
     * @throws InterruptedException
     */
Override this method and place your code here.

Please do not swallow the InterruptedException, as it is used in cases
where the op mode needs to be terminated early.


Override this method and place your code here . Please do not swallow the InterruptedException as it is used in cases where the op mode needs to be terminated early .
@Override
    public void runOpMode() throws InterruptedException {
        telemetry.addData("STATUS", "Initializing");
        telemetry.update();
        int id = DataBinder.getInstance().integers().get(DataBinder.CAMERA_VIEW);
        VuforiaLocalizer.Parameters vuforiaParams = new VuforiaLocalizer.Parameters(id);
        try {
            vuforiaParams.vuforiaLicenseKey = Files.toString(new File("/sdcard/robot/vuforia.key"), Charset.forName("utf-8"));
        } catch (IOException e) {
            e.printStackTrace();
        }
        vuforiaParams.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
        VuforiaLocalizer vuforia = ClassFactory.createVuforiaLocalizer(vuforiaParams);
        VuforiaTrackables vuforiaTrackables = vuforia.loadTrackablesFromAsset("FTC_2016-17");
        VUF_WHEEL_TARGET = 0;
        vuforiaTrackables.get(VUF_WHEEL_TARGET).setName("Wheels");
        vuforiaTrackables.get(VUF_TOOL_TARGET).setName("Tools");
        VUF_LEGOS_TARGET = 2;
        vuforiaTrackables.get(VUF_LEGOS_TARGET).setName("Legos");
        VUF_GEARS_TARGET = 3;
        vuforiaTrackables.get(VUF_GEARS_TARGET).setName("Gears");
        /*
        * Blue Audience Wall  |
        *                     |
        *              Wheels |
        *                     |
        *               Legos |
        *                     |
        *                     |_____________
        *                        |    |
        *                    Tools    Gears
        *
        *                    Red Audience Wall
        *
        *  Each "_" is equal to 12", whereas each "|" is equal to 24." However the edges are equal to
        *  their respective figures minus 1" (edge "_" = 11"; edge "|' = 23")
        */
        float mmPerInch = 25.4f;
        float mmBotWidth = 18 * mmPerInch;            
        float mmFTCFieldWidth = (12 * 12 - 2) * mmPerInch;   
        final OpenGLMatrix gearsFieldLocation = OpenGLMatrix
                /* Then we translate the target off to the RED WALL. Our translation here
                is a negative translation in X.*/
                .translation(-mmFTCFieldWidth / 2, -12 * mmPerInch, 0)
                .multiplied(Orientation.getRotationMatrix(
                        /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */
                        AxesReference.EXTRINSIC, AxesOrder.XZX,
                        AngleUnit.DEGREES, 90, 90, 0));
        vuforiaTrackables.get(3).setLocation(gearsFieldLocation);
        final OpenGLMatrix toolsFieldLocation = OpenGLMatrix
                /* Then we translate the target off to the RED WALL. Our translation here
                is a negative translation in X.*/
                .translation(-mmFTCFieldWidth / 2, (24 + 12) * mmPerInch, 0)
                .multiplied(Orientation.getRotationMatrix(
                        /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */
                        AxesReference.EXTRINSIC, AxesOrder.XZX,
                        AngleUnit.DEGREES, 90, 90, 0));
        vuforiaTrackables.get(1).setLocation(toolsFieldLocation);
        final OpenGLMatrix wheelsLocationOnField = OpenGLMatrix
                /* Then we translate the target off to the Blue Audience wall.
                Our translation here is a positive translation in Y.*/
                .translation(12 * mmPerInch, mmFTCFieldWidth / 2, 0)
                .multiplied(Orientation.getRotationMatrix(
                        /* First, in the fixed (field) coordinate system, we rotate 90deg in X */
                        AxesReference.EXTRINSIC, AxesOrder.XZX,
                        AngleUnit.DEGREES, 90, 0, 0));
        vuforiaTrackables.get(0).setLocation(wheelsLocationOnField);
        final OpenGLMatrix legosLocationOnField = OpenGLMatrix
                /* Then we translate the target off to the Blue Audience wall.
                Our translation here is a positive translation in Y.*/
                .translation((-24 - 12) * mmPerInch, mmFTCFieldWidth / 2, 0)
                .multiplied(Orientation.getRotationMatrix(
                        /* First, in the fixed (field) coordinate system, we rotate 90deg in X */
                        AxesReference.EXTRINSIC, AxesOrder.XZX,
                        AngleUnit.DEGREES, 90, 0, 0));
        vuforiaTrackables.get(2).setLocation(legosLocationOnField);
        final OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix
                .translation(mmBotWidth / 2, mmBotWidth / 2, mmBotWidth / 2)
                .multiplied(Orientation.getRotationMatrix(
                        AxesReference.EXTRINSIC, AxesOrder.YZY,
                        AngleUnit.DEGREES, -90, 0, 0));
        for (VuforiaTrackable trackable : vuforiaTrackables) {
            ((VuforiaTrackableDefaultListener) trackable.getListener()).setPhoneInformation(phoneLocationOnRobot, VuforiaLocalizer.CameraDirection.BACK);
        }
        idle();
        final SensorManager systemService = (SensorManager) hardwareMap.appContext.getSystemService(Context.SENSOR_SERVICE);
        final Sensor accelerometer = systemService.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        AccelerometerListener listener1 = new AccelerometerListener();
        systemService.registerListener(listener1, accelerometer, SensorManager.SENSOR_DELAY_NORMAL);
        Thread aDataLoop = new Thread(new Runnable() {
            @Override
            public void run() {
                listener1.processData();
            }
        });
        aDataLoop.start();
        try {
            telemetry.addData("STATUS", "Ready");
            telemetry.update();
            waitForStart();
            telemetry.addData("STATUS", "Running");
        } catch (RuntimeException ex) {
        }
        vuforiaTrackables.activate();
        while (opModeIsActive()) {
            for (VuforiaTrackable trackable : vuforiaTrackables) {
                VuforiaTrackableDefaultListener listener = (VuforiaTrackableDefaultListener) trackable.getListener();
                telemetry.addData(trackable.getName(), listener.isVisible());
                OpenGLMatrix updatedRobotLocation = listener.getUpdatedRobotLocation();
                if (updatedRobotLocation != null)
                    lastLocation = updatedRobotLocation;
            }
            if (lastLocation != null)
                telemetry.addData("I_LOCATION", lastLocation.formatAsTransform());
            Position currentPosition = listener1.getCurrentPosition();
            telemetry.addData("A_LOCATION", "X: " + currentPosition.x + " Y: " + currentPosition.y + " Z: " + currentPosition.z);
            telemetry.addData("ACCEL_DATA", listener1.getDataDisplay());
            telemetry.addData("ACCEL_DATA_READY", listener1.isInitialized());
            telemetry.update();
        }
        aDataLoop.interrupt();
        vuforiaTrackables.deactivate();
    }
@ Override public void runOpMode ( ) throws InterruptedException { telemetry . addData ( "STATUS" , "Initializing" ) ; telemetry . update ( ) ; int id = DataBinder . getInstance ( ) . integers ( ) . get ( DataBinder . CAMERA_VIEW ) ; VuforiaLocalizer . Parameters vuforiaParams = new VuforiaLocalizer . Parameters ( id ) ; try { vuforiaParams . vuforiaLicenseKey = Files . toString ( new File ( "/sdcard/robot/vuforia.key" ) , Charset . forName ( "utf-8" ) ) ; } catch ( IOException e ) { e . printStackTrace ( ) ; } vuforiaParams . cameraDirection = VuforiaLocalizer . CameraDirection . BACK ; VuforiaLocalizer vuforia = ClassFactory . createVuforiaLocalizer ( vuforiaParams ) ; VuforiaTrackables vuforiaTrackables = vuforia . loadTrackablesFromAsset ( "FTC_2016-17" ) ; VUF_WHEEL_TARGET = 0 ; vuforiaTrackables . get ( VUF_WHEEL_TARGET ) . setName ( "Wheels" ) ; vuforiaTrackables . get ( VUF_TOOL_TARGET ) . setName ( "Tools" ) ; VUF_LEGOS_TARGET = 2 ; vuforiaTrackables . get ( VUF_LEGOS_TARGET ) . setName ( "Legos" ) ; VUF_GEARS_TARGET = 3 ; vuforiaTrackables . get ( VUF_GEARS_TARGET ) . setName ( "Gears" ) ; /*
        * Blue Audience Wall  |
        *                     |
        *              Wheels |
        *                     |
        *               Legos |
        *                     |
        *                     |_____________
        *                        |    |
        *                    Tools    Gears
        *
        *                    Red Audience Wall
        *
        *  Each "_" is equal to 12", whereas each "|" is equal to 24." However the edges are equal to
        *  their respective figures minus 1" (edge "_" = 11"; edge "|' = 23")
        */ float mmPerInch = 25.4f ; float mmBotWidth = 18 * mmPerInch ; float mmFTCFieldWidth = ( 12 * 12 - 2 ) * mmPerInch ; final OpenGLMatrix gearsFieldLocation = OpenGLMatrix /* Then we translate the target off to the RED WALL. Our translation here
                is a negative translation in X.*/ . translation ( - mmFTCFieldWidth / 2 , - 12 * mmPerInch , 0 ) . multiplied ( Orientation . getRotationMatrix ( /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ AxesReference . EXTRINSIC , AxesOrder . XZX , AngleUnit . DEGREES , 90 , 90 , 0 ) ) ; vuforiaTrackables . get ( 3 ) . setLocation ( gearsFieldLocation ) ; final OpenGLMatrix toolsFieldLocation = OpenGLMatrix /* Then we translate the target off to the RED WALL. Our translation here
                is a negative translation in X.*/ . translation ( - mmFTCFieldWidth / 2 , ( 24 + 12 ) * mmPerInch , 0 ) . multiplied ( Orientation . getRotationMatrix ( /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ AxesReference . EXTRINSIC , AxesOrder . XZX , AngleUnit . DEGREES , 90 , 90 , 0 ) ) ; vuforiaTrackables . get ( 1 ) . setLocation ( toolsFieldLocation ) ; final OpenGLMatrix wheelsLocationOnField = OpenGLMatrix /* Then we translate the target off to the Blue Audience wall.
                Our translation here is a positive translation in Y.*/ . translation ( 12 * mmPerInch , mmFTCFieldWidth / 2 , 0 ) . multiplied ( Orientation . getRotationMatrix ( /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ AxesReference . EXTRINSIC , AxesOrder . XZX , AngleUnit . DEGREES , 90 , 0 , 0 ) ) ; vuforiaTrackables . get ( 0 ) . setLocation ( wheelsLocationOnField ) ; final OpenGLMatrix legosLocationOnField = OpenGLMatrix /* Then we translate the target off to the Blue Audience wall.
                Our translation here is a positive translation in Y.*/ . translation ( ( - 24 - 12 ) * mmPerInch , mmFTCFieldWidth / 2 , 0 ) . multiplied ( Orientation . getRotationMatrix ( /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ AxesReference . EXTRINSIC , AxesOrder . XZX , AngleUnit . DEGREES , 90 , 0 , 0 ) ) ; vuforiaTrackables . get ( 2 ) . setLocation ( legosLocationOnField ) ; final OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix . translation ( mmBotWidth / 2 , mmBotWidth / 2 , mmBotWidth / 2 ) . multiplied ( Orientation . getRotationMatrix ( AxesReference . EXTRINSIC , AxesOrder . YZY , AngleUnit . DEGREES , - 90 , 0 , 0 ) ) ; for ( VuforiaTrackable trackable : vuforiaTrackables ) { ( ( VuforiaTrackableDefaultListener ) trackable . getListener ( ) ) . setPhoneInformation ( phoneLocationOnRobot , VuforiaLocalizer . CameraDirection . BACK ) ; } idle ( ) ; final SensorManager systemService = ( SensorManager ) hardwareMap . appContext . getSystemService ( Context . SENSOR_SERVICE ) ; final Sensor accelerometer = systemService . getDefaultSensor ( Sensor . TYPE_ACCELEROMETER ) ; AccelerometerListener listener1 = new AccelerometerListener ( ) ; systemService . registerListener ( listener1 , accelerometer , SensorManager . SENSOR_DELAY_NORMAL ) ; Thread aDataLoop = new Thread ( new Runnable ( ) { @ Override public void run ( ) { listener1 . processData ( ) ; } } ) ; aDataLoop . start ( ) ; try { telemetry . addData ( "STATUS" , "Ready" ) ; telemetry . update ( ) ; waitForStart ( ) ; telemetry . addData ( "STATUS" , "Running" ) ; } catch ( RuntimeException ex ) { } vuforiaTrackables . activate ( ) ; while ( opModeIsActive ( ) ) { for ( VuforiaTrackable trackable : vuforiaTrackables ) { VuforiaTrackableDefaultListener listener = ( VuforiaTrackableDefaultListener ) trackable . getListener ( ) ; telemetry . addData ( trackable . getName ( ) , listener . isVisible ( ) ) ; OpenGLMatrix updatedRobotLocation = listener . getUpdatedRobotLocation ( ) ; if ( updatedRobotLocation != null ) lastLocation = updatedRobotLocation ; } if ( lastLocation != null ) telemetry . addData ( "I_LOCATION" , lastLocation . formatAsTransform ( ) ) ; Position currentPosition = listener1 . getCurrentPosition ( ) ; telemetry . addData ( "A_LOCATION" , "X: " + currentPosition . x + " Y: " + currentPosition . y + " Z: " + currentPosition . z ) ; telemetry . addData ( "ACCEL_DATA" , listener1 . getDataDisplay ( ) ) ; telemetry . addData ( "ACCEL_DATA_READY" , listener1 . isInitialized ( ) ) ; telemetry . update ( ) ; } aDataLoop . interrupt ( ) ; vuforiaTrackables . deactivate ( ) ; }
Override this method and place your code here.
Override this method and place your code here .
// Set names of trackables // Location on the field // ... or whatever is right for your robot // the FTC field is ~11'10" center-to-center of the glass panels // Assume the phone is at the center of the robot // ignore bug