Gobilda Four Stage Slides Test

FourStageSlides.java

package org.firstinspires.ftc.teamcode;

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

@TeleOp(name="Four Stage Slides Test", group="Linear OpMode")
public class FourStageSlides extends LinearOpMode {

    private ElapsedTime runtime = new ElapsedTime();
    private DcMotor armOne = null;
    private DcMotor armTwo = null;
    int armOneUp = 3100;
    int armOneDown = 1;
    int armTwoUp = -3100;
    int armTwoDown = -1;
    int posOne = 0;
    int posTwo = 0;

    @Override
    public void runOpMode() {
        telemetry.addData("Status", "Initialized");
        telemetry.update();
        
        armOne = hardwareMap.get(DcMotor.class, "left_drive");
        armTwo = hardwareMap.get(DcMotor.class, "right_drive");

        armOne.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        armOne.setTargetPosition(armOneDown);
        armOne.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        
        armTwo.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        armTwo.setTargetPosition(armTwoDown);
        armTwo.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        
        armOne.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        armTwo.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        waitForStart();
        runtime.reset();

        while (opModeIsActive()) {
            if (gamepad2.a) {
                armOne.setTargetPosition(armOneUp);
                armTwo.setTargetPosition(armTwoUp);
                armOne.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                armTwo.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                armOne.setPower(0.5);
                armTwo.setPower(0.5);
            }
            
            if (gamepad2.b) {
                armOne.setTargetPosition(armOneDown);
                armTwo.setTargetPosition(armTwoDown);
                armOne.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                armTwo.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                armOne.setPower(0.5);
                armTwo.setPower(0.5);
            }
            
            if (armOne.isBusy() || armTwo.isBusy()) {
                armOne.setPower(0.5);
                armTwo.setPower(0.5);
            } else {
                armOne.setPower(0);
                armTwo.setPower(0);
            }

            telemetry.addData("Status", "Run Time: " + runtime.toString());
            
            telemetry.addData("One Power", armOne.getPower());
            telemetry.addData("Two Power", armTwo.getPower());
            
            telemetry.addData("One Pos", armOne.getCurrentPosition());
            telemetry.addData("Two Pos", armTwo.getCurrentPosition());
            
            telemetry.addData("One Target", armOne.getTargetPosition());
            telemetry.addData("Two Target", armTwo.getTargetPosition());
            
            telemetry.addData("Left Stick", gamepad2.left_stick_y);
            telemetry.addData("Right Stick", gamepad2.right_stick_y);
            
            telemetry.update();
        }
    }
}