TELEOP

package org.firstinspires.ftc.teamcode;

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



/**
 * Created by some1 on 12/18/2017.
 */

@TeleOp(name="Mechanum2.0", group="tests")

public class Mechanum2 extends LinearOpMode
{
    private ElapsedTime runtime = new ElapsedTime();
    private DcMotor leftFrontDrive = null;
    private DcMotor rightFrontDrive = null;
    private DcMotor leftBackDrive = null;
    private DcMotor rightBackDrive = null;
    private DcMotor elevator = null;
    private Servo rightServo = null;
    private Servo leftServo = null;
    private final double BACKDRIVEMUL = 0.86;
    private final double LFRONTDRIVEMUL = 0.9;

    @Override
    public void runOpMode()
    {

        telemetry.addData("Status", "Initialized");
        telemetry.update();

        leftFrontDrive  = hardwareMap.get(DcMotor.class, "left_front_drive");
        leftBackDrive  = hardwareMap.get(DcMotor.class, "left_back_drive");
        rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive");
        rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive");
        elevator = hardwareMap.get(DcMotor.class,"elevator");
        rightServo = hardwareMap.get(Servo.class, "right_servo");
        leftServo = hardwareMap.get(Servo.class, "left_servo");

        leftFrontDrive.setDirection(DcMotor.Direction.FORWARD);
        leftBackDrive.setDirection(DcMotor.Direction.FORWARD);
        rightFrontDrive.setDirection(DcMotor.Direction.REVERSE);
        rightBackDrive.setDirection(DcMotor.Direction.REVERSE);

        waitForStart();
        runtime.reset();

        while (opModeIsActive())
        {

            mechAndPickup();
            telemetry.addData("right front:\t", rightFrontDrive.getCurrentPosition());
            telemetry.addData("left front:\t", leftFrontDrive.getCurrentPosition());
            telemetry.update();


        }
    }

    public void mechAndPickup()
    {
        double powerLeft = gamepad1.left_stick_y;
        double powerRight = gamepad1.right_stick_y;
        double powerSlideLeft = gamepad1.left_trigger;
        double powerSlideRight = gamepad1.right_trigger;
        boolean elevatorPowUp = gamepad2.x;
        boolean elevatorPowDown = gamepad2.b;
        boolean open = true;

//elevator and servo
        if(gamepad2.a)
        {
            open = !open;
        }

        if(!open)
        {
            leftServo.setPosition(0.6);
            rightServo.setPosition(0.4);
        }
        else
        {
            leftServo.setPosition(1);
            rightServo.setPosition(0.07);
        }


        if(elevatorPowDown)
        {
            elevator.setPower(0.6);
        }
        else if(elevatorPowUp)
        {
            elevator.setPower(-0.5);
        }
        else
        {
            elevator.setPower(0);
        }


        if ((powerLeft > 0.2 || powerLeft < -0.2 || powerRight > 0.2 || powerRight < -0.2) && powerSlideLeft == 0 && powerSlideRight == 0)
        {
            leftFrontDrive.setPower(LFRONTDRIVEMUL * powerLeft);
            leftBackDrive.setPower(BACKDRIVEMUL * powerLeft);
            rightFrontDrive.setPower(powerRight);
            rightBackDrive.setPower(BACKDRIVEMUL * powerRight);
        }/*
        else if ((powerSlideLeft > 0.2 || powerSlideLeft < -0.2) && powerLeft == 0 && powerRight == 0)
        {
            leftFrontDrive.setPower(LFRONTDRIVEMUL * powerSlideLeft);
            leftBackDrive.setPower(-BACKDRIVEMUL * powerSlideLeft);
            rightFrontDrive.setPower(-powerSlideLeft);
            rightBackDrive.setPower(BACKDRIVEMUL * powerSlideLeft);
        }
        else if ((powerSlideRight > 0.2 || powerSlideRight < -0.2) && powerLeft == 0 && powerRight == 0)
        {
            leftFrontDrive.setPower(-powerSlideRight);
            leftBackDrive.setPower(powerSlideRight);
            rightFrontDrive.setPower(powerSlideRight);
            rightBackDrive.setPower(-powerSlideRight);
        }*/
        else
        {
            leftFrontDrive.setPower(0);
            leftBackDrive.setPower(0);
            rightFrontDrive.setPower(0);
            rightBackDrive.setPower(0);
        }
    }
}