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