MecaWheels

package org.firstinspires.ftc.teamcode;


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

@TeleOp(name="Meca", group="Linear Opmode")
public class MecaWheels extends LinearOpMode{

    // Declare OpMode members
    HardwareBase robot = new HardwareBase();

    @Override
    public void runOpMode() {
        telemetry.addData("Status", "Initialized");
        telemetry.update();

        /* Initialize the hardware variables.
         * The init() method of the hardware class does all the work here
         */
        robot.init(hardwareMap);

        // Most robots need the motor on one side to be reversed to drive forward
        // Reverse the motor that runs backwards when connected directly to the battery

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

        // run until the end of the match (driver presses STOP)
        while (opModeIsActive()) {

           double y = gamepad1.left_stick_y;
           double x = gamepad1.left_stick_x;
           double z = 0;
            if(gamepad1.left_stick_x != 0)
                 z = (y/ x);



            // Regular forward, backward, and sideways motions
            if( z != 0)
            {
                robot.right_drive.setPower(scale((y-x)));
                robot.right_drive1.setPower(scale((y+x)));
                robot.left_drive.setPower(scale((y+x)));
                robot.left_drive1.setPower(scale((y-x)));
            }
            robot.right_drive.setPower(scale((y-x-z)));
            robot.right_drive1.setPower(scale((y+x-z)));
            robot.left_drive.setPower(scale((y+x+z)));
            robot.left_drive1.setPower(scale((y-x+z)));


            //More Complex motions and manuvers
            if(gamepad1.right_stick_y != 0 )
            { 
                
                if(gamepad1.right_stick_y < 0)
                {
                    robot.right_drive.setPower(scale((-y)));
                    robot.right_drive1.setPower(0);
                    robot.left_drive.setPower(scale((y)));
                    robot.left_drive1.setPower(0);
                }
                if(gamepad1.right_stick_y > 0)
                {
                    robot.right_drive.setPower(0);
                    robot.right_drive1.setPower(scale((-y)));
                    robot.left_drive.setPower(0);
                    robot.left_drive1.setPower(scale((y)));
                }
            }
            if(gamepad1.right_stick_x != 0 && gamepad1.right_stick_x == 0)
            { 
                robot.right_drive.setDirection(DcMotor.Direction.REVERSE);
                robot.left_drive.setDirection(DcMotor.Direction.REVERSE);
                robot.right_drive1.setDirection(DcMotor.Direction.FORWARD);
                robot.left_drive1.setDirection(DcMotor.Direction.FORWARD);
                
                robot.right_drive.setPower(scale((y-x-z)));
                robot.right_drive1.setPower(scale((y+x-z)));
                robot.left_drive.setPower(scale((y+x+z)));
                robot.left_drive1.setPower(scale((y-x+z)));
            }
            
            // Acqusition system controls
            double extendArm = gamepad2.left_stick_y;
            double rotate = gamepad2.right_stick_y;
            
            robot.acq1.setPower(extendArm);
            robot.acq2.setPower(rotate);
            
            telemetry.addData("Current Position of Rotation", + robot.acq2.getCurrentPosition());
            telemetry.addData("Current Position of Extension", + robot.acq1.getCurrentPosition() );
            telemetry.update();
        }
    }
    public double scale(double n)
    {
      return n;
    }
}