import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
/**
 * This method sets the properties of the specified target.
 *
 * @param index specifies the target index in the XML file.
 * @param name specifies the target name.
 * @param locationOnField specifies the target location on the field, can be null if no robot tracking.
 * @param phoneLocationOnRobot specifies the phone location on the robot, can be null if no robot tracking.
 */
public void setTargetInfo(int index, String name, OpenGLMatrix locationOnField, OpenGLMatrix phoneLocationOnRobot)
{
    if (targetList != null)
    {
        VuforiaTrackable target = targetList.get(index);
        target.setName(name);

        if (locationOnField != null)
        {
            target.setLocation(locationOnField);
        }

        if (phoneLocationOnRobot != null)
        {
            ((VuforiaTrackableDefaultListener) target.getListener()).setPhoneInformation(
                    phoneLocationOnRobot, cameraDir);
        }
    }
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
public VectorF getVuMarkPosition()
{
    VectorF targetPos = null;
    VuforiaTrackable target = vuforia.getTarget(0);
    RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(target);

    if (vuforia.isTargetVisible(target) && vuMark != RelicRecoveryVuMark.UNKNOWN)
    {
        OpenGLMatrix pose = vuforia.getTargetPose(target);
        if (pose != null)
        {
            targetPos = pose.getTranslation();
            robot.tracer.traceInfo("TargetPos", "%s: x=%6.2f, y=%6.2f, z=%6.2f",
                                   vuMark.toString(),
                                   targetPos.get(0)/RobotInfo.MM_PER_INCH,
                                   targetPos.get(1)/RobotInfo.MM_PER_INCH,
                                   -targetPos.get(2)/RobotInfo.MM_PER_INCH);
        }
    }

    return targetPos;
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
public Orientation getVuMarkOrientation()
{
    Orientation targetAngle = null;
    VuforiaTrackable target = vuforia.getTarget(0);
    RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(target);

    if (vuforia.isTargetVisible(target) && vuMark != RelicRecoveryVuMark.UNKNOWN)
    {
        OpenGLMatrix pose = vuforia.getTargetPose(target);
        if (pose != null)
        {
            targetAngle = Orientation.getOrientation(
                    pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
            robot.tracer.traceInfo("TargetRot", "%s: xRot=%6.2f, yRot=%6.2f, zRot=%6.2f",
                    vuMark.toString(),
                    targetAngle.firstAngle, targetAngle.secondAngle, targetAngle.thirdAngle);
        }
    }

    return targetAngle;
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
public void updateLocation(){
    OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicVuMark.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
        tX = trans.get(0);
        tY = trans.get(1);
        tZ = trans.get(2);

        // Extract the rotational components of the target relative to the robot
        rX = rot.firstAngle;
        rY = rot.secondAngle;
        rZ = rot.thirdAngle;
        //Z is forward-backward
        //x is sideways
    }
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
private VectorF getVuMarkPosition()
{
    VectorF targetPos = null;
    VuforiaTrackable target = vuforia.getTarget(0);
    RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(target);

    if (vuforia.isTargetVisible(target) && vuMark != RelicRecoveryVuMark.UNKNOWN)
    {
        OpenGLMatrix pose = vuforia.getTargetPose(target);
        if (pose != null)
        {
            targetPos = pose.getTranslation();
        }
    }

    return targetPos;
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
private Orientation getVuMarkOrientation()
{
    Orientation targetAngle = null;
    VuforiaTrackable target = vuforia.getTarget(0);
    RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(target);

    if (vuforia.isTargetVisible(target) && vuMark != RelicRecoveryVuMark.UNKNOWN)
    {
        OpenGLMatrix pose = vuforia.getTargetPose(target);
        if (pose != null)
        {
            targetAngle = Orientation.getOrientation(
                    pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
        }
    }

    return targetAngle;
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
/**
 * This method sets tracking info for the targets described in the given target array.
 *
 * @param targets specifies the array of targets to set tracking info.
 * @param phoneLocationOnRobot specifies the location marix of the phone on the robot.
 */
public void setTargets(Target[] targets, OpenGLMatrix phoneLocationOnRobot)
{
    for (int i = 0; i < targets.length; i++)
    {
        OpenGLMatrix targetLocationOnField =
                phoneLocationOnRobot == null?
                        null:
                        locationMatrix(
                                targets[i].rotateX, targets[i].rotateY, targets[i].rotateZ,
                                targets[i].translateX, targets[i].translateY, targets[i].translateZ);
        setTargetInfo(i, targets[i].name, targetLocationOnField, phoneLocationOnRobot);
    }
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
public void runOpMode() throws InterruptedException
{
	setupVuforia();
	
	lastKnownLocation = createMatrix(0,0,0,0,0,0);
	
	waitForStart();
	
	visionTargets.activate();
	while(opModeIsActive())
	{
		OpenGLMatrix latestLocation = listener.getUpdatedRobotLocation();
		vuMark = RelicRecoveryVuMark.from(relicVuMark);
	
		if(latestLocation !=null)
		{lastKnownLocation = latestLocation;}
			
		float[] coordinates = lastKnownLocation.getTranslation().getData();
		
		robotX = coordinates[0];
		robotY = coordinates[1];
		robotAngle = Orientation.getOrientation(lastKnownLocation, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
           RelicRecoveryVuMark key = vuMark;
		if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
               telemetry.addData("Navi", "%s visible", vuMark);
           }else{
               telemetry.addData("Navi", "not visible");
               telemetry.addData("Navi Sees:", vuMark);
           }
		telemetry.addData("Last Known Location", formatMatrix(lastKnownLocation));
		telemetry.addData("key",key.toString());
		telemetry.update();
	}
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
public void runOpMode() throws InterruptedException
{
    setupVuforia();

    lastKnownLocation = createMatrix(0,0,0,0,0,0);

    waitForStart();

    visionTargets.activate();
    while(opModeIsActive())
    {
        OpenGLMatrix latestLocation = listener.getUpdatedRobotLocation();
        vuMark = RelicRecoveryVuMark.from(relicVuMark);

        if(latestLocation !=null)
        {lastKnownLocation = latestLocation;}

        float[] coordinates = lastKnownLocation.getTranslation().getData();

        robotX = coordinates[0];
        robotY = coordinates[1];
        robotAngle = Orientation.getOrientation(lastKnownLocation, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
        if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
            key = vuMark;
        }
    }
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
public static int getBeaconConfig(Image img, VuforiaTrackableDefaultListener beacon, CameraCalibration camCal) {

        OpenGLMatrix pose = beacon.getRawPose();

        if (pose != null && img != null && img.getPixels() != null) {

            Matrix34F rawPose = new Matrix34F();
            float[] poseData = Arrays.copyOfRange(pose.transposed().getData(), 0, 12);

            rawPose.setData(poseData);

            float[][] corners = new float[4][2];

            corners[0] = Tool.projectPoint(camCal, rawPose, new Vec3F(-127, 276, 0)).getData(); //upper left of beacon
            corners[1] = Tool.projectPoint(camCal, rawPose, new Vec3F(127, 276, 0)).getData(); //upper right of beacon
            corners[2] = Tool.projectPoint(camCal, rawPose, new Vec3F(127, -92, 0)).getData(); //lower right of beacon
            corners[3] = Tool.projectPoint(camCal, rawPose, new Vec3F(-127, -92, 0)).getData(); //lower left of beacon

            //getting camera image...
            Bitmap bm = Bitmap.createBitmap(img.getWidth(), img.getHeight(), Bitmap.Config.RGB_565);
            bm.copyPixelsFromBuffer(img.getPixels());

            //turning the corner pixel coordinates into a proper bounding box
            Mat crop = bitmapToMat(bm, CvType.CV_8UC3);
            float x = Math.min(Math.min(corners[1][0], corners[3][0]), Math.min(corners[0][0], corners[2][0]));
            float y = Math.min(Math.min(corners[1][1], corners[3][1]), Math.min(corners[0][1], corners[2][1]));
            float width = Math.max(Math.abs(corners[0][0] - corners[2][0]), Math.abs(corners[1][0] - corners[3][0]));
            float height = Math.max(Math.abs(corners[0][1] - corners[2][1]), Math.abs(corners[1][1] - corners[3][1]));


            //make sure our bounding box doesn't go outside of the image
            //OpenCV doesn't like that...
            x = Math.max(x, 0);
            y = Math.max(y, 0);
            width = (x + width > crop.cols())? crop.cols() - x : width;
            height = (y + height > crop.rows())? crop.rows() - y : height;

            //cropping bounding box out of camera image
            final Mat cropped = new Mat(crop, new Rect((int) x, (int) y, (int) width, (int) height));

            //filtering out non-beacon-blue colours in HSV colour space
            Imgproc.cvtColor(cropped, cropped, Imgproc.COLOR_RGB2HSV_FULL);

            //get filtered mask
            //if pixel is within acceptable blue-beacon-colour range, it's changed to white.
            //Otherwise, it's turned to black
            Mat mask = new Mat();

            Core.inRange(cropped, BEACON_BLUE_LOW, BEACON_BLUE_HIGH, mask);
            Moments mmnts = Imgproc.moments(mask, true);

            //calculating centroid of the resulting binary mask via image moments
            Log.i("CentroidX", "" + ((mmnts.get_m10() / mmnts.get_m00())));
            Log.i("CentroidY", "" + ((mmnts.get_m01() / mmnts.get_m00())));

            //checking if blue either takes up the majority of the image (which means the beacon is all blue)
            //or if there's barely any blue in the image (which means the beacon is all red or off)
//            if (mmnts.get_m00() / mask.total() > 0.8) {
//                return VortexUtils.BEACON_ALL_BLUE;
//            } else if (mmnts.get_m00() / mask.total() < 0.1) {
//                return VortexUtils.BEACON_NO_BLUE;
//            }//elseif

            //Note: for some reason, we end up with a image that is rotated 90 degrees
            //if centroid is in the bottom half of the image, the blue beacon is on the left
            //if the centroid is in the top half, the blue beacon is on the right
            if ((mmnts.get_m01() / mmnts.get_m00()) < cropped.rows() / 2) {
                return BEACON_RED_BLUE;
            } else {
                return BEACON_BLUE_RED;
            }
        }

        return NOT_VISIBLE;
    }
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
String format(OpenGLMatrix transformationMatrix) {
    return (transformationMatrix != null) ? transformationMatrix.formatAsTransform() : "null";
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
public  OpenGLMatrix createMatrix(float x,float y,float z,float u,float v,float w)
{
	return OpenGLMatrix.translation(x,y,z).
		multiplied(Orientation.getRotationMatrix(
			AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES, u,v,w));
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
public String formatMatrix(OpenGLMatrix matrix)
{
	return matrix.formatAsTransform();
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
public OpenGLMatrix createMatrix(float x, float y, float z, float u, float v, float w)
{
    return OpenGLMatrix.translation(x,y,z).
            multiplied(Orientation.getRotationMatrix(
                    AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES, u,v,w));
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
public String formatMatrix(OpenGLMatrix matrix)
{
    return matrix.formatAsTransform();
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
@Override
public void loop(){
    OpenGLMatrix latestLocation = listener.getUpdatedRobotLocation();
    vuMark = RelicRecoveryVuMark.from(relicVuMark);

    if(latestLocation !=null) {lastKnownLocation = latestLocation;}
    float[] coordinates = lastKnownLocation.getTranslation().getData();
    robotX = coordinates[0];
    robotY = coordinates[1];
    robotAngle = Orientation.getOrientation(lastKnownLocation, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
    if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
        key = vuMark;
    }
    double x = gamepad1.right_stick_x;
    double y = gamepad1.right_stick_y;
    double c = gamepad1.left_trigger-gamepad1.right_trigger;
    motor1.setPower(Range.clip(y-x+c, -1, 1));
    motor2.setPower(Range.clip(y+x-c, -1, 1));
    motor3.setPower(Range.clip(y+x+c, -1, 1));
    motor4.setPower(Range.clip(y-x-c, -1, 1));

    if (gamepad2.right_trigger > 0) {
        leftcr.setPower(1);
        rightcr.setPower(1);
    }else if(gamepad2.left_trigger > 0) {
        leftcr.setPower(-1);
        rightcr.setPower(-1);
    }else {
        leftcr.setPower(0.1);
        rightcr.setPower(0);
    }
    if(gamepad2.x) {
        glyph.setPosition(1);
    }
    else if(gamepad2.a) {
        glyph.setPosition(0);
    }
    else if(gamepad2.b) {
        glyph.setPosition(-1);
    }

    telemetry.addData("motor 1 pos", motor1.getCurrentPosition());
    telemetry.addData("motor 2 pos", motor2.getCurrentPosition());
    telemetry.addData("motor 3 pos", motor3.getCurrentPosition());
    telemetry.addData("motor 4 pos", motor4.getCurrentPosition());
    telemetry.addData("jewel servo pos", jewelcr.getPower());
    updateLocation();
    telemetry.addData("Last Known Location", formatMatrix(lastKnownLocation));

    telemetry.addData("Red",color1.red());
    telemetry.addData("Blue",color1.blue());

    telemetry.update();
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
public OpenGLMatrix createMatrix(float x,float y,float z,float u,float v,float w)
{
    return OpenGLMatrix.translation(x,y,z).multiplied(Orientation.getRotationMatrix(AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES, u,v,w));
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
public  OpenGLMatrix createMatrix(float x,float y,float z,float u,float v,float w)
{
    return OpenGLMatrix.translation(x,y,z).
            multiplied(Orientation.getRotationMatrix(
                    AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES, u,v,w));
}
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
@Override
    public void runPeriodic(double elapsedTime)
    {
        double left = gamepad.getLeftStickY(true);
        double right = gamepad.getRightStickY(true);
        driveBase.tankDrive(left, right);

        final int LABEL_WIDTH = 180;
        dashboard.displayPrintf(1, LABEL_WIDTH, "Power(L/R) = ", "%.2f/%.2f", left, right);
        dashboard.displayPrintf(2, LABEL_WIDTH, "GyroHeading = ", "%.2f", gyro.getZHeading().value);
        dashboard.displayPrintf(3, LABEL_WIDTH, "SoundEnvelope = ", "%s", envelopeToggle.getState()? "ON": "OFF");
        dashboard.displayPrintf(4, LABEL_WIDTH, "ToneDevice = ", "%s",
                                analogToneToggle.getState()? "AnalogOut": "Android");
        dashboard.displayPrintf(5, LABEL_WIDTH, "Vision = ", "%s (%d)", visionEnabled, targetIndex);

        if (isVisionEnabled())
        {
            boolean followTarget = followTargetToggle.getState();

            for (int i = 0; i < targets.length; i++)
            {
                VuforiaTrackable target = vuforia.getTarget(i);
                boolean visible = vuforia.isTargetVisible(target);

                if (USE_SPEECH)
                {
                    if (visible != targetsFound[i])
                    {
                        targetsFound[i] = visible;
                        String sentence = String.format(
                                "%s is %s.", target.getName(), visible ? "in view" : "out of view");
                        textToSpeech.speak(sentence, TextToSpeech.QUEUE_FLUSH, null);
                    }
                }

                if (visible)
                {
                    String label = String.format(i == targetIndex ? "<%s> = " : "%s = ", target.getName());
                    OpenGLMatrix pose = vuforia.getTargetPose(target);
                    if (pose != null)
                    {
                        VectorF translation = pose.getTranslation();
                        float targetX = translation.get(0) / MM_PER_INCH;
                        float targetY = translation.get(1) / MM_PER_INCH;
                        float targetZ = -translation.get(2) / MM_PER_INCH;

                        dashboard.displayPrintf(
                                i + 5, LABEL_WIDTH, label, "%6.2f,%6.2f,%6.2f", targetX, targetY, targetZ);

                        if (followTarget && i == targetIndex)
                        {
//                            targetDistance = targetZ;
                            targetDistance = 24.0;
                            // TODO: doesn't work! Event won't fire until distance is satisfied.
                            if (visionEvent.isSignaled())
                            {
                                targetHeading = Math.toDegrees(Math.atan2(targetX, targetZ));
                                visionEvent.set(false);
                                visionPidDrive.setTarget(24.0, targetHeading, false, visionEvent);
                            }
                        }
                    }

                    OpenGLMatrix robotLocation = vuforia.getRobotLocation(target);
                    if (robotLocation != null)
                    {
                        lastKnownRobotLocation = robotLocation;
                    }
                }
            }

            if (lastKnownRobotLocation != null)
            {
                dashboard.displayPrintf(9, LABEL_WIDTH, "RobotLoc = ", lastKnownRobotLocation.formatAsTransform());
            }
        }
    }
 

import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; //导入依赖的package包/类
@Override
public void initRobot()
{
    hardwareMap.logDevices();
    dashboard = HalDashboard.getInstance();
    FtcRobotControllerActivity activity = (FtcRobotControllerActivity)hardwareMap.appContext;
    dashboard.setTextView((TextView)activity.findViewById(R.id.textOpMode));

    vuforia = new FtcVuforia(VUFORIA_LICENSE_KEY, CAMERAVIEW_ID, CAMERA_DIR, TRACKABLES_FILE, targets.length);
    //
    // Phone location: Mounted center on the front of the robot with the back camera facing outward.
    //
    OpenGLMatrix phoneLocationOnRobot =
            TRACK_ROBOT_LOC? vuforia.locationMatrix(90.0f, 0.0f, 0.0f, 0.0f, ROBOT_WIDTH/2.0f, 0.0f): null;

    vuforia.setTargets(targets, phoneLocationOnRobot);
    //
    // Text To Speech.
    //
    if (SPEECH_ENABLED)
    {
        textToSpeech = new TextToSpeech(
                hardwareMap.appContext,
                new TextToSpeech.OnInitListener()
                {
                    @Override
                    public void onInit(int status)
                    {
                        if (status != TextToSpeech.ERROR)
                        {
                            textToSpeech.setLanguage(Locale.US);
                        }
                    }
                });
        targetsFound = new boolean[targets.length];
        for (int i = 0; i < targetsFound.length; i++)
        {
            targetsFound[i] = false;
        }
    }
}