Untitled
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