TeleOp

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;

/**
 * Created by mgandhi on 12/18/16.
 */

@TeleOp(name = "TeleOpTest")
public class Tele_Op_Strafe extends LinearOpMode {

    private DcMotor motorFrontLeft;
    private DcMotor motorFrontRight;
    private DcMotor motorBackLeft;
    private DcMotor motorBackRight;
    private DcMotor motorConveyor;
    private DcMotor motorShooterLeft;
    private DcMotor motorShooterRight;
    private DcMotor motorLift;

    private Servo servoTopCap;
    private Servo servoBottomCap;

    private static final double drivePower = .8;
    private static final double shooterPower = .55;

    boolean flaps_open = false;
    boolean top_ball_holder = false;

    @Override
    public void runOpMode() throws InterruptedException {
        motorFrontLeft = hardwareMap.dcMotor.get("FLM");
        motorFrontRight = hardwareMap.dcMotor.get("FRM");
        motorBackLeft = hardwareMap.dcMotor.get("BLM");
        motorBackRight = hardwareMap.dcMotor.get("BRM");
        motorConveyor = hardwareMap.dcMotor.get("CM");
        motorShooterLeft = hardwareMap.dcMotor.get("SML");
        motorShooterRight = hardwareMap.dcMotor.get("SMR");
        motorLift = hardwareMap.dcMotor.get("YM");

        servoBottomCap = hardwareMap.servo.get("LF");
        servoTopCap = hardwareMap.servo.get("RFS");

        motorBackRight.setDirection(DcMotor.Direction.REVERSE);
        motorShooterLeft.setDirection(DcMotor.Direction.REVERSE);
        motorFrontRight.setDirection(DcMotor.Direction.REVERSE);

        servoTopCap.setPosition(1);
        servoBottomCap.setPosition(.55);

        waitForStart();

        while (opModeIsActive()) {


            motorBackLeft.setPower((-gamepad1.right_stick_y - gamepad1.right_stick_x - ((gamepad1.left_stick_x) / 1.5)) * drivePower);
            motorFrontLeft.setPower((-gamepad1.right_stick_y + gamepad1.right_stick_x - ((gamepad1.left_stick_x) / 1.5)) * drivePower);
            motorBackRight.setPower((-gamepad1.right_stick_y + gamepad1.right_stick_x + ((gamepad1.left_stick_x) / 1.5)) * drivePower);
            motorFrontRight.setPower((-gamepad1.right_stick_y - gamepad1.right_stick_x + ((gamepad1.left_stick_x) / 1.5)) * drivePower);
            motorLift.setPower(-gamepad2.right_stick_y * .95);

            if (gamepad1.left_trigger != 0) {
                motorShooterLeft.setPower(-shooterPower);
                motorShooterRight.setPower(-shooterPower);
            } else {
                motorShooterRight.setPower(0);
                motorShooterLeft.setPower(0);
            }

            if (gamepad1.right_trigger != 0) {
                motorConveyor.setPower(-.95);
            } else if (gamepad1.right_bumper) {
                motorConveyor.setPower(.95);
            } else {
                motorConveyor.setPower(0);
            }

            if (gamepad2.y) {
                // Yoga lift servo
                if (flaps_open) {
                    servoBottomCap.setPosition(0.55);
                } else {
                    servoBottomCap.setPosition(1);
                }

                flaps_open = !flaps_open;
            }

            if (gamepad2.x) {
                // Top servo
                if (top_ball_holder) {
                    servoTopCap.setPosition(1);
                } else {
                    servoTopCap.setPosition(0);
                }

                top_ball_holder = !top_ball_holder;
            }

                }

            idle();
}}