3005_StarterBot_Base_2022

package org.firstinspires.ftc.teamcode;

// This `import` statements are used to pull in the FTC code into our program
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;

// Assign this teleop program a specific name so that we can choose it on the driver station
@TeleOp(name="StarterBot Base", group="Linear Opmode")

public class StarterBot_Base extends LinearOpMode {

    // This is a timer. It's effectively a stopwatch that we can use to check
    // how long the robot has been running. This is useful for defining movements
    // such as "Move forward for 1 second"
    private ElapsedTime runtime = new ElapsedTime();

    // These variables represent the physical motors/servos on our robot. In order to adjust
    // things like motor speed or servo position we have to use these variables. These
    // are set to `null` by default because we need to ask the Control Hub to actually find
    // the hardware before we can use it
    private DcMotor leftFront  = null;
    private DcMotor rightFront = null;
    private DcMotor leftRear   = null;
    private DcMotor rightRear  = null;

    private DcMotorEx arm1   = null;
    private DcMotorEx arm2  = null;

    private CRServo intake1    = null;
    private CRServo intake2    = null;

    private int targetArmPosition = 0;

    private final int ARM_DOWN   = 0;
    private final int ARM_INTAKE = 0;
    private final int ARM_GROUND = 100;
    private final int ARM_LOW    = 225;
    private final int ARM_MID    = 375;
    private final int ARM_HIGH   = 600;
    private final int ARM_MAX    = 800;

    // This function contains the core of our program. When the teleop is selected and started,
    // it automatically calls this function. This is where our gameplay logic will go
    @Override
    public void runOpMode() {
        // This adds data to our driver station dashboard. This is purely informational, it does
        // not have any impact on the robot
        telemetry.addData("Status", "Initialized");
        telemetry.update();

        // This is where we find the physical motors and actually save them into our
        // variables that are declared above. On the driver station we assign a name
        // to each physical port on the control hub + expansion hub. Then we use
        // those names in our program to find each motor/servo
        leftFront  = hardwareMap.get(DcMotor.class, "frontLeft");
        rightFront = hardwareMap.get(DcMotor.class, "frontRight");
        leftRear   = hardwareMap.get(DcMotor.class, "rearLeft");
        rightRear  = hardwareMap.get(DcMotor.class, "rearRight");

        arm1       = hardwareMap.get(DcMotorEx.class, "armRight");
        arm2       = hardwareMap.get(DcMotorEx.class, "armLeft");

        //arm1.setDirection(DcMotor.Direction.REVERSE);


        //intake1  = hardwareMap.get(CRServo.class, "intake1");
        //intake2  = hardwareMap.get(CRServo.class, "intake2");

        // Here we set the direction of each of our motors. Because the motors are mirrored
        // on the right vs. the left of the robot, they're effectively backwards. This is
        // an easy way to account for that
        leftFront.setDirection(DcMotor.Direction.FORWARD);
        leftRear.setDirection(DcMotor.Direction.FORWARD);
        rightRear.setDirection(DcMotor.Direction.REVERSE);
        rightFront.setDirection(DcMotor.Direction.REVERSE);

        // Wait for the game to start (driver presses PLAY)
        waitForStart();

        // Reset the timer (stopwatch) because we only care about time since the game
        // actually starts
        runtime.reset();

        arm1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        arm2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        targetArmPosition = ARM_DOWN;

        // This `while` loop runs until the end of the game. All of the code inside the loop
        // gets run over and over in order to control our robot. We do things such as
        // reading the controller input and setting motor speeds inside this loop because we
        // need to do those things constantly while we control our robot
        while (opModeIsActive()) {
            stopArmIfTargetReached();

            // Reads the controller inputs and stores the values into variables representing
            // how the robot will move
            double drive  = -gamepad1.left_stick_y;  // forwards and backwards movement
            double strafe =  gamepad1.left_stick_x;  // side to side movement
            double turn   =  gamepad1.right_stick_x; // rotation

            // Mecanum is weird. Because of the physics of how mecanum wheels actually move
            // the robot we have to do some weird math in order to get our robot to move how
            // we want it to. How this math works isn't very important. If you want more
            // information, ask Orion
            leftFront.setPower(Range.clip(  drive + strafe + turn, -1.0, 1.0));
            rightFront.setPower(Range.clip( drive - strafe - turn, -1.0, 1.0));
            leftRear.setPower(Range.clip(   drive - strafe + turn, -1.0, 1.0));
            rightRear.setPower(Range.clip(  drive + strafe - turn, -1.0, 1.0));

            // reads the controller inputs and stores the values into variables representing
            // how we want to control intake servos
            boolean intakeServoIn  = gamepad1.right_bumper;
            boolean intakeServoOut = gamepad1.left_bumper;

            // Adds the data for how our robot is moving to the driver station dashboard so
            // we can verify that controller input is being used properly
            telemetry.addData("Drive", drive);
            telemetry.addData("Strafe", strafe);
            telemetry.addData("Turn", turn);

            // This variable stores the value that we're going to use to control our intake servo.
            // We default it to 0 because we want the intake servos to remain still if
            // we're not pushing any buttons
            double intakePower = 0.0;

            // If the "intake in" button is pressed, set the intake servos to full speed forward
            if (intakeServoIn == true)
            {
                intakePower = 1;
            }
            // Otherwise, if the "intake in" button is pressed, set the intake servos to
            //  full speed backward
            // Note: Because we use "else if", this code will never run if the previous
            // condition (if (intakeServoIn == true)) was true. This means that if we push
            // the "intake in" button, pressing the "intake out" button as well does nothing
            else if (intakeServoOut == true)
            {
                intakePower = -1;
            }

            if (gamepad1.y)
            {
                targetArmPosition = ARM_HIGH;
            }
            else if (gamepad1.b)
            {
                targetArmPosition = ARM_MID;
            }
            else if (gamepad1.a)
            {
                targetArmPosition = ARM_LOW;
            }
            else if (gamepad1.x)
            {
                targetArmPosition = ARM_GROUND;
            }
            else if (gamepad1.right_stick_button)
            {
                targetArmPosition = ARM_DOWN;
            }

            setArmPosition(targetArmPosition);

            // This is what actually sends all the telemetry data to the driver station
            telemetry.update();
        }
    }

    public void setArmPosition(int position)
    {
        arm1.setTargetPosition(position);
        arm2.setTargetPosition(position);

        arm1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        arm2.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        arm1.setVelocity(Range.clip(1500, -1500, 1500));
        arm2.setVelocity(Range.clip(1500, -1500, 1500));
    }

    public void stopArmIfTargetReached()
    {
        telemetry.addData("Arm target pos 1", arm1.getTargetPosition());
        telemetry.addData("Arm current pos 1", arm1.getCurrentPosition());
        telemetry.addData("Arm target pos 2", arm2.getTargetPosition());
        telemetry.addData("Arm current pos 2", arm2.getCurrentPosition());

        if (!arm1.isBusy() || !arm2.isBusy())
        {
            arm1.setVelocity(0);
            arm2.setVelocity(0);
        }
    }
}