Everybot.java

package org.firstinspires.ftc.teamcode.Everybot;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.subsystems.drive.Drivetrain;
import org.firstinspires.ftc.teamcode.utils.PID;
import org.firstinspires.ftc.teamcode.utils.Pose2D;

public class Everybot {
    Telemetry telemetry;
    HardwareMap hwMap;
    LinearOpMode opMode;


    public Drivetrain drivetrain;

    //Outtake
    DcMotorEx outtake1;
    DcMotorEx outtake2;

    //Intake
    DcMotorEx intake;

    //Park
    DcMotorEx park;


    public Everybot(HardwareMap hwMap, Telemetry telemetry, LinearOpMode opMode) {
        this.hwMap = hwMap;
        this.telemetry = telemetry;
        this.opMode = opMode;


        drivetrain = new Drivetrain(hwMap, telemetry);

        outtake1 = hwMap.get(DcMotorEx.class, "o1");
        outtake1.setDirection(DcMotorSimple.Direction.REVERSE);
        outtake2 = hwMap.get(DcMotorEx.class, "o2");
        intake = hwMap.get(DcMotorEx.class, "i");
        park = hwMap.get(DcMotorEx.class, "park");

        park.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        outtake1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        park.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        outtake1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

    }

    public void update() {
        updateOuttake();
        drivetrain.update();
    }

    public void intake() {
        intake.setPower(1);
    }
    public void outtakeOnTheIntake() {
        intake.setPower(-1);
    }
    public void turnOffIntake() {
        intake.setPower(0);
    }



    double OUT = 0;
    double IN = 95;
    public double outtakeTarget = 0;
    public static double p = 0.1;
    PID outtakePID = new PID(p, 0,0);
    public void updateOuttake() {
        double pid = outtakePID.update(outtakeTarget - outtake1.getCurrentPosition());
        outtake1.setPower(pid);
        outtake2.setPower(pid);
    }

    double PARK = -2200;
    public double parkTarget = 0;
    PID parkPID = new PID(0.03, 0,0);
    public void updatePark() {
        double pid = parkPID.update(parkTarget - park.getCurrentPosition());
        park.setPower(pid);
    }

    public void load() {
        outtakeTarget = IN;
    }
    public void shoot() {
        outtakeTarget = OUT;
    }

    public void goBackwards() {
        drivetrain.setDrivePower(-1, -1, -1, -1);
    }


    public void goToPoint(Pose2D pose, double maxSpeed, boolean drift) {
        drivetrain.goToPoint(pose, false, drift);
        while (!opMode.isStopRequested() && drivetrain.isBusy() ) {
            update();
        }
    }

    public void sleep(double ms) {
        double time = System.currentTimeMillis();
        while (!opMode.isStopRequested() &&  System.currentTimeMillis() - time > ms) {
            update();
        }
    }

    public void runShoot() {
        shoot();
        updateOuttake();
        sleep(250);
    }

    public void park() {
        parkTarget = PARK;
        updatePark();
        sleep(2000);
    }
}

everybotTeleOp.java

package org.firstinspires.ftc.teamcode.opmodes.TeleOps;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import org.firstinspires.ftc.teamcode.Everybot.Everybot;

@TeleOp(name="Everybot")
public class everybotTeleOp extends LinearOpMode {
    @Override
    public void runOpMode() throws InterruptedException {
        Everybot everybot = new Everybot(hardwareMap, telemetry, this);


        while (opModeInInit()) {
            everybot.load();
            everybot.updateOuttake();
        }


        waitForStart();
        while (opModeIsActive()) {
            if (gamepad1.right_trigger > 0.5) {
                everybot.shoot();
            } else {
                everybot.load();
            }

            if (gamepad1.dpad_down) {
                everybot.parkTarget -= 1;
            } else if (gamepad1.dpad_up) {
                everybot.parkTarget += 1;
            }
            if (gamepad1.a) {
                everybot.park();
            }

            if (gamepad1.x) {
                everybot.intake();
            } else if (gamepad1.b) {
                everybot.turnOffIntake();
            } else if (gamepad1.y) {
                everybot.outtakeOnTheIntake();
            }



            everybot.drivetrain.drive(gamepad1);
            everybot.updateOuttake();
//            everybot.update();
            everybot.updatePark();
            telemetry.addData("Park Target: ", everybot.parkTarget);
            telemetry.update();
        }
    }
}