private static final VuforiaLocalizer.CameraDirection CAMERA_CHOICE = FRONT;

private static final boolean PHONE_IS_PORTRAIT = true;

private static final String VUFORIA_KEY =

"AZrcPbL/////AAABmQ7qhHnAOkQDjFldRi+1gXdnIol7PdUHJo1OJXAy+0C23VNo6+UBdsRdJFEpeeHMUjDZgvflIkS92jUqHhtdckNsnbBDGUBjVC5NRweYFvtc9pKmNGwgQLYvKSZwdwBKWhx/i4rYJgWItX0JEcv9lsQ6VzJChbO3VwCyxnwRylI/HkQk21nYDhHaURDE0ogSr8GqDYnoE3F9h5fw/ll0wr5rWSgyxfcsEWg3YvBigLVyzhO/zXwA+4Og98pGaOW9mhTD78B1W0P4NUGD6ywdGP7j9uDepld/wDueVykgqHR8xcZ6VXc7DlkKOHgk8Zr6HqMUzDDsMX457wwFRbDWkYJiIyzXuO7jlpZNQ+mRqvoF";

// We will define some constants and conversions here

private static final float mmPerInch = 25.4f;

private static final float mmTargetHeight = (6) * mmPerInch; // the height of the center of the target image above the floor

private static final float stoneZ = 2.00f * mmPerInch;

// Constants for the center support targets

private static final float bridgeZ = 6.42f * mmPerInch;

private static final float bridgeY = 23 * mmPerInch;

private static final float bridgeX = 5.18f * mmPerInch;

private static final float bridgeRotY = 59; // Units are degrees

private static final float bridgeRotZ = 180;

// Constants for perimeter targets

private static final float halfField = 72 * mmPerInch;

private static final float quadField = 36 * mmPerInch;

private OpenGLMatrix lastLocation = null;

private VuforiaLocalizer vuforia = null;

private boolean targetVisible = false;

private float phoneXRotate = 0;

private float phoneYRotate = 0;

private float phoneZRotate = 0;

// Declare OpMode members

private ElapsedTime runtime = new ElapsedTime();

private DcMotor frontLeftDrive = null;

private DcMotor frontRightDrive = null;

private DcMotor backLeftDrive = null;

private DcMotor backRightDrive = null;

private double motorWeight = 0.65789473684;

private int tetrixTickCount;

private Servo servo;

double servoPower = 0.0;

int cameraMonitorViewId = 0;

@Override

public void runOpMode() {

telemetry.addData("Status", "Initialized");

telemetry.update();

// Initialize the hardware variables. Note that the strings used here as parameters

// to 'get' must correspond to the names assigned during the robot configuration

// step (using the FTC Robot Controller app on the phone).

frontLeftDrive = hardwareMap.get(DcMotor.class, "front_left_drive");

frontRightDrive = hardwareMap.get(DcMotor.class, "front_right_drive");

backLeftDrive = hardwareMap.get(DcMotor.class, "back_left_drive");

backRightDrive = hardwareMap.get(DcMotor.class, "back_right_drive");

frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

//VUFORIA STUFF

cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());

VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);

parameters.vuforiaLicenseKey = VUFORIA_KEY;

parameters.cameraDirection = CAMERA_CHOICE;

vuforia = ClassFactory.getInstance().createVuforia(parameters);

VuforiaTrackables targetsSkyStone = this.vuforia.loadTrackablesFromAsset("Skystone");

VuforiaTrackable stoneTarget = targetsSkyStone.get(0);

stoneTarget.setName("Stone Target");

List<VuforiaTrackable> allTrackables = new ArrayList<VuforiaTrackable>();

allTrackables.addAll(targetsSkyStone);

stoneTarget.setLocation(OpenGLMatrix

.translation(0, 0, stoneZ)

.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90)));

if (CAMERA_CHOICE == BACK) {

phoneYRotate = -90;

} else {

phoneYRotate = 90;

}

// Rotate the phone vertical about the X axis if it's in portrait mode

if (PHONE_IS_PORTRAIT) {

phoneXRotate = 90 ;

}

final float CAMERA_FORWARD_DISPLACEMENT = 4.0f * mmPerInch; // eg: Camera is 4 Inches in front of robot center

final float CAMERA_VERTICAL_DISPLACEMENT = 8.0f * mmPerInch; // eg: Camera is 8 Inches above ground

final float CAMERA_LEFT_DISPLACEMENT = 0; // eg: Camera is ON the robot's center line

OpenGLMatrix robotFromCamera = OpenGLMatrix

.translation(CAMERA_FORWARD_DISPLACEMENT, CAMERA_LEFT_DISPLACEMENT, CAMERA_VERTICAL_DISPLACEMENT)

.multiplied(Orientation.getRotationMatrix(EXTRINSIC, YZX, DEGREES, phoneYRotate, phoneZRotate, phoneXRotate));

/** Let all the trackable listeners know where the phone is. **/

for (VuforiaTrackable trackable : allTrackables) {

((VuforiaTrackableDefaultListener) trackable.getListener()).setPhoneInformation(robotFromCamera, parameters.cameraDirection);

}

//VUFORIA STUFF FINISHED

servo = hardwareMap.servo.get("servo");

// Most robots need the motor on one side to be reversed to drive forward

// Reverse the motor that runs backwards when connected directly to the battery

frontLeftDrive.setDirection(DcMotor.Direction.FORWARD);

frontRightDrive.setDirection(DcMotor.Direction.REVERSE);

backLeftDrive.setDirection(DcMotor.Direction.FORWARD);

backRightDrive.setDirection(DcMotor.Direction.REVERSE);

servo.setPosition(servoPower);

// Wait for the game to start (driver presses PLAY)

waitForStart();

runtime.reset();

targetsSkyStone.activate();

moveStrafe(1000, -1);

while (!isStopRequested()) {

// check all the trackable targets to see which one (if any) is visible.

targetVisible = false;

moveForward(0, -1);

for (VuforiaTrackable trackable : allTrackables) {

if (((VuforiaTrackableDefaultListener) trackable.getListener()).isVisible()) {

telemetry.addData("Visible Target", trackable.getName());

targetVisible = true;

if(trackable.getName() == "Stone Target"){

servo.setPosition(0.5);

telemetry.addData("STONE FOUND", trackable.getName());

moveForward(1000, 0);

}

// 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 mm.

VectorF translation = lastLocation.getTranslation();

float targetX = translation.get(0)/mmPerInch;

float targetY = translation.get(1)/mmPerInch;

float targetZ = translation.get(2)/mmPerInch;

telemetry.addData("Pos (in)", "{X, Y, Z} = %.1f, %.1f, %.1f",

translation.get(0) / mmPerInch, translation.get(1) / mmPerInch, translation.get(2) / mmPerInch);

telemetry.addData("X", targetX);

// 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();

}

moveStrafe(1000, 1);

moveForward(3000, -1);

}

public void moveForward(long time, int direction){

frontLeftDrive.setPower(direction);

backLeftDrive.setPower(direction);

frontRightDrive.setPower(motorWeight*direction);

backRightDrive.setPower(direction);

sleep(time);

setPower(0);

}

public void moveStrafe(long time, int direction){

frontLeftDrive.setPower(direction);

backLeftDrive.setPower(-direction);

frontRightDrive.setPower(-direction);

backRightDrive.setPower(direction);

sleep(time);

setPower(0);

}

public void moveTurn(long time, int direction){

long start = System.currentTimeMillis();

while (start<time) {

start = System.currentTimeMillis();

frontLeftDrive.setPower(motorWeight*direction);

backLeftDrive.setPower(direction);

frontRightDrive.setPower(motorWeight*-direction);

backRightDrive.setPower(-direction);

}

}

public void setPower(double power){

frontLeftDrive.setPower(motorWeight*power);

backLeftDrive.setPower(power);

frontRightDrive.setPower(motorWeight*power);

backRightDrive.setPower(power);

}

public void encoderDrive(int distance, double power){

frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

frontLeftDrive.setTargetPosition(distance);

frontRightDrive.setTargetPosition(distance);

backLeftDrive.setTargetPosition(distance);

backRightDrive.setTargetPosition(distance);

setPower(power);

frontLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

frontRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

backLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

backRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

while (frontLeftDrive.isBusy() || backLeftDrive.isBusy() || frontRightDrive.isBusy() || backRightDrive.isBusy()){

telemetry.addData("Driving", distance);

telemetry.update();

}

setPower(0);

telemetry.addData("Finished Driving:", distance);

telemetry.update();

}

}