Auto PID

gistfile1.txt

package org.firstinspires.ftc.teamcode;

import android.content.SharedPreferences;
import android.preference.PreferenceManager;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;

import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.Func;
import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
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.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 java.util.Locale;

/**
 * Created by rommac100 on 10/15/17
 * Base class for Autos Override Methods if needed in the subclasses.
 */

public class Auto3666 extends LinearOpMode {

    Orientation angles;
    Acceleration gravity;

    public final double kp = 0;
    public final double ki = 0;
    public final double kd = 0;

    private final double TURNING_OFFSET = 27;

    public Hardware3666 robot = new Hardware3666();
    private ElapsedTime runtime = new ElapsedTime();

    public String allianceColour = "";
    public int allianceMember;
    public boolean knockBallStatus = false;

    public String[] allianceColourPos = {"red", "blue"};

    enum VuMarkCryptoPos
    {
        LEFT, CENTER, RIGHT;
    }
    VuMarkCryptoPos vuPos;


    float mmperInch = 25.4f;
    float mmBotWidth = 18 * mmperInch;
    float mmFTCField = (12*12-2) * mmperInch;



    OpenGLMatrix blueTarget1 = OpenGLMatrix.translation(-mmFTCField/2, mmFTCField/8, mmFTCField/24).multiplied(Orientation.getRotationMatrix(
            AxesReference.EXTRINSIC, AxesOrder.XZX,
            AngleUnit.DEGREES, 90, 90, 0));

    OpenGLMatrix blueTarget2 = OpenGLMatrix.translation(-mmFTCField/2, mmFTCField*5/8, mmFTCField/24).multiplied(Orientation.getRotationMatrix(
            AxesReference.EXTRINSIC, AxesOrder.XZX,
            AngleUnit.DEGREES, 90, 90, 0));
    OpenGLMatrix redTarget1 = OpenGLMatrix.translation(mmFTCField/2, mmFTCField/8, mmFTCField/24).multiplied(Orientation.getRotationMatrix(
            AxesReference.EXTRINSIC, AxesOrder.XZX,
            AngleUnit.DEGREES, 90, 90, 0));

    OpenGLMatrix redTarget2 = OpenGLMatrix.translation(mmFTCField/2, mmFTCField*5/8, mmFTCField/24).multiplied(Orientation.getRotationMatrix(
            AxesReference.EXTRINSIC, AxesOrder.XZX,
            AngleUnit.DEGREES, 90, 90, 0));


    OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix
            .translation(mmBotWidth/2,0,0)
            .multiplied(Orientation.getRotationMatrix(
                    AxesReference.EXTRINSIC, AxesOrder.YZY,
                    AngleUnit.DEGREES, -90, 0, 0));


    OpenGLMatrix lastLocation = null;
    VuforiaLocalizer vuforia;

    VuforiaTrackables relicTrackables;
    VuforiaTrackable relicTemplate;

    RelicRecoveryVuMark vuMark;

    private GyroListener imuListener;

    private double tX;
    private double tY;
    private double tZ;

    public void initRobot() {
        robot.init(hardwareMap);
        robot.setBrakeSettings();
    }

    public double getTX() {return tX;}
    public double getTY() {return tY;}
    public double getTZ() {return tZ;}

    public void initVuforia() {

        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);

        parameters.vuforiaLicenseKey = "AVVR6lD/////AAAAGaBoMQxcC0SLtMheM5OtO54R1JrkfN5XmPfiD6JY3kjv2FpL89mvwGDfdIjGHNn3IX0hqmsQShWJTFNUoZfdqIdwk4vc6KQhttMW7KG+2P+45CsiWNqQDC5K+N7rjDJFGXuMevVOr3ZzqV4Fz75qsLw50v8hEFSO8XodWGnYNpvKyR0qmQlr2Bn87n/Ket1FVn/5vFnmrOphctH1QOTk7JPSdW8z89Ag7cVgIbEUBThve0VqW9Ygh1yMm2m6RorTuGBxmTnanAwpoRCFI27j8cZGAXoELNeFPkkASTOc3Wy8pOOQ+zjKzS8HT41q0hdx+PNWHEnD5DduKsu/LzM+gbjyqdRxnYLAP6IVNxMVBA9L";
        parameters.cameraDirection = VuforiaLocalizer.CameraDirection.FRONT;
        this.vuforia = ClassFactory.createVuforiaLocalizer(parameters);

        relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
        relicTemplate = relicTrackables.get(0);

    }

    public void startVuforia() {
        relicTrackables.activate();
        telemetry.addAction(new Runnable() {
            @Override
            public void run() {
                updatePosVuforia();
            }
        });
    }

    public void stopVuforia() {
        relicTrackables.deactivate();
    }

    String format(OpenGLMatrix transformationMatrix) {
        return (transformationMatrix != null) ? transformationMatrix.formatAsTransform() : "null";
    }

    public boolean updatePosVuforia() {
        if (allianceColour.equals(allianceColourPos[0]))
        {
            if (allianceMember ==1)
            {
                relicTemplate.setLocation(redTarget1);
            }
            else if (allianceMember == 2)
            {
                relicTemplate.setLocation(redTarget2);
            }
        }
        else if (allianceColour.equals(allianceColourPos[1]))
        {
            if (allianceMember ==1)
            {
                relicTemplate.setLocation(blueTarget1);
            }
            else if (allianceMember == 2)
            {
                relicTemplate.setLocation(blueTarget2);
            }
        }
        vuMark = RelicRecoveryVuMark.from(relicTemplate);

        if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
            telemetry.addData("VuMark", "%s visible", vuMark);
            if (vuMark == RelicRecoveryVuMark.CENTER)
            {
                vuPos = VuMarkCryptoPos.CENTER;
            }
            else if (vuMark == RelicRecoveryVuMark.LEFT)
            {
                vuPos = VuMarkCryptoPos.LEFT;
            }
            else if (vuMark == RelicRecoveryVuMark.RIGHT)
            {
                vuPos = VuMarkCryptoPos.RIGHT;
            }
            OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) relicTemplate.getListener()).getUpdatedRobotLocation();
            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);
                return true;
            } else {
                return false;
            }
        }
        return false;
    }

    public double inchtoMM(double inchesArr)
    {
        return inchesArr*25.4;
    }

    public void driveVuforia(double distance)
    {
        double desireDistance = distance +getTX();
        if (distance > 0)
        {
            while (desireDistance > getTX() && opModeIsActive())
            {
                telemetry.update();
                robot.setDrivePow(.6,.6);
            }
            robot.setDrivePow(0,0);
        }
        else if (distance < 0)
        {
            while (desireDistance < getTX() & opModeIsActive())
            {
                telemetry.update();
                robot.setDrivePow(-.6,-.6);
            }
            robot.setDrivePow(0,0);
        }
    }

    public void getAutonomousSettings() {
        SharedPreferences internalPreferences = PreferenceManager.getDefaultSharedPreferences(hardwareMap.appContext);

        //Red or Blue
        allianceColour = internalPreferences.getString("Alliance Colour", "");
        allianceColour = allianceColour.toLowerCase();
        String tempBallStatus = "";
        tempBallStatus = internalPreferences.getString("Knock Ball", "");
        if (tempBallStatus.toLowerCase().equals("yes")) {
            knockBallStatus = true;
        } else if (tempBallStatus.toLowerCase().equals("no")) {
            knockBallStatus = false;
        }
        allianceMember = internalPreferences.getInt("Alliance Member", 0);

        telemetry.addData("Alliance Member", allianceMember);
        telemetry.addData("Alliance Colour: ", allianceColour);
        telemetry.addData("Knockball Status: ", tempBallStatus);
        telemetry.update();
    }

    @Override
    public void runOpMode() throws InterruptedException {
        initRobot();

        telemetry.addData("Robot Init", "");
        telemetry.update();

        waitForStart();

        while (opModeIsActive()) {
            telemetry.update();
            idle();
        }

    }

    public enum DirectionTurning {
        FORWARD (+1.0),
        BACKWARD (-1.0),
        CLOCKWISE (+1.0),
        COUNTERCLOCKWISE (-1.0);
        public final double value;
        DirectionTurning (double value) {this.value = value;}
    }

    public double adjustAngle(double angle) {
        while (angle > 180) angle -= 360;
        while (angle <= -180) angle += 360;
        return angle;
    }

    public void turnP(double degrees, DirectionTurning direction, double timeout, double speed, double kp) {
        double targetAngle = adjustAngle(getHeading() + direction.value * degrees);
        double error =0;
        boolean errorNull = true;
        double previousError = 0;
        double power;
        double timePrev = 0;
        double time = 0;
        double elapsedTime = 0;
        double pid_p = 0;
        double pid_i = 0;
        double pid_d = 0;
        double PID = 0;

        do {
            if (!errorNull)
            {
                previousError = error;
                timePrev = time;
            }
            error = adjustAngle(targetAngle - getHeading());
            if (errorNull)
            {
                errorNull = false;
            }
            pid_p = kp * error;
             time  = (System.nanoTime())*Math.pow(10,9);
             if (timePrev > 0)
             {
                 elapsedTime = (time-timePrev);
             }

            if (-5 < error && error <5)
            {
                pid_i = pid_i+(ki*error);
            }

            if (!errorNull && elapsedTime != 0) {
                pid_d = kd * ((error - previousError) / elapsedTime);
            }

            PID = pid_p+pid_i+pid_d;

            power = speed+ PID;
            power = Range.clip(power, -speed, +speed);
            robot.setDrivePow(-power, +power);
            idle();
        } while (opModeIsActive() && Math.abs(error) > 1);
        robot.setDrivePow(0,0);
    }

    public double getHeading()
    {
        telemetry.update();
        return angles.firstAngle;
    }

    public void turnDegrees(double degrees)
    {
        double desiredDegrees = getHeading()+degrees;
        robot.setBrakeSettings();

       if (degrees >0)
       {
           desiredDegrees-= TURNING_OFFSET;
           while (getHeading() < desiredDegrees) {
               robot.setDrivePow(.15, -.15);
               telemetry.addData("Heading: ", getHeading());
               telemetry.update();
           }
           robot.setDrivePow(0,0);
           }
           else if (degrees <0)
       {
           desiredDegrees += TURNING_OFFSET;
           while (getHeading() > desiredDegrees)
           {
               robot.setDrivePow(-.15,.15);
               telemetry.addData("Heading: ", getHeading());
               telemetry.update();
           }
           robot.setDrivePow(0,0);
       }
    }

    String formatAngle(AngleUnit angleUnit, double angle) {
        return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
    }

    String formatDegrees(double degrees){
        return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
    }

    void composeTelemetry() {

        // At the beginning of each telemetry update, grab a bunch of data
        // from the IMU that we will then display in separate lines.
        telemetry.addAction(new Runnable() { @Override public void run()
        {
            // Acquiring the angles is relatively expensive; we don't want
            // to do that in each of the three items that need that info, as that's
            // three times the necessary expense.
            angles   = robot.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
            gravity  = robot.imu.getGravity();
        }
        });

        telemetry.addLine()
                .addData("status", new Func<String>() {
                    @Override public String value() {
                        return robot.imu.getSystemStatus().toShortString();
                    }
                })
                .addData("calib", new Func<String>() {
                    @Override public String value() {
                        return robot.imu.getCalibrationStatus().toString();
                    }
                });

        telemetry.addLine()
                .addData("heading", new Func<String>() {
                    @Override public String value() {
                        return formatAngle(angles.angleUnit, angles.firstAngle);
                    }
                })
                .addData("roll", new Func<String>() {
                    @Override public String value() {
                        return formatAngle(angles.angleUnit, angles.secondAngle);
                    }
                })
                .addData("pitch", new Func<String>() {
                    @Override public String value() {
                        return formatAngle(angles.angleUnit, angles.thirdAngle);
                    }
                });

        telemetry.addLine()
                .addData("grvty", new Func<String>() {
                    @Override public String value() {
                        return gravity.toString();
                    }
                })
                .addData("mag", new Func<String>() {
                    @Override public String value() {
                        return String.format(Locale.getDefault(), "%.3f",
                                Math.sqrt(gravity.xAccel*gravity.xAccel
                                        + gravity.yAccel*gravity.yAccel
                                        + gravity.zAccel*gravity.zAccel));
                    }
                });
    }

    public int inchToTics(double distance)
    {
        return (int)(distance*robot.ticksPerInch);
    }

    //Glyph side is forward
    public void encoderDrive(int leftDistance, int rightDistance, Hardware3666.Direction direction) {

        //Forward or Reverse in Motion

        //Sets encoders states to be runtoPos and Braking on
        if (robot.leftDrive1.getMode() != DcMotor.RunMode.RUN_TO_POSITION) {
            robot.leftDrive1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            robot.leftDrive2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

            robot.rightDrive1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            robot.rightDrive2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

            robot.leftDrive1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            robot.leftDrive2.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            robot.rightDrive1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            robot.rightDrive2.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            robot.leftDrive1.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
            robot.leftDrive2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
            robot.rightDrive1.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
            robot.rightDrive2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        }
        if (direction == Hardware3666.Direction.BACKWARD) {
            robot.leftDrive1.setTargetPosition(leftDistance + robot.leftDrive1.getCurrentPosition());
            robot.leftDrive2.setTargetPosition(leftDistance + robot.leftDrive2.getCurrentPosition());

            robot.rightDrive1.setTargetPosition(rightDistance + robot.rightDrive1.getCurrentPosition());
            robot.rightDrive2.setTargetPosition(rightDistance + robot.rightDrive2.getCurrentPosition());
        } else if (direction == Hardware3666.Direction.FORWARD) {
            robot.leftDrive1.setTargetPosition(-leftDistance + robot.leftDrive1.getCurrentPosition());
            robot.leftDrive2.setTargetPosition(-leftDistance + robot.leftDrive2.getCurrentPosition());

            robot.rightDrive1.setTargetPosition(-rightDistance + robot.rightDrive1.getCurrentPosition());
            robot.rightDrive2.setTargetPosition(-rightDistance + robot.rightDrive2.getCurrentPosition());
        }


        robot.setDrivePow(direction.value, direction.value);
        //loop until the motor is no longer busy - finished the traveling
        while (robot.leftDrive2.isBusy() && robot.rightDrive1.isBusy() && opModeIsActive()) {
            telemetry.addData("Distance Traveled", robot.leftDrive2.getCurrentPosition());
            telemetry.update();
            idle();
        }
        robot.setDrivePow(0, 0);

        //reset motors to their normal behavior, float, runw/oEncoder
        robot.leftDrive1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        robot.leftDrive2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        robot.rightDrive1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        robot.rightDrive2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        robot.leftDrive1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        robot.leftDrive2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

        robot.rightDrive1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        robot.rightDrive2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

        robot.leftDrive1.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
        robot.leftDrive2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
        robot.rightDrive1.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
        robot.rightDrive2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
    }
}