Fixed BasicPushbot_v4

BasicPushbot_v4.java

package org.firstinspires.ftc.teamcode;

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

/**
 * Created by Administrator on 9/29/2017.
 */
@TeleOp (name = "BasicPushbot_v4", group = "IHaveNoIdeaWhat" )
public class BasicPushbot_v4 extends LinearOpMode {
    private DcMotor motorLeft;
    private DcMotor motorRight;
    private DcMotor motorLeftMiddle;
    private DcMotor motorRightMiddle;
    private Servo armServo1;
    private Servo armServo2;
    private Servo armColorServo;
    private Servo wenchServo;

    private static final double ARM_RETRACTED_POSITION = 0;
    private static final double ARM_EXTENDED_POSITION = 1;

    private static final double ARM_PICKUP_POSITION = .5;

    @Override
    public void runOpMode() throws InterruptedException {
        motorLeft = hardwareMap.dcMotor.get("motorLeft");
        motorRight = hardwareMap.dcMotor.get("motorRight");
        motorLeftMiddle = hardwareMap.dcMotor.get("motorLeftMiddle");
        motorRightMiddle = hardwareMap.dcMotor.get("motorRightMiddle");

        motorLeftMiddle.setDirection(DcMotor.Direction.REVERSE);
        motorLeft.setDirection(DcMotor.Direction.REVERSE);

        armServo1 = hardwareMap.servo.get("armServo1");

        armServo1.setPosition(ARM_RETRACTED_POSITION);

        armServo2 = hardwareMap.servo.get("armServo2");

        armServo2.setPosition(ARM_EXTENDED_POSITION);

        armColorServo = hardwareMap.servo.get("armColorServo");

        armColorServo.setPosition(ARM_EXTENDED_POSITION);

        wenchServo = hardwareMap.servo.get("wenchServo");
        wenchServo.setPosition(0.5);


        while (!isStarted() && !isStopRequested()) {
            // Use this while loop so you can stop before running
        }

        while (opModeIsActive() && !isStopRequested()) {
            // Simple arcade drive
            // left = power + turn
            // right = power - turn
            motorLeft.setPower(-gamepad1.left_stick_y - gamepad1.left_stick_x);
            motorRight.setPower(-gamepad1.left_stick_y + gamepad1.left_stick_x);
            motorLeftMiddle.setPower(-gamepad1.left_stick_y - gamepad1.left_stick_x);
            motorRightMiddle.setPower(-gamepad1.left_stick_y + gamepad1.left_stick_x);

            // Scale the gamepad right stick y to 1 to 0
            wenchServo.setPosition((-0.5 * gamepad1.right_stick_y) + 0.5);

            // These weren't in the while loop
            if (gamepad1.a) {
                armServo1.setPosition(ARM_PICKUP_POSITION);
                armServo2.setPosition(ARM_PICKUP_POSITION);
                // Changed to else if to handle both buttons being pressed
            } else if (gamepad1.b) {
                armServo1.setPosition(ARM_RETRACTED_POSITION);
                armServo2.setPosition(ARM_EXTENDED_POSITION);
            }

            if (gamepad1.y) {
                armColorServo.setPosition(ARM_PICKUP_POSITION);
            }
        }
    }
}