Untitled

 avatar
unknown
plain_text
a month ago
28 kB
3
Indexable
package pedroPathing.examples;

import static java.lang.Math.abs;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;

@TeleOp
public class TeleOP extends LinearOpMode {
    @Override
    public void runOpMode() throws InterruptedException {
        DcMotor motorFrontLeft = hardwareMap.dcMotor.get("FL");
        DcMotor motorBackLeft = hardwareMap.dcMotor.get("BL");
        DcMotor motorFrontRight = hardwareMap.dcMotor.get("FR");
        DcMotor motorBackRight = hardwareMap.dcMotor.get("BR");
        DcMotor LeftAngle = hardwareMap.dcMotor.get("LeftAngle");
        DcMotor RightAngle = hardwareMap.dcMotor.get("RightAngle");
        DcMotor LeftLift = hardwareMap.dcMotor.get("LeftLift");
        DcMotor RightLift = hardwareMap.dcMotor.get("RightLift");

        Servo claw = hardwareMap.get(Servo.class, "claw");
        Servo LeftArm = hardwareMap.get(Servo.class, "LeftArm");
        Servo RightArm = hardwareMap.get(Servo.class, "RightArm");
        Servo claw_degree = hardwareMap.get(Servo.class, "claw_degree");

        motorFrontLeft.setDirection(DcMotor.Direction.REVERSE);
        motorBackLeft.setDirection(DcMotor.Direction.REVERSE);
        LeftAngle.setDirection(DcMotor.Direction.REVERSE);
        RightLift.setDirection(DcMotor.Direction.REVERSE);
        LeftArm.setDirection(Servo.Direction.REVERSE);
        // claw_degree.setDirection(Servo.Direction.REVERSE);
        LeftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        RightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        LeftAngle.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        RightAngle.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        LeftLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        RightLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        IMU imu = hardwareMap.get(IMU.class, "imu");
        IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
                RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
                RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
        imu.initialize(parameters);

        double claw_open = 0.5, claw_closed = 0.25;
        double arm_lowest = 0.28, arm_perpendicular = 0.6;
        double claw_0_pos = 0.1, claw_180_pos = 0.77, claw_90_pos = 0.43;
        boolean lift_down = false;
        double speed = 1;
        boolean lift_from_box_to_submersible_1 = false, lift_from_box_to_submersible_2 = false;
        boolean lift_from_submersible_to_box_1 = false, lift_from_submersible_to_box_2 = false;
        boolean lift_from_human_to_specimen_1 = false, lift_from_human_to_specimen_2 = false;
        boolean lift_from_specimen_to_human_1 = false, lift_from_specimen_to_human_2 = false;
        boolean Speciment = false;

        // INITIALIZE

        LeftArm.setPosition(arm_lowest);
        RightArm.setPosition(arm_lowest);
        claw_degree.setPosition(0.1);

        LeftLift.setTargetPosition(0);
        RightLift.setTargetPosition(0);
        LeftLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        LeftLift.setPower(0.7);
        RightLift.setPower(0.7);

        LeftAngle.setTargetPosition(0);
        RightAngle.setTargetPosition(0);
        LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        LeftAngle.setPower(1);
        RightAngle.setPower(1);

        waitForStart();

        if (isStopRequested()) return;

        while (opModeIsActive()) {
            // double r = Math.hypot(gamepad1.left_stick_x, gamepad1.left_stick_y);
            // double robotAngle = Math.atan2(gamepad1.left_stick_y, -gamepad1.left_stick_x) - Math.PI/4;
            // double rightX = gamepad1.right_stick_x *  0.5;
            // final double v1 = (r * Math.cos(robotAngle) - rightX) * speed;
            // final double v2 = (r * Math.sin(robotAngle) + rightX) * speed;
            // final double v3 = (r * Math.sin(robotAngle) - rightX) * speed;
            // final double v4 = (r * Math.cos(robotAngle) + rightX) * speed;
            // motorFrontLeft.setPower(-v1);
            // motorFrontRight.setPower(-v2);
            // motorBackLeft.setPower(-v3);
            // motorBackRight.setPower(-v4);

            double y = -gamepad1.left_stick_y;
            double x = gamepad1.left_stick_x;
            double rx = gamepad1.right_stick_x;

            if (gamepad1.options) {
                imu.resetYaw();
            }

            double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);

            double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
            double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);

            rotX = rotX * 1.1;

            double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
            double frontLeftPower = (rotY + rotX + rx) / denominator;
            double backLeftPower = (rotY - rotX + rx) / denominator;
            double frontRightPower = (rotY - rotX - rx) / denominator;
            double backRightPower = (rotY + rotX - rx) / denominator;

            motorFrontLeft.setPower(frontLeftPower * speed);
            motorBackLeft.setPower(backLeftPower * speed);
            motorFrontRight.setPower(frontRightPower * speed);
            motorBackRight.setPower(backRightPower * speed);

            ////////////////////////////////////////////////////////////////////////////////////

            if(Speciment == false){
                if(lift_from_box_to_submersible_1 == true){
                    if(LeftLift.isBusy() == true || RightLift.isBusy() == true){
                        LeftLift.setPower(1);
                        RightLift.setPower(1);
                        LeftArm.setPosition(0.28);
                        RightArm.setPosition(0.28);
                        claw_degree.setPosition(claw_0_pos);
                        LeftLift.setTargetPosition(0);
                        RightLift.setTargetPosition(0);
                        LeftLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        telemetry.addData("lift_from_box_1", lift_from_box_to_submersible_1);

                        lift_down = false;
                    }
                    else{
                        lift_from_box_to_submersible_1 = false;
                        lift_from_box_to_submersible_2 = true;

                        LeftAngle.setTargetPosition(660);
                        RightAngle.setTargetPosition(660);
                        LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        LeftAngle.setPower(0.9);
                        RightAngle.setPower(0.9);
                        lift_down = false;
                    }
                }
                if(lift_from_box_to_submersible_2 == true){
                    if(LeftAngle.isBusy() == true || RightAngle.isBusy() == true){
                        LeftAngle.setTargetPosition(660);
                        RightAngle.setTargetPosition(660);
                        LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        LeftAngle.setPower(0.9);
                        RightAngle.setPower(0.9);
                    }
                    else{
                        lift_from_box_to_submersible_2 = false;
                        lift_down = false;
                    }
                }

                if(lift_from_submersible_to_box_1 == true){
                    if(LeftAngle.isBusy() == true || RightAngle.isBusy() == true){
                        LeftAngle.setTargetPosition(0);
                        RightAngle.setTargetPosition(0);
                        LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        LeftAngle.setPower(0.9);
                        RightAngle.setPower(0.9);

                        lift_down = false;
                    }
                    else{
                        LeftLift.setTargetPosition(1000);
                        RightLift.setTargetPosition(1000);
                        LeftLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        LeftLift.setPower(1);
                        RightLift.setPower(1);
                        LeftArm.setPosition(0.85);
                        RightArm.setPosition(0.85);
                        claw_degree.setPosition(claw_90_pos);

                        lift_from_submersible_to_box_1 = false;
                        lift_from_submersible_to_box_2 = true;
                    }
                }
                if(lift_from_submersible_to_box_2 == true){
                    if(LeftLift.isBusy() == true || RightLift.isBusy() == true){
                        LeftLift.setTargetPosition(1000);
                        RightLift.setTargetPosition(1000);
                        LeftLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        LeftLift.setPower(0.9);
                        RightLift.setPower(0.9);
                        LeftArm.setPosition(0.85);
                        RightArm.setPosition(0.85);
                        claw_degree.setPosition(claw_90_pos);

                        lift_down = false;
                    }
                    else{
                        lift_from_submersible_to_box_2 = false;
                        lift_down = false;
                    }
                }
                if(gamepad2.dpad_up == true){
                    LeftAngle.setTargetPosition(0);
                    RightAngle.setTargetPosition(0);
                    LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                    RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                    LeftAngle.setPower(0.4);
                    RightAngle.setPower(0.4);

                    LeftLift.setPower(0);
                    RightLift.setPower(0);

                    lift_from_submersible_to_box_1 = true;
                    lift_down = false;
                }
                if(gamepad2.dpad_down == true){
                    LeftLift.setPower(1);
                    RightLift.setPower(1);
                    LeftArm.setPosition(0.28);
                    RightArm.setPosition(0.28);
                    claw_degree.setPosition(claw_0_pos);
                    LeftLift.setTargetPosition(0);
                    RightLift.setTargetPosition(0);
                    LeftLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                    RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);

                    LeftAngle.setPower(0);
                    RightAngle.setPower(0);

                    lift_from_box_to_submersible_1 = true;
                    lift_down = false;
                }
            }
            else{
                if(lift_from_box_to_submersible_1 == true){
                    if(LeftLift.isBusy() == true || RightLift.isBusy() == true){
                        LeftLift.setPower(1);
                        RightLift.setPower(1);
                        LeftArm.setPosition(0.28);
                        RightArm.setPosition(0.28);
                        claw_degree.setPosition(claw_0_pos);
                        LeftLift.setTargetPosition(0);
                        RightLift.setTargetPosition(0);
                        LeftLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        telemetry.addData("lift_from_box_1", lift_from_box_to_submersible_1);

                        lift_down = false;
                    }
                    else{
                        lift_from_box_to_submersible_1 = false;
                        lift_from_box_to_submersible_2 = true;

                        LeftAngle.setTargetPosition(660);
                        RightAngle.setTargetPosition(660);
                        LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        LeftAngle.setPower(0.9);
                        RightAngle.setPower(0.9);
                        lift_down = false;
                    }
                }
                if(lift_from_box_to_submersible_2 == true){
                    if(LeftAngle.isBusy() == true || RightAngle.isBusy() == true){
                        LeftAngle.setTargetPosition(660);
                        RightAngle.setTargetPosition(660);
                        LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        LeftAngle.setPower(0.9);
                        RightAngle.setPower(0.9);
                    }
                    else{
                        lift_from_box_to_submersible_2 = false;
                        lift_down = false;
                    }
                }
                if(lift_from_human_to_specimen_1 == true){
                    if(LeftAngle.isBusy() == true || RightAngle.isBusy() == true){
                        LeftAngle.setTargetPosition(0);
                        RightAngle.setTargetPosition(0);
                        LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        LeftAngle.setPower(0.8);
                        RightAngle.setPower(0.8);
                        LeftArm.setPosition(0.95);
                        RightArm.setPosition(0.95);
                        claw_degree.setPosition(claw_180_pos);

                        lift_down = false;
                    }
                    else{
                        LeftLift.setPower(1);
                        RightLift.setPower(1);
                        LeftLift.setTargetPosition(425);
                        RightLift.setTargetPosition(425);
                        LeftLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);

                        lift_down = false;
                        lift_from_human_to_specimen_1 = false;
                        lift_from_human_to_specimen_2 = true;
                    }
                }
                if(lift_from_human_to_specimen_2 == true){
                    if(LeftLift.isBusy() == true || RightLift.isBusy() == true){
                        LeftLift.setPower(1);
                        RightLift.setPower(1);
                        LeftLift.setTargetPosition(425);
                        RightLift.setTargetPosition(425);
                        LeftLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);

                        lift_down = false;
                    }
                    else{
                        lift_from_human_to_specimen_2 = false;
                        lift_down = false;
                    }
                }

                if(lift_from_specimen_to_human_1 == true){
                    if(LeftLift.isBusy() == true || RightLift.isBusy() == true){
                        LeftLift.setPower(1);
                        RightLift.setPower(1);
                        LeftLift.setTargetPosition(125);
                        RightLift.setTargetPosition(125);
                        LeftLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);

                        LeftAngle.setPower(0);
                        RightAngle.setPower(0);

                        lift_from_specimen_to_human_1 = true;
                        lift_down = false;
                    }
                    else{
                        LeftArm.setPosition(arm_perpendicular);
                        RightArm.setPosition(arm_perpendicular);
                        claw_degree.setPosition(claw_0_pos);
                        LeftAngle.setPower(0.6);
                        RightAngle.setPower(0.6);
                        LeftAngle.setTargetPosition(675);
                        RightAngle.setTargetPosition(675);
                        LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);

                        lift_from_specimen_to_human_1 = false;
                        lift_from_specimen_to_human_2 = true;
                        lift_down = false;
                    }
                }
                if(lift_from_specimen_to_human_2 == true){
                    if(LeftAngle.isBusy() == true || RightAngle.isBusy() == true){
                        LeftArm.setPosition(arm_perpendicular);
                        RightArm.setPosition(arm_perpendicular);
                        claw_degree.setPosition(claw_0_pos);
                        LeftAngle.setPower(0.6);
                        RightAngle.setPower(0.6);
                        LeftAngle.setTargetPosition(675);
                        RightAngle.setTargetPosition(675);
                        LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                        claw_degree.setPosition(claw_0_pos);

                        lift_down = false;
                    }
                    else{
                        lift_from_specimen_to_human_2 = false;
                        lift_down = false;
                    }
                }
                /////////////////////////////////////
                if(gamepad2.dpad_right == true){
                    LeftAngle.setTargetPosition(0);
                    RightAngle.setTargetPosition(0);
                    LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                    RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                    LeftAngle.setPower(0.6);
                    RightAngle.setPower(0.6);
                    LeftArm.setPosition(0.95);
                    RightArm.setPosition(0.95);
                    claw_degree.setPosition(claw_180_pos);

                    LeftLift.setPower(0);
                    RightLift.setPower(0);

                    lift_from_human_to_specimen_1 = true;
                    lift_down = false;
                }
                if(gamepad2.dpad_left == true){
                    LeftLift.setPower(1);
                    RightLift.setPower(1);
                    LeftLift.setTargetPosition(125);
                    RightLift.setTargetPosition(125);
                    LeftLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                    RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);

                    LeftAngle.setPower(0);
                    RightAngle.setPower(0);

                    lift_from_specimen_to_human_1 = true;
                    lift_down = false;
                }
                if(gamepad2.dpad_down == true){
                    LeftLift.setPower(1);
                    RightLift.setPower(1);
                    LeftArm.setPosition(0.28);
                    RightArm.setPosition(0.28);
                    claw_degree.setPosition(claw_0_pos);
                    LeftLift.setTargetPosition(0);
                    RightLift.setTargetPosition(0);
                    LeftLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                    RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);

                    LeftAngle.setPower(0);
                    RightAngle.setPower(0);

                    lift_from_box_to_submersible_1 = true;
                    lift_down = false;
                }
            }

            if(gamepad2.left_bumper == true){
                claw.setPosition(claw_open);
            }
            if(gamepad2.right_bumper == true){
                claw.setPosition(claw_closed);
            }
            if(gamepad2.y == true){
                LeftAngle.setTargetPosition(660);
                RightAngle.setTargetPosition(660);
                LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                LeftAngle.setPower(0.5);
                RightAngle.setPower(0.5);
                LeftArm.setPosition(0.28);
                RightArm.setPosition(0.28);
                // claw_degree.setPosition(claw_0_pos);
            }
            if(gamepad1.y == true){
                LeftAngle.setTargetPosition(660);
                RightAngle.setTargetPosition(660);
                LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                LeftAngle.setPower(0.5);
                RightAngle.setPower(0.5);
                claw_degree.setPosition(claw_0_pos);
                LeftArm.setPosition(0.28);
                RightArm.setPosition(0.28);
                // claw_degree.setPosition(claw_0_pos);

                lift_down = false;
            }
            if(gamepad1.dpad_left == true){
                LeftLift.setPower(1);
                RightLift.setPower(1);
                LeftArm.setPosition(arm_perpendicular);
                RightArm.setPosition(arm_perpendicular);
                claw_degree.setPosition(claw_0_pos);
                LeftLift.setTargetPosition(0);
                RightLift.setTargetPosition(0);
                LeftLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                RightLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);

                LeftAngle.setTargetPosition(625);
                RightAngle.setTargetPosition(625);
                LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                LeftAngle.setPower(0.5);
                RightAngle.setPower(0.5);

                lift_down = false;
            }
            if(gamepad1.x == true){
                LeftAngle.setTargetPosition(750);
                RightAngle.setTargetPosition(750);
                LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                LeftAngle.setPower(0.7);
                RightAngle.setPower(0.7);
                LeftArm.setPosition(0.28);
                RightArm.setPosition(0.28);
                // claw_degree.setPosition(claw_0_pos);

                lift_down = true;
            }
            if(gamepad2.a == true){
                LeftAngle.setTargetPosition(750);
                RightAngle.setTargetPosition(750);
                LeftAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                RightAngle.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                LeftAngle.setPower(0.7);
                RightAngle.setPower(0.7);
                LeftArm.setPosition(0.28);
                RightArm.setPosition(0.28);
                // claw_degree.setPosition(claw_0_pos);

                lift_down = true;
            }
            if(gamepad2.right_trigger != 0){
                LeftLift.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
                RightLift.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
                LeftLift.setPower(gamepad2.right_trigger);
                RightLift.setPower(gamepad2.right_trigger);

                lift_down = true;
            }
            else if(gamepad2.left_trigger != 0){
                LeftLift.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
                RightLift.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
                LeftLift.setPower(-gamepad2.left_trigger);
                RightLift.setPower(-gamepad2.left_trigger);

                lift_down = true;
            }
            else if(lift_down == true){
                LeftLift.setPower(0);
                RightLift.setPower(0);
            }
            if(gamepad2.x == true){
                claw_degree.setPosition(claw_0_pos);
            }
            if(gamepad2.b == true){
                claw_degree.setPosition(claw_90_pos);
            }
            if(gamepad1.b == true){
                speed = 1;
            }
            if(gamepad1.a == true){
                speed = 0.5;
            }
            if(gamepad2.left_bumper == true && gamepad2.right_bumper == true){
                Speciment = false;
            }
            if(gamepad2.x == true && gamepad2.b == true){
                Speciment = true;
            }

            // if(gamepad1.a == true){
            //     claw_degree.setPosition(claw_0_pos);
            // }
            // if(gamepad1.b == true){
            //     claw_degree.setPosition(claw_90_pos);
            // }
            if(Speciment == true){
                telemetry.addData("MODE SPECIMENT, MODE SPECIMENT", true);
                telemetry.addData("MODE SPECIMENT, MODE SPECIMENT", true);
                telemetry.addData("MODE SPECIMENT, MODE SPECIMENT", true);
                telemetry.addData("MODE SPECIMENT, MODE SPECIMENT", true);
                telemetry.addData("MODE SPECIMENT, MODE SPECIMENT", true);
                telemetry.addData("MODE SPECIMENT, MODE SPECIMENT", true);
            }
            else{
                telemetry.addData("BOX, BOX, BOX, BOX, BOX, BOX", true);
                telemetry.addData("BOX, BOX, BOX, BOX, BOX, BOX", true);
                telemetry.addData("BOX, BOX, BOX, BOX, BOX, BOX", true);
                telemetry.addData("BOX, BOX, BOX, BOX, BOX, BOX", true);
                telemetry.addData("BOX, BOX, BOX, BOX, BOX, BOX", true);
                telemetry.addData("BOX, BOX, BOX, BOX, BOX, BOX", true);
            }
            telemetry.addData("LeftAngle ", LeftAngle.getCurrentPosition());
            telemetry.addData("RightAngle ", RightAngle.getCurrentPosition());
            telemetry.addData("LeftLift ", LeftLift.getCurrentPosition());
            telemetry.addData("RightLift ", RightLift.getCurrentPosition());
            telemetry.update();
        }
    }
}
Editor is loading...
Leave a Comment